diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 2a617d3a..74d19e93 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,25 +1,25 @@ -FROM lcas.lincoln.ac.uk/lcas/ros:humble +FROM ros:humble -RUN apt-get update && export DEBIAN_FRONTEND=noninteractive \ - && apt-get -y upgrade \ - && apt-get -y install --no-install-recommends ros-humble-desktop ros-humble-simulation bash-completion python3-colcon-common-extensions \ - nano syslog-ng \ - build-essential \ - curl \ - swig \ +RUN rm -rf /var/lib/apt/lists/* \ + && apt-get update --fix-missing \ + && export DEBIAN_FRONTEND=noninteractive \ + && apt-get -y install --no-install-recommends --fix-missing \ + ros-humble-desktop \ + bash-completion \ + python3-colcon-common-extensions \ python3-pip \ - ros-humble-rviz2 \ - python3-rosinstall-generator \ - wget + python3-vcstool \ + python3-rosdep \ + nano \ + git \ + && rm -rf /var/lib/apt/lists/* RUN useradd -rm -d /home/lcas -s /bin/bash -g root -G sudo -u 1001 lcas RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers RUN echo "source /opt/ros/humble/setup.bash" >> /home/lcas/.bashrc -RUN pip3 install bloom - -COPY *repos *.sh /tmp/.devcontainer/ +COPY *.sh /tmp/.devcontainer/ RUN bash /tmp/.devcontainer/install.sh RUN mkdir -p /home/lcas/ws/src && ln -s /workspaces /home/lcas/ws/src/workspaces diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 422af812..6ae27733 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,61 +1,41 @@ -// For format details, see https://aka.ms/devcontainer.json. For config options, see the -// README at: https://github.com/devcontainers/templates/tree/main/src/ubuntu { "name": "topological_navigation", - // Or use a Dockerfile or Docker Compose file. More info: https://containers.dev/guide/dockerfile - // "image": "lcas.lincoln.ac.uk/lcas/ros:humble", "build": { "dockerfile": "Dockerfile", "context": "." }, - // Features to add to the dev container. More info: https://containers.dev/features. "features": { "ghcr.io/devcontainers/features/desktop-lite:1": {} }, - // Use 'forwardPorts' to make a list of ports inside the container available locally. - // "forwardPorts": [], - "forwardPorts": [6080, 5901], + "forwardPorts": [6080], "portsAttributes": { - "6080": { - "label": "novnc" - }, - "5901": { - "label": "vnc" - } + "6080": { "label": "novnc" } }, - // Use 'postCreateCommand' to run commands after the container is created. + "postCreateCommand": "bash .devcontainer/run.sh", "containerUser": "lcas", - // Configure tool-specific properties. - // "customizations": {}, - "runArgs": [ "--security-opt", "seccomp=unconfined", "--cap-add=NET_ADMIN" ], + "runArgs": ["--security-opt", "seccomp=unconfined"], "containerEnv": { - "ROS_LOCALHOST_ONLY": "0", - "LIBGL_ALWAYS_SOFTWARE": "1" // Needed for software rendering of opengl - }, + "ROS_LOCALHOST_ONLY": "0", + "LIBGL_ALWAYS_SOFTWARE": "1" + }, - // Uncomment to connect as root instead. More info: https://aka.ms/dev-containers-non-root. - "customizations": { + "customizations": { "vscode": { "extensions": [ - "dotjoshjohnson.xml", - "zachflower.uncrustify", "ms-python.python", "ms-vscode.cpptools", "redhat.vscode-yaml", - "smilerobotics.urdf", - "streetsidesoftware.code-spell-checker", "twxs.cmake", - "king2021.vnc-extension", "nonanonno.vscode-ros2", "deitry.colcon-helper", "yzhang.markdown-all-in-one", - "github.vscode-github-actions" + "github.vscode-github-actions", + "ArshadKhan.topological-map-visual" ] } } - } diff --git a/.devcontainer/install.sh b/.devcontainer/install.sh index d74d5f29..4177d81e 100644 --- a/.devcontainer/install.sh +++ b/.devcontainer/install.sh @@ -3,21 +3,9 @@ set -e source /opt/ros/humble/setup.bash -apt update -rosdep --rosdistro=humble update +apt-get update +rosdep init || true +rosdep --rosdistro=humble update - -rm -rf /opt/ros/lcas -mkdir -p /opt/ros/lcas/src -chown -R lcas /opt/ros/lcas -cd /opt/ros/lcas/src -vcs import < /tmp/.devcontainer/lcas.repos -rosdep install --from-paths . -i -y -cd /opt/ros/lcas -colcon build - - -#cd /home/lcas/ws -#colcon build -echo "source /opt/ros/lcas/install/setup.bash" >> ~/.bashrc +rm -rf /var/lib/apt/lists/* diff --git a/.devcontainer/lcas.repos b/.devcontainer/lcas.repos deleted file mode 100644 index 090e078a..00000000 --- a/.devcontainer/lcas.repos +++ /dev/null @@ -1,9 +0,0 @@ -repositories: - # teaching: - # type: git - # url: https://github.com/LCAS/teaching.git - # version: 2324-devel - # limo_ros2: - # type: git - # url: https://github.com/LCAS/limo_ros2.git - # version: humble diff --git a/.devcontainer/run.sh b/.devcontainer/run.sh index 9e54c517..94104a10 100644 --- a/.devcontainer/run.sh +++ b/.devcontainer/run.sh @@ -2,25 +2,22 @@ set -e -WORKSPACE="`pwd`" - -source /opt/ros/lcas/install/setup.bash -sudo apt update -rosdep --rosdistro=humble update - - -# sudo rm -rf /opt/ros/lcas -# sudo mkdir -p /opt/ros/lcas/src -# sudo chown -R lcas /opt/ros/lcas -# cd /opt/ros/lcas/src -# vcs import < $WORKSPACE/.devcontainer/lcas.repos -# rosdep install --from-paths . -i -y -# cd /opt/ros/lcas -# colcon build - +source /opt/ros/humble/setup.bash + +# Only run apt/rosdep update if not done before (avoid hanging on rebuild) +MARKER="/home/lcas/.postCreateDone" +if [ ! -f "$MARKER" ]; then + sudo apt-get update -qq || true + rosdep --rosdistro=humble update --include-eol-distros || true + cd /home/lcas/ws + rosdep install --from-paths ./src -i -y --rosdistro=humble || true + touch "$MARKER" +fi cd /home/lcas/ws -rosdep install --from-paths ./src -i -y colcon build --symlink-install -echo "source /home/lcas/ws/install/setup.bash" >> ~/.bashrc + +# Add workspace sourcing to bashrc (idempotent) +grep -q "install/setup.bash" ~/.bashrc 2>/dev/null || \ + echo "source /home/lcas/ws/install/setup.bash" >> ~/.bashrc diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md index 48ed6e5f..b22dd922 100644 --- a/.github/copilot-instructions.md +++ b/.github/copilot-instructions.md @@ -116,9 +116,8 @@ ros2 run topological_navigation validate_map.py path/to/map.tmap2.yaml -v ### Documentation - `README.md`: Package-level documentation -- `doc/PROPERTIES.md`: Comprehensive guide to the properties system - `CHANGELOG.rst`: Version history and changes - +- Additional technical documentation may be found in the `doc/` directory (if present). ### Testing - `test/`: Unit tests and integration tests - Uses `pytest` for Python tests diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 078a646f..434f75db 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -9,44 +9,59 @@ on: branches: [ humble-dev, aoc ] jobs: - test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. + # ------------------------------------------------------------------- + # Lightweight unit tests (no ROS runtime needed) + # ------------------------------------------------------------------- + unit_tests: + name: Unit tests (Python ${{ matrix.python-version }}) + runs-on: ubuntu-latest + strategy: + matrix: + python-version: ['3.10', '3.12'] + steps: + - uses: actions/checkout@v4 + + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v6 + with: + python-version: ${{ matrix.python-version }} + + - name: Install test dependencies + run: | + python -m pip install --upgrade pip + pip install pytest pyyaml jsonschema networkx scipy numpy + + - name: Run topological_navigation unit tests + working-directory: topological_navigation + run: | + python -m pytest test/ \ + --ignore=test/test_navigationcore.py \ + --ignore=test/test_localisation2.py \ + -v --tb=short + + - name: Run topological_nav_simulator unit tests + working-directory: topological_nav_simulator + run: | + python -m pytest test/ -v --tb=short || true + # These tests require geometry_msgs (ROS 2); they are + # fully exercised in the test_docker job below. + + # ------------------------------------------------------------------- + # Full ROS 2 build + colcon test + # ------------------------------------------------------------------- + test_docker: runs-on: ubuntu-latest strategy: matrix: ros_distribution: - # - noetic - humble - - iron - # Define the Docker image(s) associated with each ROS distribution. - # The include syntax allows additional variables to be defined, like - # docker_image in this case. See documentation: - # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build - # - # Platforms are defined in REP 3 and REP 2000: - # https://ros.org/reps/rep-0003.html - # https://ros.org/reps/rep-2000.html include: - # Noetic Ninjemys (May 2020 - May 2025) - # - docker_image: ubuntu:focal - # ros_distribution: noetic - # ros_version: 1 - # Humble Hawksbill (May 2022 - May 2027) - docker_image: ubuntu:jammy ros_distribution: humble ros_version: 2 - # Iron Irwini (May 2023 - November 2024) - - docker_image: ubuntu:jammy - ros_distribution: iron - ros_version: 2 - - # # Rolling Ridley (No End-Of-Life) - # - docker_image: ubuntu:jammy - # ros_distribution: rolling - # ros_version: 2 - container: image: ${{ matrix.docker_image }} steps: @@ -54,13 +69,8 @@ jobs: uses: LCAS/setup-ros@master with: required-ros-distributions: ${{ matrix.ros_distribution }} - - name: build and test ROS 1 - if: ${{ matrix.ros_version == 1 }} - uses: ros-tooling/action-ros-ci@v0.3 - with: - target-ros1-distro: ${{ matrix.ros_distribution }} - name: build and test ROS 2 if: ${{ matrix.ros_version == 2 }} uses: ros-tooling/action-ros-ci@v0.3 with: - target-ros2-distro: ${{ matrix.ros_distribution }} + target-ros2-distro: ${{ matrix.ros_distribution }} \ No newline at end of file diff --git a/.gitignore b/.gitignore index 70e59f66..17a76afa 100644 --- a/.gitignore +++ b/.gitignore @@ -68,3 +68,7 @@ coverage.xml **/*.*~ **/*.*.swp .DS_Store + +install/* +log/* +test.py diff --git a/AGENTS.md b/AGENTS.md index 3e001aca..73a8e9cc 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -44,6 +44,7 @@ topological_navigation/ │ │ ├── route_search2.py # A* path planning │ │ ├── topological_map.py # Map data structures │ │ ├── load_maps_from_yaml.py # YAML map loader +│ │ ├── networkx_utils.py # NetworkX graph and KD-tree utilities (NEW) │ │ └── restrictions_impl.py # Navigation restrictions │ ├── config/ # YAML schemas and templates │ │ └── schema2.json # JSON schema for map validation @@ -75,12 +76,18 @@ topological_navigation/ - **GUI**: Qt (for RViz panels) - **Data Format**: YAML for topological maps (`.tmap2.yaml` extension) - **Validation**: JSON Schema (`config/schema2.json`) +- **Graph Library**: NetworkX (>=2.5) for graph data structures and algorithms +- **Spatial Indexing**: scipy (>=1.5) for KD-tree nearest neighbor search +- **Numerical Operations**: numpy (>=1.19) for vectorized calculations - **Key Dependencies**: - `nav2_msgs` - Nav2 navigation actions and messages - `geometry_msgs`, `nav_msgs`, `sensor_msgs` - ROS 2 standard messages - `tf_transformations` - Coordinate frame transformations - `visualization_msgs` - RViz markers - `topological_navigation_msgs` - Custom message definitions + - `networkx` - Graph data structures and algorithms + - `scipy` - KD-tree spatial indexing + - `numpy` - Numerical operations ## Core Concepts @@ -177,12 +184,16 @@ See `topological_navigation/doc/PROPERTIES.md` for comprehensive documentation. - Validates map structure against JSON schema - Manages node and edge metadata -2. **Localisation** (`scripts/localisation2.py`) +2. **Localisation** (`scripts/localisation2.py`, `networkx_utils.py`) - Determines robot's current topological node - - Publishes `/current_node` and `/closest_node` + - Publishes `/current_node`, `/closest_node`, `/closest_node_distance`, `/closest_edges`, `/current_node/tag` - Supports pose-based and topic-based localization - - Uses influence zones to determine node proximity + - Uses influence zones with ray-casting point-in-polygon checks - Critical for navigation start conditions + - **Refactored with NetworkX and KD-tree** for O(log n) performance + - Uses `networkx_utils.py` module for graph operations and spatial indexing + - 35 unit tests in `test/test_localisation2.py` + - See `doc/LOCALISATION.md` for detailed technical documentation 3. **Navigation** (`scripts/navigation2.py`) - Executes topological navigation actions @@ -233,6 +244,119 @@ See `topological_navigation/doc/PROPERTIES.md` for comprehensive documentation. - `get_path_cost()` - Calculate path cost with property-based weighting - `is_node_blocked()` - Check if node is restricted/blocked +### 5. NetworkX Refactoring and Spatial Indexing + +The localisation system has been refactored to use NetworkX graph data structures and KD-tree spatial indexing for improved performance and maintainability. + +#### NetworkX Utils Module (`networkx_utils.py`) + +**Purpose**: Provides graph-based data structures and efficient spatial queries for topological localization + +**Key Features**: +- NetworkX DiGraph representation of topological maps +- KD-tree spatial indexing for O(log n) nearest neighbor search +- Point-in-polygon checks using ray-casting algorithm +- Vectorized edge distance calculations +- NetworkX algorithm wrappers for shortest paths and connectivity + +**Core Functions**: + +1. **Graph Construction**: + - `build_graph_from_tmap(tmap_data, logger)` - Convert YAML map to NetworkX DiGraph + - Stores node positions, influence zones, properties as graph attributes + - Stores edge metadata (edge_id, action, action_type, properties) + - Returns `nx.DiGraph` with full topological map structure + +2. **KD-Tree Spatial Indexing**: + - `build_kdtree_from_graph(graph, logger)` - Build KD-tree from node positions + - Returns tuple of `(kdtree, node_names)` for efficient spatial queries + - Construction: O(n log n), Query: O(log n) average case + - Enables fast closest node detection for large maps (1000+ nodes) + +3. **Spatial Queries**: + - `query_nearest_nodes(kdtree, node_names, pose, k)` - Find k-nearest nodes + - Returns list of `{'node': name, 'dist': distance}` sorted by distance + - Used for efficient localization without checking all nodes + +4. **Geometric Operations**: + - `point_in_poly_nx(graph, node_name, pose)` - Check if pose is within influence zone + - Ray-casting algorithm: O(m) where m is polygon vertices + - Transforms pose to node-relative coordinates before checking + - `get_edge_distances_nx(graph, pose, logger)` - Calculate distances to all edges + - Vectorized numpy operations for efficiency + - Returns sorted `(edge_ids, distances)` tuple + +5. **Localization Logic**: + - `determine_current_node(graph, kdtree, node_names, pose, loc_by_topic, nogos)` - Find current node + - Priority 1: Topic-based localization (highest priority) + - Priority 2: Geometric localization using KD-tree + influence zones + - Only checks 3 closest nodes (KD-tree optimization) + - `determine_closest_node(kdtree, node_names, graph, current_node, nogos, names_by_topic, pose)` - Find closest node + - Filters no-go and topic-based nodes + - Returns `(node_name, distance)` tuple + +6. **Topic-Based Localization**: + - `update_loc_by_topic_nx(graph, logger)` - Extract topic-based localization config + - Parses JSON configuration from node attributes + - Sets default values for `localise_anywhere` and `persistency` + - Returns `(nodes_by_topic, names_by_topic)` tuple + +7. **NetworkX Algorithm Wrappers**: + - `compute_shortest_path(graph, source, target, weight, logger)` - Dijkstra's algorithm + - `compute_path_length(graph, source, target, weight, logger)` - Path length only + - `check_connectivity(graph, source, target, logger)` - Path existence check + - `get_neighbors(graph, node, logger)` - Get adjacent nodes + +**Performance Characteristics**: +- KD-tree construction: O(n log n) where n is number of nodes +- KD-tree query: O(log n) average case for nearest neighbor +- Point-in-polygon: O(m) where m is number of polygon vertices +- Edge distance: O(e) where e is number of edges (vectorized) +- Full localization: O(log n + k*m) where k=3 closest nodes + +**Usage Pattern in localisation2.py**: +```python +# Build graph and KD-tree when map is received +self._graph = build_graph_from_tmap(self.tmap, logger=self.get_logger()) +self._kdtree, self._kdtree_node_names = build_kdtree_from_graph(self._graph, logger=self.get_logger()) + +# Update topic-based localization configuration +self.nodes_by_topic, self.names_by_topic = update_loc_by_topic_nx(self._graph, logger=self.get_logger()) + +# During localization (pose_callback) +currentstr = determine_current_node( + self._graph, self._kdtree, self._kdtree_node_names, + msg, self.loc_by_topic, self.nogos +) + +closeststr, closest_dist = determine_closest_node( + self._kdtree, self._kdtree_node_names, self._graph, + currentstr, self.nogos, self.names_by_topic, msg +) + +# Get edge distances +closest_edges, edge_dists = get_edge_distances_nx(self._graph, msg, logger=self.get_logger()) +``` + +**Key Benefits**: +- **Performance**: O(log n) spatial queries vs O(n) linear search +- **Scalability**: Handles 1000+ node maps efficiently +- **Maintainability**: Uses standard NetworkX algorithms instead of custom implementations +- **Testability**: Modular functions with clear interfaces +- **Type Safety**: Comprehensive type hints on all public functions +- **Documentation**: Detailed docstrings with examples and performance notes + +**Dependencies**: +- `networkx` (>=2.5) - Graph data structures and algorithms +- `scipy` (>=1.5) - KD-tree spatial indexing (`scipy.spatial.KDTree`) +- `numpy` (>=1.19) - Numerical operations and vectorization + +**Testing**: +- Unit tests in `test/test_networkx_utils.py` +- Localisation unit tests in `test/test_localisation2.py` (35 tests) +- Performance tests verify O(log n) query times +- Coverage: 80%+ for all networkx_utils functions + ## Development Guidelines ### Code Style @@ -545,6 +669,106 @@ class MyNode(Node): map_file = self.get_parameter('map_file').value ``` +### Pattern 5: NetworkX Graph and KD-Tree Usage + +Follow the pattern in `localisation2.py` for efficient spatial queries: + +```python +from topological_navigation.networkx_utils import ( + build_graph_from_tmap, + build_kdtree_from_graph, + query_nearest_nodes, + point_in_poly_nx, + get_edge_distances_nx, + determine_current_node, + determine_closest_node +) + +class MyLocalisationNode(Node): + def __init__(self): + super().__init__('my_localisation') + + # Initialize data structures + self._graph = None + self._kdtree = None + self._kdtree_node_names = [] + + def map_callback(self, msg): + # Build NetworkX graph from topological map + self._graph = build_graph_from_tmap( + self.tmap, + logger=self.get_logger() + ) + + if self._graph is None: + self.get_logger().error("Failed to build graph") + return + + # Build KD-tree for O(log n) spatial queries + self._kdtree, self._kdtree_node_names = build_kdtree_from_graph( + self._graph, + logger=self.get_logger() + ) + + if self._kdtree is None: + self.get_logger().error("Failed to build KD-tree") + return + + def localize(self, pose): + # Determine current node (within influence zone) + current_node = determine_current_node( + self._graph, + self._kdtree, + self._kdtree_node_names, + pose, + self.loc_by_topic, # Topic-based localization list + self.nogos # No-go nodes list + ) + + # Determine closest node by distance + closest_node, distance = determine_closest_node( + self._kdtree, + self._kdtree_node_names, + self._graph, + current_node, + self.nogos, + self.names_by_topic, + pose + ) + + # Get distances to edges + edge_ids, distances = get_edge_distances_nx( + self._graph, + pose, + logger=self.get_logger() + ) + + return current_node, closest_node, distance + + def check_influence_zone(self, node_name, pose): + # Check if pose is within node's influence zone + is_inside = point_in_poly_nx(self._graph, node_name, pose) + return is_inside + + def find_nearest_nodes(self, pose, k=3): + # Find k nearest nodes efficiently + nearest = query_nearest_nodes( + self._kdtree, + self._kdtree_node_names, + pose, + k=k + ) + # Returns: [{'node': 'WP1', 'dist': 2.5}, ...] + return nearest +``` + +**Key Points**: +- Build graph and KD-tree once when map is received +- Reuse KD-tree for multiple queries (O(log n) performance) +- Always check for None before using graph/KD-tree +- Use logger parameter for consistent error reporting +- KD-tree queries return sorted results by distance + ## Common Pitfalls ### Pitfall 1: Property Access Without Defaults @@ -600,14 +824,56 @@ self._nav_client = ActionClient( ) ``` +### Pitfall 5: Not Rebuilding KD-Tree After Map Updates +**Wrong**: +```python +def map_callback(self, msg): + self._graph = build_graph_from_tmap(self.tmap) + # Forgot to rebuild KD-tree! Old KD-tree has stale data +``` + +**Right**: +```python +def map_callback(self, msg): + # Always rebuild both graph AND KD-tree together + self._graph = build_graph_from_tmap(self.tmap, logger=self.get_logger()) + self._kdtree, self._kdtree_node_names = build_kdtree_from_graph( + self._graph, + logger=self.get_logger() + ) +``` + +### Pitfall 6: Using Linear Search Instead of KD-Tree +**Wrong**: +```python +# O(n) linear search through all nodes +min_dist = float('inf') +closest_node = None +for node_name in all_nodes: + dist = calculate_distance(pose, node_name) + if dist < min_dist: + min_dist = dist + closest_node = node_name +``` + +**Right**: +```python +# O(log n) KD-tree query +nearest = query_nearest_nodes(self._kdtree, self._kdtree_node_names, pose, k=1) +if nearest: + closest_node = nearest[0]['node'] + min_dist = nearest[0]['dist'] +``` + ## Key Files Reference ### Core Navigation Files - `scripts/navigation2.py` - Main navigation action server (topological navigation entry point) - `edge_action_manager2.py` - Edge action execution (complex, 1363 lines) - `route_search2.py` - A* path planning algorithm -- `scripts/localisation2.py` - Topological localisation node +- `scripts/localisation2.py` - Topological localisation node (refactored with NetworkX) - `scripts/map_manager.py` - Map loading and publishing node +- `networkx_utils.py` - NetworkX graph and KD-tree utilities (NEW) ### Map Data Structures - `topological_map.py` - In-memory map representation @@ -615,6 +881,7 @@ self._nav_client = ActionClient( - `manager2.py` - Map management core logic ### Utility Classes +- `networkx_utils.py` - NetworkX graph operations and spatial indexing (NEW) - `dict_tools` (in edge_action_manager2.py) - Nested dictionary operations - `tmap_utils.py` - Map manipulation utilities - `route_search.py` - Legacy route search (v1) @@ -623,7 +890,15 @@ self._nav_client = ActionClient( - `config/schema2.json` - JSON schema for map validation - `doc/PROPERTIES.md` - Properties system documentation +### Documentation +- `doc/LOCALISATION.md` - Topological localisation technical documentation +- `doc/MANAGER2_DOCUMENTATION.md` - Map manager technical documentation +- `doc/PROPERTIES.md` - Properties system guide +- `doc/FLUID_NAVIGATION.md` - Fluid navigation guide + ### Testing +- `test/test_localisation2.py` - Localisation unit tests (35 tests) +- `test/test_networkx_utils.py` - NetworkX utilities unit tests - `test/test_navigationcore.py` - Navigation core tests - `tests/topological_navigation_tester_critical.py` - Critical integration tests - `tests/map_manager_tester.py` - Map manager tests @@ -798,6 +1073,17 @@ colcon test --packages-select topological_navigation 4. **Follow existing patterns**: Don't introduce new patterns without discussion 5. **Consider agricultural use cases**: Changes impact real-world robot operations +### When Creating Documentation +1. **Location**: Place all new documentation files in the `doc/` folder +2. **Track documentation**: Maintain a list of documentation files when creating new ones +3. **Keep synchronized**: When updating script or function functionalities: + - Update corresponding documentation in `doc/` folder + - Update references in AGENTS.md, README.md, and other relevant docs + - Ensure examples and code snippets reflect current implementation +4. **Naming convention**: Use descriptive names (e.g., `EDGE_ACTIONS.md`, `ROUTING_ALGORITHM.md`) +5. **Cross-reference**: Link related documentation files for easy navigation +6. **Version updates**: Update the "Last Updated" date at the bottom of modified documentation + ### When Debugging 1. **Start with logs**: Check ROS 2 logs for errors/warnings 2. **Verify map structure**: Ensure YAML maps are valid @@ -807,6 +1093,6 @@ colcon test --packages-select topological_navigation --- -**Last Updated**: 2026-01-27 +**Last Updated**: 2026-02-15 **Branch**: agent_prep **Maintainer**: AI Coding Agents diff --git a/README.md b/README.md index 8333bb1b..82e856cc 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,701 @@ -# topological_navigation +# Topological Navigation -A topological navigation planning framework for ROS 2. +A ROS 2 framework for **graph-based topological navigation** of autonomous mobile robots. Unlike traditional metric navigation that operates in continuous coordinate space, topological navigation represents the environment as a graph of discrete **nodes** (waypoints) connected by **edges** (traversable paths), enabling high-level route planning, named-location navigation, and pluggable edge actions (e.g., Nav2 goals, row traversal, door opening). + +Originally developed for the [STRANDS](https://strands-project.eu/) long-term autonomy project, this framework is actively used in agricultural robotics for autonomous navigation in vineyards, orchards, and crop rows. + +> **ROS 2 Only** — version 4.0.0+ is ROS 2 exclusive (Humble / Iron). For ROS 1 support use version 3.x or earlier. + +--- + +## Table of Contents + +- [Topological Navigation](#topological-navigation) + - [Table of Contents](#table-of-contents) + - [Architecture Overview](#architecture-overview) + - [Packages](#packages) + - [topological\_navigation (Core)](#topological_navigation-core) + - [topological\_navigation\_msgs](#topological_navigation_msgs) + - [topological\_navigation\_visual](#topological_navigation_visual) + - [topological\_nav\_simulator](#topological_nav_simulator) + - [Installation](#installation) + - [Prerequisites](#prerequisites) + - [Build from Source](#build-from-source) + - [Python Dependencies](#python-dependencies) + - [Launching the System](#launching-the-system) + - [Full Stack (with Simulator)](#full-stack-with-simulator) + - [Standalone Simulator](#standalone-simulator) + - [Visualiser Only](#visualiser-only) + - [On a Real Robot](#on-a-real-robot) + - [Sending Navigation Goals](#sending-navigation-goals) + - [ROS 2 Node Parameters](#ros-2-node-parameters) + - [topological\_navigation (navigation2.py)](#topological_navigation-navigation2py) + - [topological\_localisation (localisation2.py)](#topological_localisation-localisation2py) + - [topological\_map\_manager\_2 (map\_manager2.py)](#topological_map_manager_2-map_manager2py) + - [manual\_tmapping\_node (manual\_topomapping.py)](#manual_tmapping_node-manual_topomappingpy) + - [topological\_map\_visualiser (topological\_map\_visualiser.py)](#topological_map_visualiser-topological_map_visualiserpy) + - [route\_visualiser / occupancy\_visualiser (topological\_visual.py)](#route_visualiser--occupancy_visualiser-topological_visualpy) + - [fake\_nav2\_server (fake\_nav2\_server.py — simulator)](#fake_nav2_server-fake_nav2_serverpy--simulator) + - [ROS 2 Topics](#ros-2-topics) + - [Published Topics](#published-topics) + - [Subscribed Topics](#subscribed-topics) + - [TF Broadcasts](#tf-broadcasts) + - [ROS 2 Services](#ros-2-services) + - [ROS 2 Actions](#ros-2-actions) + - [Action Servers](#action-servers) + - [Action Clients](#action-clients) + - [TF Frames](#tf-frames) + - [Custom Message, Service \& Action Definitions](#custom-message-service--action-definitions) + - [Messages (.msg)](#messages-msg) + - [Services (.srv)](#services-srv) + - [Actions (.action)](#actions-action) + - [Topological Map Format](#topological-map-format) + - [Flexible Properties System](#flexible-properties-system) + - [Map Validation](#map-validation) + - [Command-Line Utilities](#command-line-utilities) + - [Testing](#testing) + - [Contributing](#contributing) + - [License](#license) + +--- + +## Architecture Overview + +``` +┌──────────────────────────────────────────────────────────────────────┐ +│ Topological Navigation Stack │ +│ │ +│ ┌────────────────┐ ┌──────────────────┐ ┌───────────────────┐ │ +│ │ Map Manager │──▶│ Localisation │──▶│ Navigation │ │ +│ │ (map_manager2)│ │ (localisation2) │ │ (navigation2) │ │ +│ │ │ │ │ │ │ │ +│ │ Loads YAML map │ │ Determines │ │ A* route planning │ │ +│ │ Publishes to │ │ current/closest │ │ Edge action exec │ │ +│ │ /topological_ │ │ node via KD-tree │ │ GotoNode action │ │ +│ │ map_2 topic │ │ + influence zones │ │ server │ │ +│ └────────────────┘ └──────────────────┘ └────────┬──────────┘ │ +│ │ │ +│ ▼ │ +│ ┌───────────────────┐ │ +│ │ Nav2 Stack │ │ +│ │ (or Simulator) │ │ +│ │ NavigateToPose │ │ +│ │ NavigateThrough │ │ +│ │ FollowWaypoints │ │ +│ └───────────────────┘ │ +│ │ +│ ┌────────────────────┐ ┌──────────────────────────────────────┐ │ +│ │ Map Visualiser │ │ Fake Nav2 Simulator │ │ +│ │ (visual package) │ │ (simulator package) │ │ +│ │ RViz markers + │ │ Virtual robot + fake action servers │ │ +│ │ interactive edit │ │ for testing without real hardware │ │ +│ └────────────────────┘ └──────────────────────────────────────┘ │ +└──────────────────────────────────────────────────────────────────────┘ +``` + +**Data Flow:** +1. **Map Manager** loads a `.tmap2.yaml` file and publishes it as a JSON string on `/topological_map_2` +2. **Localisation** subscribes to the map, builds a NetworkX graph with KD-tree spatial indexing, and continuously publishes which node the robot is at (`/current_node`, `/closest_node`) +3. **Navigation** receives `GotoNode` action goals, computes A\* routes, and executes edge actions by calling Nav2 action servers +4. **Visualiser** renders the topological map as interactive RViz markers for viewing and editing + +--- ## Packages -This repository contains the following ROS 2 packages: +### topological_navigation (Core) + +| | | +|---|---| +| **Type** | `ament_python` | +| **Version** | 5.0.0 | +| **Purpose** | Core navigation stack: map management, localisation, route planning, navigation execution | + +**Key modules:** + +| Module | Description | +|--------|-------------| +| `scripts/navigation2.py` | Main navigation action server — receives `GotoNode` goals, plans routes (A\*), and executes edge actions via Nav2 | +| `scripts/localisation2.py` | Topological localisation — determines current/closest node using KD-tree + influence zone checks | +| `scripts/map_manager2.py` | Loads `.tmap2.yaml` maps, validates them against JSON schema, publishes to ROS topics | +| `scripts/manual_topomapping.py` | Joystick-driven waypoint recording for field mapping | +| `networkx_utils.py` | NetworkX graph construction, KD-tree spatial indexing, point-in-polygon, edge distance calculations | +| `navigation_graph.py` | Navigation state machine, route planning, segment merging | +| `tmap_utils.py` | YAML map loading utilities | +| `validate_map.py` | CLI tool for topological map validation against schema | +| `convert_tmap.py` | Convert between topological map format versions | + +### topological_navigation_msgs + +| | | +|---|---| +| **Type** | `ament_cmake` | +| **Version** | 3.0.5 | +| **Purpose** | ROS 2 message, service, and action interface definitions | + +Defines 16 message types, 9 service types, and 3 action types used across the navigation stack. See [Custom Message, Service & Action Definitions](#custom-message-service--action-definitions) below for full details. + +### topological_navigation_visual + +| | | +|---|---| +| **Type** | `ament_python` | +| **Version** | 1.0.0 | +| **Purpose** | RViz visualisation and interactive map editing | + +**Key executables:** + +| Executable | Description | +|-----------|-------------| +| `topological_map_visualiser.py` | Unified visualiser + interactive drag-and-drop editor for topological maps in RViz | +| `topological_visual.py` | Route and occupied-node visualisation | +| `policy_marker.py` | Policy arrow visualisation for MDP planning | + +### topological_nav_simulator + +| | | +|---|---| +| **Type** | `ament_python` | +| **Version** | 1.0.0 | +| **Purpose** | Fake Nav2 action servers with a virtual robot for testing without real hardware | + +Provides `NavigateToPose`, `NavigateThroughPoses`, and `FollowWaypoints` action servers that simulate robot movement, publishing TF transforms, odometry, and RViz markers. The simulated robot smoothly interpolates toward goal poses at a configurable speed. + +--- + +## Installation + +### Prerequisites + +- **ROS 2** Humble or Iron +- **Python 3.8+** +- **Nav2** (for real robot deployment; the simulator replaces it for testing) + +### Build from Source + +```bash +# Create workspace +mkdir -p ~/ws/src && cd ~/ws/src +git clone https://github.com/LCAS/topological_navigation.git + +# Install dependencies +cd ~/ws +rosdep install --from-paths src --ignore-src -r -y + +# Build +colcon build --symlink-install + +# Source +source install/setup.bash +``` + +### Python Dependencies + +| Package | Min Version | Purpose | +|---------|------------|---------| +| `networkx` | 2.5 | Graph data structures and algorithms | +| `scipy` | 1.5 | KD-tree spatial indexing | +| `numpy` | 1.19 | Numerical operations | +| `jsonschema` | — | Map schema validation | + +--- + +## Launching the System + +### Full Stack (with Simulator) + +Launch the complete topological navigation system with a virtual robot (no real hardware needed): + +```bash +ros2 launch topological_navigation topological_navigation.launch.py +``` + +This starts (in order with delays for dependency readiness): + +| Delay | Node | Description | +|-------|------|-------------| +| 0 s | `topological_map_manager_2` | Loads and publishes the topological map | +| 2 s | `topological_localisation` | Starts localisation | +| 2 s | `fake_nav2_server` | Virtual robot + fake Nav2 action servers | +| 3 s | `topological_navigation` | Navigation action server | +| 4 s | `topological_map_visualiser` | Interactive RViz map editor | +| 5 s | `rviz2` | RViz with pre-configured display layout | + +**Launch arguments:** + +| Argument | Default | Description | +|----------|---------|-------------| +| `map_path` | `/config/mixed_actions_map.yaml` | Absolute path to a `.tmap2.yaml` topological map | +| `rviz_config` | `/rviz/topological_navigation.rviz` | Path to the RViz config file | +| `initial_x` | `0.0` | Initial robot X position (metres) | +| `initial_y` | `0.0` | Initial robot Y position (metres) | +| `initial_yaw` | `0.0` | Initial robot yaw (radians) | + +**Example with a custom map:** + +```bash +ros2 launch topological_navigation topological_navigation.launch.py \ + map_path:=/path/to/my_map.tmap2.yaml \ + initial_x:=5.0 initial_y:=3.0 +``` + +### Standalone Simulator + +Launch only the fake Nav2 server (useful when developing against a simulated robot): + +```bash +ros2 launch topological_nav_simulator fake_nav2.launch.py +``` + +**Launch arguments:** + +| Argument | Default | Description | +|----------|---------|-------------| +| `robot_speed` | `0.8` | Simulated linear speed (m/s) | +| `angular_speed` | `1.5` | Simulated angular speed (rad/s) | +| `goal_tolerance` | `0.15` | Goal position tolerance (m) | +| `update_rate` | `30.0` | TF/marker publish rate (Hz) | +| `initial_x` | `0.0` | Initial robot X position | +| `initial_y` | `0.0` | Initial robot Y position | +| `initial_yaw` | `0.0` | Initial robot yaw (rad) | +| `map_frame` | `map` | Map TF frame | +| `odom_frame` | `odom` | Odometry TF frame | +| `base_frame` | `base_link` | Robot base TF frame | + +### Visualiser Only + +Launch the topological map visualiser standalone: + +```bash +ros2 launch topological_navigation_visual topological_map_visualiser.launch.py \ + map_file:=/path/to/map.tmap2.yaml edit_mode:=true +``` + +**Launch arguments:** + +| Argument | Default | Description | +|----------|---------|-------------| +| `map_file` | `''` | Path to `.tmap2.yaml` file (empty = subscribe to topic) | +| `auto_save` | `false` | Auto-save changes every 30 seconds | +| `marker_scale` | `0.5` | Scale factor for RViz markers | +| `edit_mode` | `true` | Enable interactive drag-and-drop editing | +| `nav_action_name` | `/topological_navigation` | GotoNode action name for click-to-navigate | + +### On a Real Robot + +When using a real robot with Nav2, launch the navigation stack without the simulator: + +```bash +# 1. Start Nav2 (robot-specific) +ros2 launch navigation.launch.py + +# 2. Start the map manager +ros2 run topological_navigation map_manager2.py /path/to/map.tmap2.yaml + +# 3. Start localisation +ros2 run topological_navigation localisation2.py + +# 4. Start topological navigation +ros2 run topological_navigation navigation2.py + +# 5. (Optional) Start visualiser +ros2 run topological_navigation_visual topological_map_visualiser.py +``` + +### Sending Navigation Goals + +Once the system is running, send a robot to a named node: + +```bash +# Navigate to a node called "node3" +ros2 action send_goal /topological_navigation \ + topological_navigation_msgs/action/GotoNode \ + "{target: 'node3', no_orientation: false}" +``` + +--- + +## ROS 2 Node Parameters + +### topological_navigation (navigation2.py) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `max_dist_to_closest_edge` | double | `1.0` | Maximum distance (m) to consider an edge as valid origin for navigation | +| `default_boundary_left` | double | `0.5` | Default left boundary offset for row corridor polygons | +| `default_boundary_right` | double | `0.5` | Default right boundary offset for row corridor polygons | +| `route_algorithm` | string | `"astar"` | Path planning algorithm: `"astar"` or `"dijkstra"` | +| `route_weight_attr` | string | `"weight"` | Edge attribute used as cost for path planning | + +### topological_localisation (localisation2.py) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `LocalisationThrottle` | int | `1` | Process every Nth pose callback (throttle rate) | +| `OnlyLatched` | bool | `true` | Only publish when values actually change | +| `base_frame` | string | `"base_link"` | Robot base TF frame for localisation lookups | + +### topological_map_manager_2 (map_manager2.py) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `cache_topological_maps` | bool | `false` | Cache maps locally to `~/.ros/topological_maps` | +| `auto_write_topological_maps` | bool | `false` | Automatically save map on any modification | +| `topological_map2_name` | string | `""` | Current map name identifier | +| `topological_map2_path` | string | `""` | Filesystem path where maps are stored | + +### manual_tmapping_node (manual_topomapping.py) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `tmap` | string | `""` | Map YAML filename | +| `tmap_dir` | string | `""` | Directory to save map files | +| `site_name` | string | `""` | Site or farm name | +| `node_thresh` | double | `0.5` | Minimum distance (m) between nodes when recording | +| `lock_btn` | int | `6` | Joystick lock button index | +| `add_btn` | int | `1` | Joystick add-node button index | +| `remove_btn` | int | `2` | Joystick remove-node button index | +| `gen_map_btn` | int | `3` | Joystick generate-map button index | +| `topic_joy` | string | `"/joy"` | Joystick input topic | +| `topic_pose` | string | `"/gps_base/odometry"` | Robot pose input topic | +| `topic_imu` | string | `"/gps_base/yaw"` | IMU yaw input topic | + +### topological_map_visualiser (topological_map_visualiser.py) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `map_file` | string | `""` | Path to `.tmap2.yaml` file for editing (empty = subscribe to topic) | +| `auto_save` | bool | `false` | Automatically save map every 30 seconds | +| `marker_scale` | double | `0.5` | Scale factor for RViz node/edge markers | +| `edit_mode` | bool | `true` | Enable interactive drag-and-drop node editing | +| `nav_action_name` | string | `"/topological_navigation"` | GotoNode action name for click-to-navigate context menu | + +### route_visualiser / occupancy_visualiser (topological_visual.py) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `tmap` | string | *(required)* | Path to a `.tmap2.yaml` topological map file | + +### fake_nav2_server (fake_nav2_server.py — simulator) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `robot_speed` | double | `0.8` | Simulated linear speed (m/s) | +| `angular_speed` | double | `1.5` | Simulated angular speed (rad/s) | +| `goal_tolerance` | double | `0.15` | Position tolerance for goal reached (m) | +| `update_rate` | double | `30.0` | TF and marker publish rate (Hz) | +| `initial_x` | double | `0.0` | Initial robot X position | +| `initial_y` | double | `0.0` | Initial robot Y position | +| `initial_yaw` | double | `0.0` | Initial robot yaw (radians) | +| `map_frame` | string | `"map"` | Map TF frame | +| `odom_frame` | string | `"odom"` | Odometry TF frame | +| `base_frame` | string | `"base_link"` | Robot base TF frame | + +--- + +## ROS 2 Topics + +### Published Topics + +| Topic | Type | Publisher Node | Description | +|-------|------|---------------|-------------| +| `/topological_map_2` | `std_msgs/String` | Map Manager | Full topological map as a JSON string (latched) | +| `/topological_map_schema` | `std_msgs/String` | Map Manager | JSON schema for map validation (latched) | +| `/current_node` | `std_msgs/String` | Localisation | Name of the node the robot is currently inside (influence zone). `"none"` if outside all zones | +| `/closest_node` | `std_msgs/String` | Localisation | Name of the nearest node by Euclidean distance | +| `/closest_node_distance` | `std_msgs/Float32` | Localisation | Distance (m) to the closest node | +| `/closest_edges` | `topological_navigation_msgs/ClosestEdges` | Localisation | Sorted list of closest edge IDs with distances | +| `/current_node/tag` | `std_msgs/String` | Localisation | Semantic tag of the current node (from properties) | +| `/current_edge` | `std_msgs/String` | Navigation | Edge currently being traversed | +| `topological_navigation/Statistics` | `topological_navigation_msgs/NavStatistics` | Navigation | Navigation statistics for completed edges | +| `topological_navigation/Route` | `topological_navigation_msgs/TopologicalRoute` | Navigation | Currently planned route (list of node names) | +| `topological_navigation/move_action_status` | `std_msgs/String` | Navigation | Status of the current Nav2 move action | +| `/boundary_checker` | `geometry_msgs/PolygonStamped` | Navigation | Row corridor boundary polygon for agricultural operations | +| `/robot_operation_current_status` | `std_msgs/String` | Navigation | Current robot operation status | +| `/virtual_robot/markers` | `visualization_msgs/MarkerArray` | Fake Nav2 | Virtual robot body visualisation in RViz | +| `/odometry/global` | `nav_msgs/Odometry` | Fake Nav2 | Simulated global odometry | +| `/virtual_robot/pose` | `geometry_msgs/PoseStamped` | Fake Nav2 | Simulated robot pose | +| `topological_map_visualisation` | `visualization_msgs/MarkerArray` | Map Visualiser | Node and edge markers for RViz | +| `topological_route_visualisation` | `visualization_msgs/MarkerArray` | Route Visualiser | Active route highlight markers | +| `/topological_navigation/visual/occupied_node` | `visualization_msgs/MarkerArray` | Occupancy Visualiser | Occupied node indicators | +| `/tmapping_nodes` | `visualization_msgs/MarkerArray` | Manual Mapping | Recorded waypoint markers | + +### Subscribed Topics + +| Topic | Type | Subscriber Node | Description | +|-------|------|----------------|-------------| +| `/topological_map_2` | `std_msgs/String` | Localisation, Navigation, Visualiser | Topological map data | +| `/current_node` | `std_msgs/String` | Navigation, Visualiser | Current node for navigation state | +| `/closest_node` | `std_msgs/String` | Navigation, Visualiser | Closest node for route origin | +| `/closest_edges` | `topological_navigation_msgs/ClosestEdges` | Navigation | Edge proximity for navigation decisions | +| `/initialpose` | `geometry_msgs/PoseStamped` | Fake Nav2 | Set the virtual robot's pose (e.g., from RViz **2D Pose Estimate**) | +| `topological_navigation/Route` | `topological_navigation_msgs/TopologicalRoute` | Route Visualiser | Route to highlight | +| `/topological_navigation/occupied_node` | `topological_navigation_msgs/TopologicalOccupiedNode` | Occupancy Visualiser | Nodes occupied by other robots | +| `/joy` | `sensor_msgs/Joy` | Manual Mapping | Joystick input for waypoint recording | +| `mdp_plan_exec/current_policy_mode` | `topological_navigation_msgs/NavRoute` | Policy Marker | Policy route for arrow rendering | + +### TF Broadcasts + +| From | To | Publisher | Type | +|------|----|----------|------| +| `map` | `odom` | Fake Nav2 | Dynamic TF | +| `odom` | `base_link` | Fake Nav2 | Dynamic TF | +| `parent` | `topo_frame_id` | Map Manager | Static TF | + +--- + +## ROS 2 Services + +| Service | Type | Node | Description | +|---------|------|------|-------------| +| `/topological_map_manager2/get_topological_map` | `std_srvs/Trigger` | Map Manager | Returns the current map as a JSON string | +| `/topological_map_manager2/write_topological_map` | `WriteTopologicalMap` | Map Manager | Saves the current map to a YAML file | +| `/topological_map_manager2/switch_topological_map` | `WriteTopologicalMap` | Map Manager | Switches to a different map file | +| `/topological_localisation/localise_pose` | `LocalisePose` | Localisation | Localise an arbitrary pose (returns current + closest node) | +| `/tmapping_robot/save_waypoints` | `std_srvs/Trigger` | Manual Mapping | Save raw waypoints to file | +| `/tmapping_robot/save_map` | `std_srvs/Trigger` | Manual Mapping | Generate and save topological map from waypoints | +| `/save_map` | `std_srvs/Trigger` | Map Visualiser | Save the edited map to YAML | + +--- + +## ROS 2 Actions + +### Action Servers + +| Action | Type | Node | Description | +|--------|------|------|-------------| +| `/topological_navigation` | `GotoNode` | Navigation | Navigate the robot to a target topological node | +| `/topological_navigation/execute_policy_mode` | `ExecutePolicyMode` | Navigation | Execute a pre-computed policy route | +| `/navigate_to_pose` | `nav2_msgs/NavigateToPose` | Fake Nav2 | Simulated single-goal metric navigation | +| `/navigate_through_poses` | `nav2_msgs/NavigateThroughPoses` | Fake Nav2 | Simulated multi-waypoint metric navigation | +| `/follow_waypoints` | `nav2_msgs/FollowWaypoints` | Fake Nav2 | Simulated waypoint-following navigation | + +### Action Clients + +| Action | Type | Node | Description | +|--------|------|------|-------------| +| *(dynamic from map actions section)* | *(e.g., `nav2_msgs/NavigateToPose`)* | Navigation | Created for each unique `(action_type, action_server)` pair defined in the map | +| `/topological_navigation` | `GotoNode` | Map Visualiser | Click-to-navigate from RViz context menu | + +--- + +## TF Frames + +``` +map (global frame) + ├── topo_frame_id (static TF, published by map_manager2) + │ └── Topological node poses are defined in this frame + │ + └── odom (dynamic TF, published by simulator or real robot) + └── base_link (robot body frame) +``` + +The localisation node resolves the robot pose by looking up `topo_frame_id → base_frame` via `tf2_ros`. + +--- + +## Custom Message, Service & Action Definitions + +All definitions reside in the `topological_navigation_msgs` package. + +### Messages (.msg) + +| Message | Fields | Description | +|---------|--------|-------------| +| **NavRoute** | `string[] source`, `string[] edge_id` | Ordered route: source nodes and edge IDs to traverse | +| **TopologicalRoute** | `string[] nodes` | Ordered list of node names forming a route | +| **ClosestEdges** | `string[] edge_ids`, `float32[] distances` | Sorted edges by proximity with distances | +| **NavStatistics** | `string edge_id`, `string status`, `string origin`, `string target`, `string final_node`, `string date_started`, `string date_at_node`, `string date_finished`, `float64 time_to_waypoint`, `float64 operation_time`, `string topological_map` | Detailed statistics for one edge traversal | +| **CurrentEdge** | `Header header`, `string edge_id`, `bool active`, `bool result` | Currently active edge status | +| **GotoNodeFeedback** | `string route` | Navigation progress feedback (remaining route) | +| **ExecutePolicyModeFeedback** | `string current_wp`, `uint8 status` | Policy execution progress | +| **ExecutePolicyModeGoal** | `Header header`, `NavRoute route` | Policy execution goal with header | +| **TopologicalMap** | `string name`, `string map`, `string pointset`, `string last_updated`, `TopologicalNode[] nodes` | Full topological map structure | +| **TopologicalNode** | `string name`, `string map`, `string pointset`, `Pose pose`, `float64 yaw_goal_tolerance`, `float64 xy_goal_tolerance`, `Vertex[] verts`, `Edge[] edges`, `string localise_by_topic` | Single topological node with edges and influence zone | +| **Edge** | `string edge_id`, `string node`, `string action`, `float64 top_vel`, `string map_2d`, `float64 inflation_radius`, `string recovery_behaviours_config` | Edge connecting two nodes | +| **Vertex** | `float32 x`, `float32 y` | 2D vertex of an influence zone polygon | +| **AddNodeReq** | `string name`, `Pose pose`, `bool add_close_nodes` | Request to add a node to the map | +| **AddEdgeReq** | `string origin`, `string destination`, `string action`, `string action_type`, `string edge_id` | Request to add an edge between nodes | +| **SetInfluenceZoneReq** | `string name`, `float64[] vertices_x`, `float64[] vertices_y` | Request to set a node's influence zone polygon | +| **UpdateEdgeConfigReq** | `string edge_id`, `string namespace_name`, `string name`, `string value`, `bool value_is_string`, `bool not_reset` | Request to update edge configuration parameters | +| **TopologicalOccupiedNode** | `string[] nodes` | List of currently occupied node names | + +### Services (.srv) + +| Service | Request → Response | Description | +|---------|--------------------|-------------| +| **LocalisePose** | `Pose pose` → `string current_node, string closest_node` | Determine which node a pose belongs to | +| **GetRouteTo** | `string goal` → `NavRoute route` | Get route from current position to goal node | +| **GetRouteBetween** | `string origin, string goal` → `NavRoute route` | Get route between two named nodes | +| **WriteTopologicalMap** | `string filename, bool no_alias` → `bool success, string message` | Write / save a topological map to disk | +| **EvaluateEdge** | `string state, string edge, bool runtime` → `bool success, bool evaluation` | Evaluate restrictions on an edge | +| **EvaluateNode** | `string state, string node, bool runtime` → `bool success, bool evaluation` | Evaluate restrictions on a node | +| **ReconfAtEdges** | `string edge_id` → `bool success` | Reconfigure parameters at edge traversal | +| **LoadTopoNavTestScenario** | `string pointset` → *(empty)* | Load a test scenario | +| **RunTopoNavTestScenario** | *(empty)* → `bool nav_success, bool nav_timeout, bool graceful_fail, bool human_success, float64 min_distance_to_human, float64 mean_speed, float64 distance_travelled, float64 travel_time` | Run a test scenario and return metrics | + +### Actions (.action) + +| Action | Goal | Result | Feedback | +|--------|------|--------|----------| +| **GotoNode** | `string target`, `bool no_orientation` | `bool success` | `string route` | +| **ExecutePolicyMode** | `NavRoute route` | `bool success` | `string current_wp`, `uint8 status` | +| **BuildTopPrediction** | `Time start_range`, `Time end_range` | `bool success` | `string result` | + +--- + +## Topological Map Format + +Topological maps are YAML files with a `.tmap2.yaml` extension. Here is an abbreviated example: + +```yaml +meta: + last_updated: 08-03-2026_12-00-00 + origin: + latitude: 53.268 + longitude: -0.524 + altitude: 0 + +metric_map: my_farm +name: my_farm +pointset: my_farm + +transformation: + topo_frame_id: my_farm # Child frame for node poses + parent: map # Parent TF frame + rotation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0} + translation: {x: 0.0, y: 0.0, z: 0.0} + +definitions: + default_bt: | + + + + + + + + + +actions: + navigate_to_pose: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + +nodes: + - meta: + map: my_farm + node: RowEntry_A1 + pointset: my_farm + node: + name: RowEntry_A1 + nav_frame: '${transformation.topo_frame_id}' + pose: + position: {x: 10.5, y: 5.2, z: 0.0} + orientation: {w: 0.707, x: 0.0, y: 0.0, z: 0.707} + properties: # Flexible key-value metadata + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + verts: # Influence zone polygon + - {x: -0.5, y: -0.5} + - {x: 0.5, y: -0.5} + - {x: 0.5, y: 0.5} + - {x: -0.5, y: 0.5} + edges: + - edge_id: RowEntry_A1_RowEnd_A1 + node: RowEnd_A1 # Target node name + action: navigate_to_pose # Action key from 'actions' section + properties: + max_speed: 0.5 +``` + +**Key sections:** +- **`meta`**: Map metadata (timestamps, origin coordinates) +- **`transformation`**: TF frame for all node poses (published as a static transform) +- **`definitions`**: Reusable templates (e.g., Nav2 behavior trees) +- **`actions`**: Named action configurations — each maps to a ROS 2 action server with a goal template supporting variable substitution (`${node.pose}`, `${definitions.default_bt}`, etc.) +- **`nodes`**: List of topological nodes, each with pose, properties, influence zone vertices, and outgoing edges + +### Flexible Properties System + +Both nodes and edges support an optional `properties` dictionary. Properties are freeform key-value pairs — no schema changes needed: + +```yaml +properties: + xy_goal_tolerance: 0.3 # Navigation tolerance + semantics: "row_entry" # Semantic label + roboflow: # Namespaced properties + enabled: true + confidence: 0.7 +``` + +Always access properties defensively in code: +```python +props = node["node"].get("properties", {}) +tolerance = props.get("xy_goal_tolerance", 0.5) +``` + +See [doc/PROPERTIES.md](topological_navigation/doc/PROPERTIES.md) for full documentation. + +### Map Validation + +```bash +ros2 run topological_navigation validate_map.py /path/to/map.tmap2.yaml -v +``` + +--- + +## Command-Line Utilities + +| Command | Description | +|---------|-------------| +| `ros2 run topological_navigation validate_map.py -v` | Validate a map against the JSON schema | +| `ros2 run topological_navigation convert_tmap.py ` | Convert between topological map format versions | +| `ros2 action send_goal /topological_navigation topological_navigation_msgs/action/GotoNode "{target: 'node_name'}"` | Send a navigation goal | +| `ros2 topic echo /current_node` | Monitor which node the robot is at | +| `ros2 topic echo /closest_node` | Monitor the nearest node | +| `ros2 service call /topological_localisation/localise_pose topological_navigation_msgs/srv/LocalisePose "{pose: {position: {x: 1.0, y: 2.0, z: 0.0}}}"` | Localise an arbitrary pose | + +--- + +## Testing + +```bash +# Build the workspace +colcon build + +# Run all tests for the core package +colcon test --packages-select topological_navigation --event-handlers console_direct+ + +# Run specific tests +cd topological_navigation +pytest test/test_networkx_utils.py -v +pytest test/test_localisation2.py -v + +# Python linting +colcon test --packages-select topological_navigation \ + --event-handlers console_direct+ --pytest-args -k "test_flake8 or test_pep257" +``` + +--- + +## Contributing + +1. Follow **PEP 8** / **PEP 257** for Python code +2. Use `ament_flake8` and `ament_pep257` for validation +3. Add tests for new functionality +4. Validate maps with `validate_map.py` +5. Update documentation when changing behavior + +--- -- **[topological_navigation](topological_navigation/README.md)** - Core topological navigation and mapping functionality -- **[topological_navigation_msgs](topological_navigation_msgs/)** - Message, service, and action definitions for topological navigation -- **[topological_utils](topological_utils/)** - Utility tools for creating and managing topological maps -- **[topological_rviz_tools](topological_rviz_tools/README.md)** - RViz-based tools for interactive topological map construction and editing +## License -Please refer to the individual package README files for detailed documentation and usage instructions. +Apache License 2.0 — see [LICENSE](LICENSE) for details. diff --git a/conftest.py b/conftest.py new file mode 100644 index 00000000..5aff1d83 --- /dev/null +++ b/conftest.py @@ -0,0 +1,29 @@ +""" +Pytest configuration for topological_navigation workspace. + +This file ensures that the topological_navigation package can be imported +during testing by adding the source and build directories to the Python path. +""" + +import sys +from pathlib import Path + +# The ROS package directory that contains the actual Python package +_pkg_dir = Path(__file__).parent / 'topological_navigation' +if _pkg_dir.exists() and str(_pkg_dir) not in sys.path: + sys.path.insert(0, str(_pkg_dir)) + +# Also try the build directory (for colcon builds) +_build_dir = Path(__file__).parent / 'build' / 'topological_navigation' +if _build_dir.exists() and str(_build_dir) not in sys.path: + sys.path.insert(0, str(_build_dir)) + + +def pytest_configure(config): + """Pytest hook that runs before test collection. + + Ensures the source and build directories are in the Python path. + """ + for d in (_pkg_dir, _build_dir): + if d.exists() and str(d) not in sys.path: + sys.path.insert(0, str(d)) diff --git a/topological_nav_simulator/README.md b/topological_nav_simulator/README.md new file mode 100644 index 00000000..f02cd5c7 --- /dev/null +++ b/topological_nav_simulator/README.md @@ -0,0 +1,114 @@ +# topological_nav_simulator + +Fake Nav2 action servers with a virtual robot for testing topological navigation **without a real robot or the full Nav2 stack**. + +## Motivation + +When developing and testing topological navigation behaviour (route planning, edge actions, localisation), you need a Nav2 action server responding to `NavigateToPose` / `NavigateThroughPoses` goals. Spinning up the full Nav2 stack with Gazebo is heavy and slow. This package provides lightweight **drop-in replacements** that: + +- Accept the same Nav2 action goals +- Simulate robot movement at configurable speed +- Publish TF (`map → odom → base_link`) so `localisation2` works +- Publish `/odometry/global` so `edge_action_manager2` works +- Show a **virtual robot marker** in RViz (cube + direction arrow + breadcrumb trail) +- Support cancel / preemption + +## Architecture + +``` +┌──────────────────────────────────────────────┐ +│ fake_nav2_server (node) │ +│ │ +│ Action Servers: │ +│ /navigate_to_pose (NavigateToPose) │ +│ /navigate_through_poses(NavigateThroughPoses)│ +│ /follow_waypoints (FollowWaypoints) │ +│ │ +│ Publishers: │ +│ TF: map → odom → base_link │ +│ /odometry/global (Odometry) │ +│ /virtual_robot/markers (MarkerArray) │ +│ /virtual_robot/pose (PoseStamped) │ +│ │ +│ Subscribers: │ +│ /initialpose (teleport from RViz) │ +└──────────────────────────────────────────────┘ +``` + +## Quick Start + +```bash +# Build +cd /path/to/workspace +colcon build --packages-select topological_nav_simulator +source install/setup.bash + +# Run +ros2 launch topological_nav_simulator fake_nav2.launch.py + +# In another terminal — send a test goal +ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \ + "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 5.0, y: 3.0}}}}" +``` + +## Parameters + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `robot_speed` | double | 0.8 | Linear speed (m/s) | +| `angular_speed` | double | 1.5 | Angular speed (rad/s) | +| `goal_tolerance` | double | 0.15 | Distance to snap to goal (m) | +| `update_rate` | double | 30.0 | TF/marker publish rate (Hz) | +| `initial_x` | double | 0.0 | Starting X position | +| `initial_y` | double | 0.0 | Starting Y position | +| `initial_yaw` | double | 0.0 | Starting yaw (rad) | +| `map_frame` | string | `map` | Map TF frame | +| `odom_frame` | string | `odom` | Odometry TF frame | +| `base_frame` | string | `base_link` | Robot base TF frame | + +## RViz Visualization + +The virtual robot appears as: +- **Blue cube** — robot body +- **Red arrow** — heading direction +- **Green ring** — footprint +- **Blue trail** — breadcrumb path +- **🤖 Sim Robot** — floating label + +Use the **2D Pose Estimate** tool in RViz to teleport the robot to any location. + +An RViz config is provided: +```bash +rviz2 -d $(ros2 pkg prefix topological_nav_simulator)/share/topological_nav_simulator/config/fake_nav2.rviz +``` + +## Usage with Topological Navigation + +Launch the full topological navigation stack, replacing the real Nav2 with this simulator: + +```bash +# Terminal 1: Fake Nav2 +ros2 launch topological_nav_simulator fake_nav2.launch.py \ + initial_x:=10.5 initial_y:=5.2 + +# Terminal 2: Map manager + localisation + navigation +ros2 run topological_navigation map_manager2.py --ros-args -p topological_map2_name:=my_map +ros2 run topological_navigation localisation2.py +ros2 run topological_navigation navigation2.py + +# Terminal 3: Send topological nav goal +ros2 action send_goal /topological_navigation \ + topological_navigation_msgs/action/GotoNode \ + "{target: 'WayPoint1'}" +``` + +## Movement Simulation + +When a goal is received, the robot: +1. **Rotates** to face the target position +2. **Drives straight** toward it at `robot_speed` +3. **Rotates** to the goal's final orientation + +For `NavigateThroughPoses`, it moves through each waypoint sequentially. + +Feedback is published with `distance_remaining` so the topological navigation stack can monitor progress. diff --git a/topological_nav_simulator/config/fake_nav2.rviz b/topological_nav_simulator/config/fake_nav2.rviz new file mode 100644 index 00000000..f559d855 --- /dev/null +++ b/topological_nav_simulator/config/fake_nav2.rviz @@ -0,0 +1,57 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/TF + Name: TF + Enabled: true + Show Arrows: true + Show Axes: true + Show Names: true + Update Interval: 0 + Frame Timeout: 15 + - Class: rviz_default_plugins/MarkerArray + Name: Virtual Robot + Enabled: true + Topic: + Value: /virtual_robot/markers + Depth: 5 + Reliability Policy: Reliable + - Class: rviz_default_plugins/Grid + Name: Grid + Enabled: true + Line Style: + Value: Lines + Cell Size: 1 + Color: 160; 160; 164 + Plane: XY + Plane Cell Count: 50 + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/SetInitialPose + Topic: + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Value: /goal_pose + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 30 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: TopDown + Pitch: 1.5707 + Yaw: 0 +Window Geometry: + Width: 1200 + Height: 800 diff --git a/topological_nav_simulator/launch/fake_nav2.launch.py b/topological_nav_simulator/launch/fake_nav2.launch.py new file mode 100644 index 00000000..7d6f2c1e --- /dev/null +++ b/topological_nav_simulator/launch/fake_nav2.launch.py @@ -0,0 +1,65 @@ +# Copyright (c) 2026, topological_navigation contributors +# Licensed under the MIT License. +"""Launch fake Nav2 server with virtual robot visualization.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + """Generate launch description for the fake Nav2 simulator.""" + return LaunchDescription([ + # --- Arguments --- + DeclareLaunchArgument( + 'robot_speed', default_value='0.8', + description='Simulated robot linear speed (m/s)'), + DeclareLaunchArgument( + 'angular_speed', default_value='1.5', + description='Simulated robot angular speed (rad/s)'), + DeclareLaunchArgument( + 'goal_tolerance', default_value='0.15', + description='Distance tolerance for reaching goals (m)'), + DeclareLaunchArgument( + 'update_rate', default_value='30.0', + description='TF and marker publish rate (Hz)'), + DeclareLaunchArgument( + 'initial_x', default_value='0.0', + description='Initial robot X position'), + DeclareLaunchArgument( + 'initial_y', default_value='0.0', + description='Initial robot Y position'), + DeclareLaunchArgument( + 'initial_yaw', default_value='0.0', + description='Initial robot yaw (radians)'), + DeclareLaunchArgument( + 'map_frame', default_value='map', + description='Map TF frame'), + DeclareLaunchArgument( + 'odom_frame', default_value='odom', + description='Odometry TF frame'), + DeclareLaunchArgument( + 'base_frame', default_value='base_link', + description='Robot base TF frame'), + + # --- Fake Nav2 Server --- + Node( + package='topological_nav_simulator', + executable='fake_nav2_server', + name='fake_nav2_server', + output='screen', + parameters=[{ + 'robot_speed': LaunchConfiguration('robot_speed'), + 'angular_speed': LaunchConfiguration('angular_speed'), + 'goal_tolerance': LaunchConfiguration('goal_tolerance'), + 'update_rate': LaunchConfiguration('update_rate'), + 'initial_x': LaunchConfiguration('initial_x'), + 'initial_y': LaunchConfiguration('initial_y'), + 'initial_yaw': LaunchConfiguration('initial_yaw'), + 'map_frame': LaunchConfiguration('map_frame'), + 'odom_frame': LaunchConfiguration('odom_frame'), + 'base_frame': LaunchConfiguration('base_frame'), + }], + ), + ]) diff --git a/topological_nav_simulator/package.xml b/topological_nav_simulator/package.xml new file mode 100644 index 00000000..2e724c3a --- /dev/null +++ b/topological_nav_simulator/package.xml @@ -0,0 +1,34 @@ + + + + topological_nav_simulator + 1.0.0 + + Fake Nav2 action servers with a virtual robot for testing + topological navigation without a real robot or Nav2 stack. + Provides NavigateToPose, NavigateThroughPoses, and FollowWaypoints + action servers that simulate movement and visualize a virtual robot + marker in RViz. + + + Ibrahim Hroob + MIT + + rclpy + nav2_msgs + geometry_msgs + nav_msgs + std_msgs + visualization_msgs + tf2_ros + tf_transformations + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/topological_navigation/__init__.py b/topological_nav_simulator/resource/topological_nav_simulator similarity index 100% rename from topological_navigation/__init__.py rename to topological_nav_simulator/resource/topological_nav_simulator diff --git a/topological_nav_simulator/setup.cfg b/topological_nav_simulator/setup.cfg new file mode 100644 index 00000000..b27d5747 --- /dev/null +++ b/topological_nav_simulator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/topological_nav_simulator +[install] +install_scripts=$base/lib/topological_nav_simulator diff --git a/topological_nav_simulator/setup.py b/topological_nav_simulator/setup.py new file mode 100644 index 00000000..89a29c13 --- /dev/null +++ b/topological_nav_simulator/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup +from glob import glob + +package_name = 'topological_nav_simulator' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch/', glob('launch/*')), + ('share/' + package_name + '/config/', glob('config/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Ibrahim Hroob', + maintainer_email='ihroob@lincoln.ac.uk', + description='Fake Nav2 action servers with virtual robot for testing topological navigation', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'fake_nav2_server = topological_nav_simulator.fake_nav2_server:main', + ], + }, +) diff --git a/topological_nav_simulator/test/test_virtual_robot.py b/topological_nav_simulator/test/test_virtual_robot.py new file mode 100644 index 00000000..3d2c0626 --- /dev/null +++ b/topological_nav_simulator/test/test_virtual_robot.py @@ -0,0 +1,62 @@ +# Copyright (c) 2026, topological_navigation contributors +# Licensed under the MIT License. +"""Tests for virtual_robot module.""" + +import math + +import pytest +geometry_msgs = pytest.importorskip('geometry_msgs', reason='geometry_msgs not available') +from geometry_msgs.msg import Pose, Quaternion # noqa: E402 +from topological_nav_simulator.virtual_robot import ( + _euler_from_quaternion, + _lerp, + _quaternion_from_yaw, + _slerp_yaw, +) + + +def test_quaternion_from_yaw_zero(): + """Yaw=0 should produce identity-like quaternion.""" + q = _quaternion_from_yaw(0.0) + assert abs(q.w - 1.0) < 1e-6 + assert abs(q.z) < 1e-6 + + +def test_quaternion_from_yaw_90(): + """Yaw=pi/2 should produce z~0.707, w~0.707.""" + q = _quaternion_from_yaw(math.pi / 2.0) + assert abs(q.z - math.sin(math.pi / 4.0)) < 1e-6 + assert abs(q.w - math.cos(math.pi / 4.0)) < 1e-6 + + +def test_euler_from_quaternion_roundtrip(): + """Converting yaw to quaternion and back should preserve the value.""" + for yaw in [0.0, math.pi / 4, -math.pi / 3, math.pi, -math.pi / 2]: + q = _quaternion_from_yaw(yaw) + recovered = _euler_from_quaternion(q) + diff = abs(math.atan2(math.sin(yaw - recovered), math.cos(yaw - recovered))) + assert diff < 1e-6, f'Yaw {yaw} roundtrip failed: got {recovered}' + + +def test_lerp(): + """Linear interpolation basic checks.""" + assert abs(_lerp(0.0, 10.0, 0.0) - 0.0) < 1e-9 + assert abs(_lerp(0.0, 10.0, 1.0) - 10.0) < 1e-9 + assert abs(_lerp(0.0, 10.0, 0.5) - 5.0) < 1e-9 + + +def test_slerp_yaw(): + """Yaw interpolation basic checks.""" + assert abs(_slerp_yaw(0.0, math.pi, 0.0) - 0.0) < 1e-6 + r = _slerp_yaw(0.0, math.pi / 2, 0.5) + assert abs(r - math.pi / 4) < 1e-6 + + +def test_slerp_yaw_wraparound(): + """Slerp should take the short path around +-pi.""" + # From -170 to 170 degrees should go through 180, not through 0 + a = math.radians(-170) + b = math.radians(170) + mid = _slerp_yaw(a, b, 0.5) + # Should be near +-180 degrees + assert abs(abs(mid) - math.pi) < math.radians(5) diff --git a/topological_nav_simulator/topological_nav_simulator/__init__.py b/topological_nav_simulator/topological_nav_simulator/__init__.py new file mode 100644 index 00000000..f86b3e62 --- /dev/null +++ b/topological_nav_simulator/topological_nav_simulator/__init__.py @@ -0,0 +1,2 @@ +# Copyright (c) 2026, topological_navigation contributors +# Licensed under the MIT License. diff --git a/topological_nav_simulator/topological_nav_simulator/fake_nav2_server.py b/topological_nav_simulator/topological_nav_simulator/fake_nav2_server.py new file mode 100644 index 00000000..a306e1ef --- /dev/null +++ b/topological_nav_simulator/topological_nav_simulator/fake_nav2_server.py @@ -0,0 +1,446 @@ +# Copyright (c) 2026, topological_navigation contributors +# Licensed under the MIT License. +""" +Fake Nav2 action servers for testing topological navigation. + +Provides NavigateToPose, NavigateThroughPoses, and FollowWaypoints action +servers that simulate robot movement and visualize a virtual robot in RViz. + +The virtual robot: +- Publishes TF (map -> odom -> base_link) so localisation2 works +- Publishes /odometry/global so edge_action_manager2 works +- Shows a coloured cube + direction arrow + trail in RViz +- Moves at a configurable speed toward goal poses + +Usage: + ros2 run topological_nav_simulator fake_nav2_server + ros2 run topological_nav_simulator fake_nav2_server --ros-args \ + -p robot_speed:=1.0 \ + -p initial_x:=0.0 \ + -p initial_y:=0.0 +""" + +import math +import threading +import time + +import rclpy +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node + +from geometry_msgs.msg import Pose, PoseStamped, Quaternion +from nav2_msgs.action import ( + FollowWaypoints, + NavigateThroughPoses, + NavigateToPose, +) + +from topological_nav_simulator.virtual_robot import ( + VirtualRobot, + _euler_from_quaternion, + _lerp, + _quaternion_from_yaw, + _slerp_yaw, +) + + +class FakeNav2Server(Node): + """ROS 2 node providing fake Nav2 action servers with a virtual robot.""" + + def __init__(self): + super().__init__('fake_nav2_server') + + # --- Parameters --- + self.declare_parameter('robot_speed', 0.8) + self.declare_parameter('angular_speed', 1.5) + self.declare_parameter('goal_tolerance', 0.15) + self.declare_parameter('update_rate', 30.0) + self.declare_parameter('initial_x', 0.0) + self.declare_parameter('initial_y', 0.0) + self.declare_parameter('initial_yaw', 0.0) + self.declare_parameter('map_frame', 'map') + self.declare_parameter('odom_frame', 'odom') + self.declare_parameter('base_frame', 'base_link') + + self._speed = self.get_parameter('robot_speed').value + self._ang_speed = self.get_parameter('angular_speed').value + self._goal_tol = self.get_parameter('goal_tolerance').value + self._rate = self.get_parameter('update_rate').value + init_x = self.get_parameter('initial_x').value + init_y = self.get_parameter('initial_y').value + init_yaw = self.get_parameter('initial_yaw').value + map_frame = self.get_parameter('map_frame').value + odom_frame = self.get_parameter('odom_frame').value + base_frame = self.get_parameter('base_frame').value + + # --- Virtual robot --- + self._robot = VirtualRobot(self, map_frame, base_frame, odom_frame) + initial_pose = Pose() + initial_pose.position.x = float(init_x) + initial_pose.position.y = float(init_y) + initial_pose.orientation = _quaternion_from_yaw(float(init_yaw)) + self._robot.set_pose(initial_pose) + + # Lock for concurrent goal handling + self._nav_lock = threading.Lock() + self._current_goal_handle = None + + # Callback group for action servers + cb_group = ReentrantCallbackGroup() + + # --- Action Servers --- + self._nav_to_pose_server = ActionServer( + self, + NavigateToPose, + 'navigate_to_pose', + execute_callback=self._execute_navigate_to_pose, + goal_callback=self._goal_callback, + cancel_callback=self._cancel_callback, + callback_group=cb_group, + ) + + self._nav_through_poses_server = ActionServer( + self, + NavigateThroughPoses, + 'navigate_through_poses', + execute_callback=self._execute_navigate_through_poses, + goal_callback=self._goal_callback, + cancel_callback=self._cancel_callback, + callback_group=cb_group, + ) + + self._follow_waypoints_server = ActionServer( + self, + FollowWaypoints, + 'follow_waypoints', + execute_callback=self._execute_follow_waypoints, + goal_callback=self._goal_callback, + cancel_callback=self._cancel_callback, + callback_group=cb_group, + ) + + # --- Initial pose subscriber (RViz 2D Pose Estimate) --- + self.create_subscription( + PoseStamped, + '/initialpose', + self._initialpose_cb, + 10, + ) + + # --- Periodic TF + marker publishing (even when idle) --- + timer_period = 1.0 / self._rate + self._pub_timer = self.create_timer(timer_period, self._publish_tick) + + self.get_logger().info( + '=== Fake Nav2 Server Started ===\n' + f' Speed: {self._speed} m/s | Angular: {self._ang_speed} rad/s\n' + f' Tolerance: {self._goal_tol} m | Rate: {self._rate} Hz\n' + f' Initial pose: ({init_x:.2f}, {init_y:.2f}, yaw={init_yaw:.2f})\n' + f' Action servers:\n' + f' /navigate_to_pose\n' + f' /navigate_through_poses\n' + f' /follow_waypoints\n' + f' Topics published:\n' + f' /odometry/global\n' + f' /virtual_robot/markers\n' + f' /virtual_robot/pose\n' + f' TF: {map_frame} -> {odom_frame} -> {base_frame}' + ) + + # ── Callbacks ──────────────────────────────────────────────────────── + + def _goal_callback(self, goal_request): + """Accept all incoming goals.""" + self.get_logger().info('Goal received') + return GoalResponse.ACCEPT + + def _cancel_callback(self, goal_handle): + """Accept all cancel requests.""" + self.get_logger().info('Cancel requested') + return CancelResponse.ACCEPT + + def _initialpose_cb(self, msg): + """Teleport robot to initial pose from RViz 2D Pose Estimate.""" + self.get_logger().info( + f'Teleporting robot to ({msg.pose.position.x:.2f}, ' + f'{msg.pose.position.y:.2f})' + ) + self._robot.set_pose(msg.pose) + self._robot.clear_trail() + + def _publish_tick(self): + """Periodic broadcast of TF and markers (even when idle).""" + self._robot.publish_all() + + # ── NavigateToPose ─────────────────────────────────────────────────── + + def _execute_navigate_to_pose(self, goal_handle): + """Execute NavigateToPose action — move to a single goal.""" + self.get_logger().info( + f'NavigateToPose -> ' + f'({goal_handle.request.pose.pose.position.x:.2f}, ' + f'{goal_handle.request.pose.pose.position.y:.2f})' + ) + + target = goal_handle.request.pose.pose + success = self._move_to_pose(target, goal_handle, 'NavigateToPose') + + result = NavigateToPose.Result() + if success: + goal_handle.succeed() + self.get_logger().info('NavigateToPose succeeded') + elif goal_handle.is_cancel_requested or goal_handle.status == 5: + # Already transitioned to CANCELED inside _move_to_pose + self.get_logger().info('NavigateToPose cancelled') + else: + goal_handle.abort() + self.get_logger().warn('NavigateToPose aborted') + return result + + # ── NavigateThroughPoses ───────────────────────────────────────────── + + def _execute_navigate_through_poses(self, goal_handle): + """Execute NavigateThroughPoses — move through a list of poses.""" + poses = goal_handle.request.poses + self.get_logger().info( + f'NavigateThroughPoses -> {len(poses)} waypoint(s)' + ) + + success = True + for i, pose_stamped in enumerate(poses): + self.get_logger().info( + f' Waypoint {i + 1}/{len(poses)}: ' + f'({pose_stamped.pose.position.x:.2f}, ' + f'{pose_stamped.pose.position.y:.2f})' + ) + if not self._move_to_pose( + pose_stamped.pose, goal_handle, + f'NavigateThroughPoses [{i + 1}/{len(poses)}]' + ): + success = False + break + + result = NavigateThroughPoses.Result() + if success: + goal_handle.succeed() + self.get_logger().info('NavigateThroughPoses succeeded') + elif goal_handle.is_cancel_requested or goal_handle.status == 5: + # Already transitioned to CANCELED inside _move_to_pose + self.get_logger().info('NavigateThroughPoses cancelled') + else: + goal_handle.abort() + self.get_logger().warn('NavigateThroughPoses aborted') + return result + + # ── FollowWaypoints ────────────────────────────────────────────────── + + def _execute_follow_waypoints(self, goal_handle): + """Execute FollowWaypoints — move through waypoints sequentially.""" + poses = goal_handle.request.poses + self.get_logger().info( + f'FollowWaypoints -> {len(poses)} waypoint(s)' + ) + + missed = [] + for i, pose_stamped in enumerate(poses): + self.get_logger().info( + f' Waypoint {i + 1}/{len(poses)}: ' + f'({pose_stamped.pose.position.x:.2f}, ' + f'{pose_stamped.pose.position.y:.2f})' + ) + if not self._move_to_pose( + pose_stamped.pose, goal_handle, + f'FollowWaypoints [{i + 1}/{len(poses)}]' + ): + missed.append(i) + break + + result = FollowWaypoints.Result() + result.missed_waypoints = missed + if not missed: + goal_handle.succeed() + self.get_logger().info('FollowWaypoints succeeded') + elif goal_handle.is_cancel_requested or goal_handle.status == 5: + # Already transitioned to CANCELED inside _move_to_pose + self.get_logger().info('FollowWaypoints cancelled') + else: + goal_handle.abort() + self.get_logger().warn( + f'FollowWaypoints aborted (missed: {missed})' + ) + return result + + # ── Core movement simulation ───────────────────────────────────────── + + def _move_to_pose(self, target_pose, goal_handle, action_label): + """ + Simulate robot movement from current pose to target pose. + + The robot first rotates to face the target, then drives in a + straight line, and finally rotates to the goal orientation. + Returns True on success, False if cancelled. + """ + dt = 1.0 / self._rate + + # Phase 1: Rotate to face the target + if not self._rotate_toward_target(target_pose, goal_handle, dt): + return False + + # Phase 2: Drive to the target position + if not self._drive_to_position(target_pose, goal_handle, dt, action_label): + return False + + # Phase 3: Rotate to final orientation + if not self._rotate_to_orientation(target_pose, goal_handle, dt): + return False + + return True + + def _rotate_toward_target(self, target_pose, goal_handle, dt): + """Rotate the robot to face the target position.""" + dx = target_pose.position.x - self._robot.pose.position.x + dy = target_pose.position.y - self._robot.pose.position.y + dist = math.hypot(dx, dy) + + if dist < self._goal_tol: + return True # Already close, skip rotation + + target_yaw = math.atan2(dy, dx) + current_yaw = _euler_from_quaternion(self._robot.pose.orientation) + yaw_diff = math.atan2( + math.sin(target_yaw - current_yaw), + math.cos(target_yaw - current_yaw), + ) + + if abs(yaw_diff) < 0.05: + return True # Close enough + + rotation_time = abs(yaw_diff) / self._ang_speed + steps = max(int(rotation_time / dt), 1) + start_yaw = current_yaw + + for i in range(1, steps + 1): + if goal_handle.is_cancel_requested: + goal_handle.canceled() + return False + t = float(i) / steps + yaw = _slerp_yaw(start_yaw, target_yaw, t) + self._robot.pose.orientation = _quaternion_from_yaw(yaw) + time.sleep(dt) + + return True + + def _drive_to_position(self, target_pose, goal_handle, dt, action_label): + """Drive the robot in a straight line toward the target.""" + while True: + if goal_handle.is_cancel_requested: + goal_handle.canceled() + return False + + dx = target_pose.position.x - self._robot.pose.position.x + dy = target_pose.position.y - self._robot.pose.position.y + dist = math.hypot(dx, dy) + + if dist < self._goal_tol: + # Snap to exact position + self._robot.pose.position.x = target_pose.position.x + self._robot.pose.position.y = target_pose.position.y + return True + + # Move toward target + step = min(self._speed * dt, dist) + angle = math.atan2(dy, dx) + self._robot.pose.position.x += step * math.cos(angle) + self._robot.pose.position.y += step * math.sin(angle) + self._robot.pose.orientation = _quaternion_from_yaw(angle) + + # Publish feedback + self._publish_feedback(goal_handle, action_label, dist) + + time.sleep(dt) + + def _rotate_to_orientation(self, target_pose, goal_handle, dt): + """Rotate the robot to the goal's final orientation.""" + target_yaw = _euler_from_quaternion(target_pose.orientation) + current_yaw = _euler_from_quaternion(self._robot.pose.orientation) + yaw_diff = math.atan2( + math.sin(target_yaw - current_yaw), + math.cos(target_yaw - current_yaw), + ) + + if abs(yaw_diff) < 0.05: + self._robot.pose.orientation = target_pose.orientation + return True + + rotation_time = abs(yaw_diff) / self._ang_speed + steps = max(int(rotation_time / dt), 1) + start_yaw = current_yaw + + for i in range(1, steps + 1): + if goal_handle.is_cancel_requested: + goal_handle.canceled() + return False + t = float(i) / steps + yaw = _slerp_yaw(start_yaw, target_yaw, t) + self._robot.pose.orientation = _quaternion_from_yaw(yaw) + time.sleep(dt) + + self._robot.pose.orientation = target_pose.orientation + return True + + def _publish_feedback(self, goal_handle, label, distance_remaining): + """Publish action feedback with distance remaining.""" + # NavigateToPose and NavigateThroughPoses have different feedback types + # but both have a current_pose field and distance_remaining + try: + # Try NavigateThroughPoses feedback + feedback = NavigateThroughPoses.Feedback() + feedback.current_pose = PoseStamped() + feedback.current_pose.header.frame_id = ( + self.get_parameter('map_frame').value + ) + feedback.current_pose.header.stamp = ( + self.get_clock().now().to_msg() + ) + feedback.current_pose.pose = self._robot.pose + feedback.distance_remaining = float(distance_remaining) + goal_handle.publish_feedback(feedback) + except Exception: + # Fallback: try NavigateToPose feedback + try: + feedback = NavigateToPose.Feedback() + feedback.current_pose = PoseStamped() + feedback.current_pose.header.frame_id = ( + self.get_parameter('map_frame').value + ) + feedback.current_pose.header.stamp = ( + self.get_clock().now().to_msg() + ) + feedback.current_pose.pose = self._robot.pose + feedback.distance_remaining = float(distance_remaining) + goal_handle.publish_feedback(feedback) + except Exception: + pass # FollowWaypoints has different feedback + + +def main(args=None): + """Entry point.""" + rclpy.init(args=args) + node = FakeNav2Server() + + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(node) + + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/topological_nav_simulator/topological_nav_simulator/virtual_robot.py b/topological_nav_simulator/topological_nav_simulator/virtual_robot.py new file mode 100644 index 00000000..1da6a4bf --- /dev/null +++ b/topological_nav_simulator/topological_nav_simulator/virtual_robot.py @@ -0,0 +1,231 @@ +# Copyright (c) 2026, topological_navigation contributors +# Licensed under the MIT License. +"""Virtual robot that tracks simulated pose, publishes TF and RViz markers.""" + +import math + +import rclpy +from geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Vector3, +) +from nav_msgs.msg import Odometry +from std_msgs.msg import ColorRGBA, Header +from tf2_ros import TransformBroadcaster +from visualization_msgs.msg import Marker, MarkerArray + + +def _euler_from_quaternion(q): + """Extract yaw from quaternion (z-axis rotation).""" + siny_cosp = 2.0 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) + return math.atan2(siny_cosp, cosy_cosp) + + +def _quaternion_from_yaw(yaw): + """Create quaternion from yaw angle.""" + return Quaternion(x=0.0, y=0.0, z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0)) + + +def _lerp(a, b, t): + """Linear interpolation between a and b.""" + return a + (b - a) * t + + +def _slerp_yaw(yaw_a, yaw_b, t): + """Spherical linear interpolation for yaw angles.""" + diff = yaw_b - yaw_a + # Normalize to [-pi, pi] + diff = math.atan2(math.sin(diff), math.cos(diff)) + return yaw_a + diff * t + + +class VirtualRobot: + """Manages virtual robot state, TF broadcasting, and RViz marker publishing.""" + + def __init__(self, node, map_frame, base_frame, odom_frame): + self._node = node + self._map_frame = map_frame + self._base_frame = base_frame + self._odom_frame = odom_frame + + # Current pose + self.pose = Pose() + self.pose.orientation.w = 1.0 + + # TF broadcaster + self._tf_broadcaster = TransformBroadcaster(node) + + # Publishers + self._marker_pub = node.create_publisher( + MarkerArray, '/virtual_robot/markers', 10 + ) + self._odom_pub = node.create_publisher( + Odometry, '/odometry/global', 10 + ) + self._pose_pub = node.create_publisher( + PoseStamped, '/virtual_robot/pose', 10 + ) + + # Trail of past positions for visualization + self._trail_points = [] + self._max_trail_points = 500 + + node.get_logger().info( + f'Virtual robot initialized: {map_frame} -> {odom_frame} -> {base_frame}' + ) + + def set_pose(self, pose): + """Set the robot pose directly.""" + self.pose = pose + + def publish_all(self): + """Publish TF, odometry, markers, and pose.""" + now = self._node.get_clock().now().to_msg() + self._publish_tf(now) + self._publish_odometry(now) + self._publish_pose(now) + self._publish_markers(now) + + def _publish_tf(self, stamp): + """Broadcast map->odom (identity) and odom->base_link transforms.""" + # map -> odom (identity) + t_map_odom = TransformStamped() + t_map_odom.header.stamp = stamp + t_map_odom.header.frame_id = self._map_frame + t_map_odom.child_frame_id = self._odom_frame + t_map_odom.transform.rotation.w = 1.0 + + # odom -> base_link (robot pose) + t_odom_base = TransformStamped() + t_odom_base.header.stamp = stamp + t_odom_base.header.frame_id = self._odom_frame + t_odom_base.child_frame_id = self._base_frame + t_odom_base.transform.translation.x = self.pose.position.x + t_odom_base.transform.translation.y = self.pose.position.y + t_odom_base.transform.translation.z = self.pose.position.z + t_odom_base.transform.rotation = self.pose.orientation + + self._tf_broadcaster.sendTransform([t_map_odom, t_odom_base]) + + def _publish_odometry(self, stamp): + """Publish odometry message.""" + odom = Odometry() + odom.header.stamp = stamp + odom.header.frame_id = self._odom_frame + odom.child_frame_id = self._base_frame + odom.pose.pose = self.pose + self._odom_pub.publish(odom) + + def _publish_pose(self, stamp): + """Publish pose stamped.""" + ps = PoseStamped() + ps.header.stamp = stamp + ps.header.frame_id = self._map_frame + ps.pose = self.pose + self._pose_pub.publish(ps) + + def _publish_markers(self, stamp): + """Publish RViz markers for the virtual robot.""" + markers = MarkerArray() + header = Header(stamp=stamp, frame_id=self._map_frame) + + # --- Robot body: a cube --- + body = Marker() + body.header = header + body.ns = 'virtual_robot' + body.id = 0 + body.type = Marker.CUBE + body.action = Marker.ADD + body.pose = self.pose + body.scale = Vector3(x=0.5, y=0.3, z=0.3) + body.color = ColorRGBA(r=0.1, g=0.7, b=1.0, a=0.9) + body.lifetime.sec = 0 + markers.markers.append(body) + + # --- Direction arrow --- + arrow = Marker() + arrow.header = header + arrow.ns = 'virtual_robot' + arrow.id = 1 + arrow.type = Marker.ARROW + arrow.action = Marker.ADD + arrow.pose = self.pose + arrow.scale = Vector3(x=0.6, y=0.12, z=0.12) + arrow.color = ColorRGBA(r=1.0, g=0.3, b=0.1, a=0.9) + arrow.lifetime.sec = 0 + markers.markers.append(arrow) + + # --- Label text --- + label = Marker() + label.header = header + label.ns = 'virtual_robot' + label.id = 2 + label.type = Marker.TEXT_VIEW_FACING + label.action = Marker.ADD + label.pose = Pose() + label.pose.position.x = self.pose.position.x + label.pose.position.y = self.pose.position.y + label.pose.position.z = self.pose.position.z + 0.5 + label.pose.orientation.w = 1.0 + label.scale = Vector3(x=0.0, y=0.0, z=0.2) + label.color = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0) + label.text = '\U0001F916 Sim Robot' + label.lifetime.sec = 0 + markers.markers.append(label) + + # --- Footprint ring (circle of small spheres) --- + footprint = Marker() + footprint.header = header + footprint.ns = 'virtual_robot' + footprint.id = 3 + footprint.type = Marker.LINE_STRIP + footprint.action = Marker.ADD + footprint.pose = self.pose + footprint.scale = Vector3(x=0.03, y=0.0, z=0.0) + footprint.color = ColorRGBA(r=0.1, g=1.0, b=0.3, a=0.6) + num_pts = 32 + radius = 0.35 + for i in range(num_pts + 1): + angle = 2.0 * math.pi * i / num_pts + pt = Point( + x=radius * math.cos(angle), + y=radius * math.sin(angle), + z=0.0, + ) + footprint.points.append(pt) + footprint.lifetime.sec = 0 + markers.markers.append(footprint) + + # --- Trail (breadcrumb path) --- + self._trail_points.append(Point( + x=self.pose.position.x, + y=self.pose.position.y, + z=0.02, + )) + if len(self._trail_points) > self._max_trail_points: + self._trail_points = self._trail_points[-self._max_trail_points:] + + if len(self._trail_points) > 1: + trail = Marker() + trail.header = header + trail.ns = 'virtual_robot' + trail.id = 4 + trail.type = Marker.LINE_STRIP + trail.action = Marker.ADD + trail.pose.orientation.w = 1.0 + trail.scale = Vector3(x=0.04, y=0.0, z=0.0) + trail.color = ColorRGBA(r=0.3, g=0.3, b=1.0, a=0.4) + trail.points = list(self._trail_points) + trail.lifetime.sec = 0 + markers.markers.append(trail) + + self._marker_pub.publish(markers) + + def clear_trail(self): + """Clear the breadcrumb trail.""" + self._trail_points.clear() diff --git a/topological_navigation/README.md b/topological_navigation/README.md index 240f9eed..6021d344 100644 --- a/topological_navigation/README.md +++ b/topological_navigation/README.md @@ -1,13 +1,40 @@ Topological Navigation ====================== +> **⚠️ IMPORTANT: ROS2 Version** +> This is the ROS2 version of topological navigation (v4.0.0+). All ROS1 code has been removed. +> For ROS1 support, use version 3.x or earlier. + + +## Interactive Map Editor (NEW in v4.0.0) + +A Python-based tool for editing topological maps visually in RViz2: + +```bash +# Launch the editor +ros2 launch topological_navigation interactive_map_editor.launch.py \ + map_file:=/path/to/your/map.tmap2.yaml + +# Open RViz2 and add Interactive Markers display +rviz2 +``` + +Features: +- ✅ Move node positions by dragging markers +- ✅ Rotate node orientations using rotation controls +- ✅ Real-time visual feedback +- ✅ Save changes back to YAML files +- ✅ Auto-save option + +--- + **These instructions are intended for the legacy branch of topological navigation. Instructions for the master branch (topological navigation 2) are to do.** This package provides support for topological navigation, originally devised for the STRANDS system. ## Flexible Properties System -The topological map schema now supports a flexible properties system that allows you to attach custom metadata to nodes and edges without requiring schema modifications. This enables domain-specific customization such as setting navigation tolerances, semantic labels, operational zones, and application-specific attributes. See [doc/PROPERTIES.md](doc/PROPERTIES.md) for detailed documentation. +The topological map schema now supports a flexible properties system that allows you to attach custom metadata to nodes and edges without requiring schema modifications. This enables domain-specific customization such as setting navigation tolerances, semantic labels, operational zones, and application-specific attributes. This module requires: * move_base diff --git a/topological_navigation/config/bt_tree_default.xml b/topological_navigation/config/bt_tree_default.xml deleted file mode 100644 index 76464f11..00000000 --- a/topological_navigation/config/bt_tree_default.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/topological_navigation/config/bt_tree_goal_align.xml b/topological_navigation/config/bt_tree_goal_align.xml deleted file mode 100644 index a6341f23..00000000 --- a/topological_navigation/config/bt_tree_goal_align.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/topological_navigation/config/bt_tree_in_row.xml b/topological_navigation/config/bt_tree_in_row.xml deleted file mode 100644 index f87538dc..00000000 --- a/topological_navigation/config/bt_tree_in_row.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/topological_navigation/config/bt_tree_in_row_operation.xml b/topological_navigation/config/bt_tree_in_row_operation.xml deleted file mode 100644 index 1387f230..00000000 --- a/topological_navigation/config/bt_tree_in_row_operation.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/topological_navigation/config/bt_tree_in_row_recovery.xml b/topological_navigation/config/bt_tree_in_row_recovery.xml deleted file mode 100644 index 1387f230..00000000 --- a/topological_navigation/config/bt_tree_in_row_recovery.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/topological_navigation/config/navigation_goal.yaml b/topological_navigation/config/navigation_goal.yaml deleted file mode 100644 index 7da02e70..00000000 --- a/topological_navigation/config/navigation_goal.yaml +++ /dev/null @@ -1,8 +0,0 @@ -topological_navigation/navigation_goal: - action_type: geometry_msgs/PoseStamped - action: NavigateToPose - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose diff --git a/topological_navigation/config/sample_edge_reconfig_groups.yaml b/topological_navigation/config/sample_edge_reconfig_groups.yaml deleted file mode 100644 index d3010d48..00000000 --- a/topological_navigation/config/sample_edge_reconfig_groups.yaml +++ /dev/null @@ -1,141 +0,0 @@ -edge_nav_reconfig_groups: - group_A: - edges: [] - parameters: - - &id001 - name: forward_point_distance - ns: move_base/DWAPlannerROS - type: double - value: 0.48297542551971 - - &id002 - name: vth_samples - ns: move_base/DWAPlannerROS - type: int - value: 67.0 - - &id003 - name: prune_plan - ns: move_base/DWAPlannerROS - type: bool - value: true - - &id004 - name: goal_distance_bias - ns: move_base/DWAPlannerROS - type: double - value: 37.0 - - &id005 - name: occdist_scale - ns: move_base/DWAPlannerROS - type: double - value: 0.17095456476021975 - - &id006 - name: vx_samples - ns: move_base/DWAPlannerROS - type: int - value: 35.0 - - &id007 - name: path_distance_bias - ns: move_base/DWAPlannerROS - type: double - value: 28.786107042177363 - - &id008 - name: sim_time - ns: move_base/DWAPlannerROS - type: double - value: 2.56592652856906 - - &id009 - name: max_scaling_factor - ns: move_base/DWAPlannerROS - type: double - value: 0.572649261744206 - - &id010 - name: scaling_speed - ns: move_base/DWAPlannerROS - type: double - value: 0.013910991085600694 - - &id011 - name: sim_granularity - ns: move_base/DWAPlannerROS - type: double - value: 0.04403610426260697 - - &id012 - name: stop_time_buffer - ns: move_base/DWAPlannerROS - type: double - value: 0.18229162576485147 - - &id013 - name: oscillation_reset_dist - ns: move_base/DWAPlannerROS - type: double - value: 0.015779064792816565 - group_B: - edges: [] - parameters: - - *id001 - - *id002 - - *id003 - - *id004 - - *id005 - - *id006 - - *id007 - - *id008 - - *id009 - - *id010 - - *id011 - - *id012 - - *id013 - group_C: - edges: [] - parameters: - - name: forward_point_distance - ns: move_base/DWAPlannerROS - type: double - value: 0.1402690117118645 - - name: vth_samples - ns: move_base/DWAPlannerROS - type: int - value: 73.0 - - name: prune_plan - ns: move_base/DWAPlannerROS - type: bool - value: false - - name: goal_distance_bias - ns: move_base/DWAPlannerROS - type: double - value: 55.0 - - name: occdist_scale - ns: move_base/DWAPlannerROS - type: double - value: 0.5922531975286119 - - name: vx_samples - ns: move_base/DWAPlannerROS - type: int - value: 50.0 - - name: path_distance_bias - ns: move_base/DWAPlannerROS - type: double - value: 7.0 - - name: sim_time - ns: move_base/DWAPlannerROS - type: double - value: 2.801314735789211 - - name: max_scaling_factor - ns: move_base/DWAPlannerROS - type: double - value: 0.8779488542935023 - - name: scaling_speed - ns: move_base/DWAPlannerROS - type: double - value: 0.31177421746048395 - - name: sim_granularity - ns: move_base/DWAPlannerROS - type: double - value: 0.02537100021859829 - - name: stop_time_buffer - ns: move_base/DWAPlannerROS - type: double - value: 1.0 - - name: oscillation_reset_dist - ns: move_base/DWAPlannerROS - type: double - value: 0.054441156382406555 - diff --git a/topological_navigation/config/template_action.yaml b/topological_navigation/config/template_action.yaml new file mode 100644 index 00000000..b018d31f --- /dev/null +++ b/topological_navigation/config/template_action.yaml @@ -0,0 +1,22 @@ +definitions: + default_bt: | + + + + + + + + + +actions: + navigate_to_pose: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: # Nested to match geometry_msgs/PoseStamped + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' \ No newline at end of file diff --git a/topological_navigation/config/template_edge.yaml b/topological_navigation/config/template_edge.yaml index 04fb1b76..4b02da1d 100644 --- a/topological_navigation/config/template_edge.yaml +++ b/topological_navigation/config/template_edge.yaml @@ -1,16 +1,4 @@ -action: move_base -action_type: move_base_msgs/MoveBaseGoal -config: [] +action: navigate_to_pose edge_id: origin_destination -fail_policy: fail -fluid_navigation: True -goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose node: destination -properties: {} -recovery_behaviours_config: '' -restrictions_planning: 'True' -restrictions_runtime: 'True' \ No newline at end of file +properties: {} \ No newline at end of file diff --git a/topological_navigation/config/template_node.yaml b/topological_navigation/config/template_node.yaml new file mode 100644 index 00000000..a57a658f --- /dev/null +++ b/topological_navigation/config/template_node.yaml @@ -0,0 +1,30 @@ +meta: + map: map_2d + node: NodeName + pointset: PointSet +node: + edges: [] + name: NodeName + nav_frame: map + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 6.29 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 \ No newline at end of file diff --git a/topological_navigation/config/template_node_2.yaml b/topological_navigation/config/template_node_2.yaml deleted file mode 100644 index 16684d7e..00000000 --- a/topological_navigation/config/template_node_2.yaml +++ /dev/null @@ -1,41 +0,0 @@ -meta: - map: map_2d - node: NodeName - pointset: PointSet -node: - edges: [] - localise_by_topic: '' - name: NodeName - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 0.0 - y: 0.0 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.29 - restrictions_planning: 'True' - restrictions_runtime: 'True' - verts: - - x: 0.689999997616 - y: 0.287000000477 - - x: 0.287000000477 - y: 0.490000009537 - - x: -0.287000000477 - y: 0.490000009537 - - x: -0.689999997616 - y: 0.287000000477 - - x: -0.689999997616 - y: -0.287000000477 - - x: -0.287000000477 - y: -0.490000009537 - - x: 0.287000000477 - y: -0.490000009537 - - x: 0.689999997616 - y: -0.287000000477 \ No newline at end of file diff --git a/topological_navigation/config/test_complex_tmap2.yaml b/topological_navigation/config/test_complex_tmap2.yaml new file mode 100644 index 00000000..55185593 --- /dev/null +++ b/topological_navigation/config/test_complex_tmap2.yaml @@ -0,0 +1,1252 @@ +# ============================================================================= +# Complex topological map for comprehensive navigation testing. +# +# Layout (top-down view): +# +# TopicNode Isolated +# (-5,0) (30,0) +# | +# Charging ──nav──► HN1 ──nav_poses──► HN2 ──nav_poses──► HN3 ──nav──► WP_E +# (0,0) (5,0) (10,0) (15,0) (20,0) +# ▲ │ ▲ │ ▲ │ ▲ │ +# │ nav nav nav nav nav nav nav +# │ ▼ │ ▼ │ ▼ │ │ +# │ RowA0 │ RowB0 │ RowC0 │ ▼ +# │ (5,-3)│ (10,-3) │ (15,-3) │ AlignDock +# │ │ │ │ │ │ │ (20,-10) +# │ row │ row │ row │ +# │ │ │ │ │ │ │ +# │ ▼ │ ▼ │ ▼ │ +# │ RowA1 │ RowB1 │ RowC1 │ +# │ (5,-8)│ (10,-8) │ (15,-8) │ +# │ │ │ │ │ │ │ +# │ row │ row │ row │ +# │ │ │ │ │ │ │ +# │ ▼ │ ▼ │ ▼ │ +# │ RowA2 │ RowB2 │ RowC2 │ +# │ (5,-13)│ (10,-13) │ (15,-13) │ +# │ │ │ │ │ │ │ +# │ row │ row │ row │ +# │ │ │ │ │ │ │ +# │ ▼ │ ▼ │ ▼ │ +# │ RowA3 │ RowB3 │ RowC3 │ +# │ (5,-18)│ (10,-18) │ (15,-18) │ +# │ │ │ │ NoGo +# │ align align align (25,-10) +# │ ▼ ▼ ▼ +# │ HS1 ──nav_poses──► HS2 ──nav_poses──► HS3 ──nav──► WP_SE +# │ (5,-20) (10,-20) (15,-20) (20,-20) +# │ │ │ +# └──────nav─────┘ │ +# ◄────────────nav─────────────┘ +# +# Edge types used: +# navigate_to_pose (non-composable) – headland links, junctions +# navigate_to_poses (composable) – headland traversals, multi-waypoint +# row_traversal (composable) – crop row segments with boundaries +# goal_align (non-composable) – precision docking / row exit alignment +# +# Bidirectional edges: HN1↔HN2, HN2↔HN3, HS1↔HS2, HS2↔HS3, +# HN1↔RowA0, HN2↔RowB0, HN3↔RowC0, +# WP_E↔WP_SE, Charging↔HN1 +# Unidirectional edges: row traversals (southward only), +# align exits (row end → headland south), +# HN3→WP_E, WP_E→AlignDock, HS1→Charging, +# WP_SE→Charging +# +# Special nodes: +# Isolated – disconnected (connectivity test) +# NoGo – no_go: true (restricted area) +# TopicNode – localise_by_topic (topic-based localisation) +# AlignDock – dead-end precision docking target +# ============================================================================= + +meta: + last_updated: 09-03-2026_12-00-00 + origin: + latitude: 53.268642038 + longitude: -0.524509505881 + altitude: 0 + +metric_map: complex_nav_test +name: complex_nav_test +pointset: complex_nav_test + +transformation: + topo_frame_id: complex_nav_test + parent: map + rotation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + translation: + x: 0.0 + y: 0.0 + z: 0.0 + +# ───────────────────────────────────────────────────────────────────────────── +# Definitions – behaviour trees referenced by actions +# ───────────────────────────────────────────────────────────────────────────── +definitions: + default_bt: | + + + + + + + + + goal_align_bt: | + + + + + + + + + row_traversal_bt: | + + + + + + + + + +# ───────────────────────────────────────────────────────────────────────────── +# Actions – all four action types +# ───────────────────────────────────────────────────────────────────────────── +actions: + navigate_to_pose: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + + navigate_to_poses: + composable: true + action_type: nav2_msgs.action.NavigateThroughPoses + action_server: /navigate_through_poses + action_goal_template: + poses: + - header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + + goal_align: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.goal_align_bt}' + + row_traversal: + composable: true + action_type: nav2_msgs.action.NavigateThroughPoses + action_server: /navigate_through_poses + action_goal_template: + poses: + - header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.row_traversal_bt}' + +# ───────────────────────────────────────────────────────────────────────────── +# Nodes +# ───────────────────────────────────────────────────────────────────────────── +nodes: + + # ========================= Charging Station ================================ + - meta: + map: complex_nav_test + node: Charging + pointset: complex_nav_test + tag: ['charging_station', 'home'] + node: + edges: + # Bidirectional: Charging <-> HN1 + - action: navigate_to_pose + edge_id: Charging_HN1 + node: HN1 + properties: + max_speed: 1.0 + name: Charging + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + semantics: charging_station + priority: 10 + verts: + - x: -1.5 + y: -1.5 + - x: 1.5 + y: -1.5 + - x: 1.5 + y: 1.5 + - x: -1.5 + y: 1.5 + + # ==================== Headland North ======================================= + - meta: + map: complex_nav_test + node: HN1 + pointset: complex_nav_test + tag: ['headland', 'junction'] + node: + edges: + # Bidirectional: HN1 <-> Charging + - action: navigate_to_pose + edge_id: HN1_Charging + node: Charging + properties: + max_speed: 1.0 + # Bidirectional: HN1 <-> HN2 (composable navigate_to_poses) + - action: navigate_to_poses + edge_id: HN1_HN2 + node: HN2 + properties: + max_speed: 0.8 + # Unidirectional: HN1 -> RowA0 (enter row A from north) + - action: navigate_to_pose + edge_id: HN1_RowA0 + node: RowA0 + properties: + max_speed: 0.5 + name: HN1 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 5.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: headland_junction + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: complex_nav_test + node: HN2 + pointset: complex_nav_test + tag: ['headland', 'junction'] + node: + edges: + # Bidirectional: HN2 <-> HN1 + - action: navigate_to_poses + edge_id: HN2_HN1 + node: HN1 + properties: + max_speed: 0.8 + # Bidirectional: HN2 <-> HN3 + - action: navigate_to_poses + edge_id: HN2_HN3 + node: HN3 + properties: + max_speed: 0.8 + # Unidirectional: HN2 -> RowB0 (enter row B from north) + - action: navigate_to_pose + edge_id: HN2_RowB0 + node: RowB0 + properties: + max_speed: 0.5 + name: HN2 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 10.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: headland_junction + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: complex_nav_test + node: HN3 + pointset: complex_nav_test + tag: ['headland', 'junction'] + node: + edges: + # Bidirectional: HN3 <-> HN2 + - action: navigate_to_poses + edge_id: HN3_HN2 + node: HN2 + properties: + max_speed: 0.8 + # Unidirectional: HN3 -> RowC0 (enter row C from north) + - action: navigate_to_pose + edge_id: HN3_RowC0 + node: RowC0 + properties: + max_speed: 0.5 + # Unidirectional: HN3 -> WP_E + - action: navigate_to_pose + edge_id: HN3_WP_E + node: WP_E + properties: + max_speed: 0.8 + name: HN3 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 15.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: headland_junction + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + # ========================= Row A (x=5) ==================================== + - meta: + map: complex_nav_test + node: RowA0 + pointset: complex_nav_test + tag: ['row_entry'] + node: + edges: + # Bidirectional: RowA0 -> HN1 (return to headland) + - action: navigate_to_pose + edge_id: RowA0_HN1 + node: HN1 + properties: + max_speed: 0.5 + # Unidirectional: RowA0 -> RowA1 (row traversal south) + - action: row_traversal + edge_id: RowA0_RowA1 + node: RowA1 + properties: + boundary_left: 0.4 + boundary_right: 0.4 + max_speed: 0.3 + row_type: vineyard + name: RowA0 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 5.0 + y: -3.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.05 + semantics: row_entry + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: complex_nav_test + node: RowA1 + pointset: complex_nav_test + tag: ['row_mid'] + node: + edges: + # Unidirectional: RowA1 -> RowA2 (continue row traversal) + - action: row_traversal + edge_id: RowA1_RowA2 + node: RowA2 + properties: + boundary_left: 0.4 + boundary_right: 0.4 + max_speed: 0.3 + row_type: vineyard + name: RowA1 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 5.0 + y: -8.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.1 + semantics: row_mid + verts: + - x: -0.4 + y: -0.4 + - x: 0.4 + y: -0.4 + - x: 0.4 + y: 0.4 + - x: -0.4 + y: 0.4 + + - meta: + map: complex_nav_test + node: RowA2 + pointset: complex_nav_test + tag: ['row_mid'] + node: + edges: + # Unidirectional: RowA2 -> RowA3 (continue row traversal) + - action: row_traversal + edge_id: RowA2_RowA3 + node: RowA3 + properties: + boundary_left: 0.4 + boundary_right: 0.4 + max_speed: 0.3 + row_type: vineyard + name: RowA2 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 5.0 + y: -13.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.1 + semantics: row_mid + verts: + - x: -0.4 + y: -0.4 + - x: 0.4 + y: -0.4 + - x: 0.4 + y: 0.4 + - x: -0.4 + y: 0.4 + + - meta: + map: complex_nav_test + node: RowA3 + pointset: complex_nav_test + tag: ['row_exit'] + node: + edges: + # Unidirectional: RowA3 -> HS1 (goal_align exit to south headland) + - action: goal_align + edge_id: RowA3_HS1 + node: HS1 + name: RowA3 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 5.0 + y: -18.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + semantics: row_exit + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + # ========================= Row B (x=10) =================================== + - meta: + map: complex_nav_test + node: RowB0 + pointset: complex_nav_test + tag: ['row_entry'] + node: + edges: + # Bidirectional: RowB0 -> HN2 + - action: navigate_to_pose + edge_id: RowB0_HN2 + node: HN2 + properties: + max_speed: 0.5 + # Unidirectional: RowB0 -> RowB1 + - action: row_traversal + edge_id: RowB0_RowB1 + node: RowB1 + properties: + boundary_left: 0.5 + boundary_right: 0.5 + max_speed: 0.25 + row_type: orchard + roboflow: + enabled: true + confidence: 0.7 + name: RowB0 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 10.0 + y: -3.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.05 + semantics: row_entry + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: complex_nav_test + node: RowB1 + pointset: complex_nav_test + tag: ['row_mid'] + node: + edges: + - action: row_traversal + edge_id: RowB1_RowB2 + node: RowB2 + properties: + boundary_left: 0.5 + boundary_right: 0.5 + max_speed: 0.25 + row_type: orchard + roboflow: + enabled: true + confidence: 0.7 + name: RowB1 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 10.0 + y: -8.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.1 + semantics: row_mid + verts: + - x: -0.4 + y: -0.4 + - x: 0.4 + y: -0.4 + - x: 0.4 + y: 0.4 + - x: -0.4 + y: 0.4 + + - meta: + map: complex_nav_test + node: RowB2 + pointset: complex_nav_test + tag: ['row_mid'] + node: + edges: + - action: row_traversal + edge_id: RowB2_RowB3 + node: RowB3 + properties: + boundary_left: 0.5 + boundary_right: 0.5 + max_speed: 0.25 + row_type: orchard + name: RowB2 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 10.0 + y: -13.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.1 + semantics: row_mid + verts: + - x: -0.4 + y: -0.4 + - x: 0.4 + y: -0.4 + - x: 0.4 + y: 0.4 + - x: -0.4 + y: 0.4 + + - meta: + map: complex_nav_test + node: RowB3 + pointset: complex_nav_test + tag: ['row_exit'] + node: + edges: + - action: goal_align + edge_id: RowB3_HS2 + node: HS2 + name: RowB3 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 10.0 + y: -18.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + semantics: row_exit + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + # ========================= Row C (x=15) =================================== + - meta: + map: complex_nav_test + node: RowC0 + pointset: complex_nav_test + tag: ['row_entry'] + node: + edges: + # Bidirectional: RowC0 -> HN3 + - action: navigate_to_pose + edge_id: RowC0_HN3 + node: HN3 + properties: + max_speed: 0.5 + # Unidirectional: RowC0 -> RowC1 + - action: row_traversal + edge_id: RowC0_RowC1 + node: RowC1 + properties: + boundary_left: 0.6 + boundary_right: 0.6 + max_speed: 0.2 + row_type: crop_row + name: RowC0 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 15.0 + y: -3.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.05 + semantics: row_entry + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: complex_nav_test + node: RowC1 + pointset: complex_nav_test + tag: ['row_mid'] + node: + edges: + - action: row_traversal + edge_id: RowC1_RowC2 + node: RowC2 + properties: + boundary_left: 0.6 + boundary_right: 0.6 + max_speed: 0.2 + row_type: crop_row + name: RowC1 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 15.0 + y: -8.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.1 + semantics: row_mid + verts: + - x: -0.4 + y: -0.4 + - x: 0.4 + y: -0.4 + - x: 0.4 + y: 0.4 + - x: -0.4 + y: 0.4 + + - meta: + map: complex_nav_test + node: RowC2 + pointset: complex_nav_test + tag: ['row_mid'] + node: + edges: + - action: row_traversal + edge_id: RowC2_RowC3 + node: RowC3 + properties: + boundary_left: 0.6 + boundary_right: 0.6 + max_speed: 0.2 + row_type: crop_row + name: RowC2 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 15.0 + y: -13.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.1 + semantics: row_mid + verts: + - x: -0.4 + y: -0.4 + - x: 0.4 + y: -0.4 + - x: 0.4 + y: 0.4 + - x: -0.4 + y: 0.4 + + - meta: + map: complex_nav_test + node: RowC3 + pointset: complex_nav_test + tag: ['row_exit'] + node: + edges: + - action: goal_align + edge_id: RowC3_HS3 + node: HS3 + name: RowC3 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 15.0 + y: -18.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + semantics: row_exit + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + # ==================== Headland South ======================================= + - meta: + map: complex_nav_test + node: HS1 + pointset: complex_nav_test + tag: ['headland', 'junction'] + node: + edges: + # Bidirectional: HS1 <-> HS2 (composable navigate_to_poses) + - action: navigate_to_poses + edge_id: HS1_HS2 + node: HS2 + properties: + max_speed: 0.8 + # Unidirectional: HS1 -> Charging (return home shortcut) + - action: navigate_to_pose + edge_id: HS1_Charging + node: Charging + properties: + max_speed: 1.0 + name: HS1 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 5.0 + y: -20.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: headland_junction + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: complex_nav_test + node: HS2 + pointset: complex_nav_test + tag: ['headland', 'junction'] + node: + edges: + # Bidirectional: HS2 <-> HS1 + - action: navigate_to_poses + edge_id: HS2_HS1 + node: HS1 + properties: + max_speed: 0.8 + # Bidirectional: HS2 <-> HS3 + - action: navigate_to_poses + edge_id: HS2_HS3 + node: HS3 + properties: + max_speed: 0.8 + name: HS2 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 10.0 + y: -20.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: headland_junction + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: complex_nav_test + node: HS3 + pointset: complex_nav_test + tag: ['headland', 'junction'] + node: + edges: + # Bidirectional: HS3 <-> HS2 + - action: navigate_to_poses + edge_id: HS3_HS2 + node: HS2 + properties: + max_speed: 0.8 + # Unidirectional: HS3 -> WP_SE + - action: navigate_to_pose + edge_id: HS3_WP_SE + node: WP_SE + properties: + max_speed: 0.8 + name: HS3 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 15.0 + y: -20.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: headland_junction + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + # ==================== Eastern Waypoints ==================================== + - meta: + map: complex_nav_test + node: WP_E + pointset: complex_nav_test + tag: ['waypoint'] + node: + edges: + # Bidirectional: WP_E <-> WP_SE + - action: navigate_to_pose + edge_id: WP_E_WP_SE + node: WP_SE + properties: + max_speed: 0.8 + # Unidirectional: WP_E -> AlignDock (precision docking) + - action: goal_align + edge_id: WP_E_AlignDock + node: AlignDock + name: WP_E + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 20.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: waypoint + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: complex_nav_test + node: WP_SE + pointset: complex_nav_test + tag: ['waypoint'] + node: + edges: + # Bidirectional: WP_SE <-> WP_E + - action: navigate_to_pose + edge_id: WP_SE_WP_E + node: WP_E + properties: + max_speed: 0.8 + # Unidirectional: WP_SE -> Charging (long return path) + - action: navigate_to_pose + edge_id: WP_SE_Charging + node: Charging + properties: + max_speed: 1.0 + name: WP_SE + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 20.0 + y: -20.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: waypoint + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + # ========================= AlignDock (dead-end) ============================ + - meta: + map: complex_nav_test + node: AlignDock + pointset: complex_nav_test + tag: ['docking_station'] + node: + edges: + # Unidirectional: AlignDock -> WP_E (undock and leave) + - action: navigate_to_pose + edge_id: AlignDock_WP_E + node: WP_E + properties: + max_speed: 0.3 + name: AlignDock + nav_frame: map # override: use 'map' frame instead of topo_frame_id + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: -0.707 + position: + x: 20.0 + y: -10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.02 + yaw_goal_tolerance: 0.01 + semantics: docking_station + dock_type: precision + verts: + - x: -0.3 + y: -0.3 + - x: 0.3 + y: -0.3 + - x: 0.3 + y: 0.3 + - x: -0.3 + y: 0.3 + + # ========================= Isolated node (disconnected) ==================== + - meta: + map: complex_nav_test + node: Isolated + pointset: complex_nav_test + tag: ['isolated', 'test'] + node: + edges: [] + name: Isolated + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 30.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 6.28 + semantics: isolated + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + # ========================= No-Go Zone (restricted) ======================== + - meta: + map: complex_nav_test + node: NoGo + pointset: complex_nav_test + tag: ['no_go'] + node: + edges: [] + name: NoGo + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 25.0 + y: -10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 6.28 + no_go: true + semantics: restricted_area + verts: + - x: -2.0 + y: -2.0 + - x: 2.0 + y: -2.0 + - x: 2.0 + y: 2.0 + - x: -2.0 + y: 2.0 + + # ========================= Topic Localisation Node ========================= + - meta: + map: complex_nav_test + node: TopicNode + pointset: complex_nav_test + tag: ['topic_localise'] + node: + edges: + # Unidirectional: TopicNode -> Charging + - action: navigate_to_pose + edge_id: TopicNode_Charging + node: Charging + properties: + max_speed: 0.5 + localise_by_topic: '{"topic": "/special_zone", "field": "data", "val": "active", "localise_anywhere": true, "persistency": 5}' + name: TopicNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: -5.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.5 + yaw_goal_tolerance: 6.28 + semantics: special_zone + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 diff --git a/topological_navigation/config/test_simple_tmap2.yaml b/topological_navigation/config/test_simple_tmap2.yaml new file mode 100644 index 00000000..9bd14a4e --- /dev/null +++ b/topological_navigation/config/test_simple_tmap2.yaml @@ -0,0 +1,266 @@ +meta: + last_updated: 08-03-2026_12-00-00 + origin: + latitude: 53.268642038 + longitude: -0.524509505881 + altitude: 0 + +metric_map: test_map +name: test_map +pointset: test_map + +transformation: + topo_frame_id: test_map + parent: map + rotation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + translation: + x: 0.0 + y: 0.0 + z: 0.0 + +definitions: + default_bt: | + + + + + + + + + goal_align_bt: | + + + + + + + + + row_traversal_bt: | + + + + + + + + + +actions: + navigate_to_pose: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: # Nested to match geometry_msgs/PoseStamped + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + + goal_align: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: # Nested to match geometry_msgs/PoseStamped + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.goal_align_bt}' + + row_traversal: + composable: true + action_type: nav2_msgs.action.NavigateThroughPoses + action_server: /navigate_through_poses + action_goal_template: + poses: # Needs to be an array of PoseStamped objects + - header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.row_traversal_bt}' + +nodes: + - meta: + map: test_map + node: node0 + pointset: test_map + node: + edges: + - action: row_traversal + edge_id: node0_node1 + node: node1 + properties: + boundary_left: 0.4 + boundary_right: 0.6 + name: node0 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 6.28 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: test_map + node: node1 + pointset: test_map + node: + edges: + - action: row_traversal + edge_id: node1_node2 + node: node2 + properties: + boundary_left: 0.4 + boundary_right: 0.6 + name: node1 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 2.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 6.28 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: test_map + node: node2 + pointset: test_map + node: + edges: + - action: row_traversal + edge_id: node2_node3 + node: node3 + properties: + boundary_left: 0.4 + boundary_right: 0.6 + name: node2 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 4.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 6.28 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: test_map + node: node3 + pointset: test_map + node: + edges: + - action: row_traversal + edge_id: node3_node4 + node: node4 + properties: + boundary_left: 0.4 + boundary_right: 0.6 + name: node3 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 6.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 6.28 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: test_map + node: node4 + pointset: test_map + node: + edges: [] + name: node4 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 8.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 6.28 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 diff --git a/topological_navigation/config/tmap-schema.yaml b/topological_navigation/config/tmap-schema.yaml index 412a262e..d303dd89 100644 --- a/topological_navigation/config/tmap-schema.yaml +++ b/topological_navigation/config/tmap-schema.yaml @@ -14,7 +14,7 @@ properties: properties: last_updated: type: string - pattern: ^[0-9]{4}-[0-9]{2}-[0-9]{2}_[0-9]{2}-[0-9]{2}-[0-9]{2}$ + pattern: ^[0-9]{2}-[0-9]{2}-[0-9]{4}_[0-9]{2}-[0-9]{2}-[0-9]{2}$ required: - last_updated additionalProperties: true @@ -48,7 +48,7 @@ properties: required: - edges - name - - parent_frame + # - parent_frame additionalProperties: true properties: edges: diff --git a/topological_navigation/cyclonedds.log b/topological_navigation/cyclonedds.log deleted file mode 100644 index ffdadc87..00000000 --- a/topological_navigation/cyclonedds.log +++ /dev/null @@ -1,19889 +0,0 @@ -1711462090.567383 [0] python3: config: Domain/General/MulticastRecvNetworkInterfaceAddresses/#text: preferred {} -1711462090.567392 [0] python3: config: Domain/General/Interfaces/NetworkInterface[@autodetermine]: false {} -1711462090.567395 [0] python3: config: Domain/General/Interfaces/NetworkInterface[@name]: wlp0s20f3 {} -1711462090.567397 [0] python3: config: Domain/General/Interfaces/NetworkInterface[@address]: (null) {} -1711462090.567399 [0] python3: config: Domain/General/Interfaces/NetworkInterface[@priority]: default {} -1711462090.567401 [0] python3: config: Domain/General/Interfaces/NetworkInterface[@prefer_multicast]: false {} -1711462090.567403 [0] python3: config: Domain/General/Interfaces/NetworkInterface[@presence_required]: true {} -1711462090.567405 [0] python3: config: Domain/General/Interfaces/NetworkInterface[@multicast]: default {} -1711462090.567407 [0] python3: config: Domain/General/ExternalNetworkAddress/#text: auto {} -1711462090.567409 [0] python3: config: Domain/General/ExternalNetworkMask/#text: 0.0.0.0 {} -1711462090.567412 [0] python3: config: Domain/General/AllowMulticast/#text: true {1} -1711462090.567414 [0] python3: config: Domain/General/MulticastTimeToLive/#text: 32 {} -1711462090.567416 [0] python3: config: Domain/General/DontRoute/#text: false {} -1711462090.567418 [0] python3: config: Domain/General/Transport/#text: udp {1} -1711462090.567420 [0] python3: config: Domain/General/EnableMulticastLoopback/#text: true {} -1711462090.567422 [0] python3: config: Domain/General/MaxMessageSize/#text: 10 MiB {1} -1711462090.567424 [0] python3: config: Domain/General/MaxRexmitMessageSize/#text: 1456 B {} -1711462090.567427 [0] python3: config: Domain/General/FragmentSize/#text: 20500 B {1} -1711462090.567428 [0] python3: config: Domain/General/RedundantNetworking/#text: false {} -1711462090.567430 [0] python3: config: Domain/General/EntityAutoNaming/#text: empty {} -1711462090.567433 [0] python3: config: Domain/General/EntityAutoNaming[@seed]: 3774755673 3481944393 1593628943 1496008116 2812686077 1486759785 1135171669 2415018896 {} -1711462090.567436 [0] python3: config: Domain/Sizing/ReceiveBufferSize/#text: 1 MiB {} -1711462090.567438 [0] python3: config: Domain/Sizing/ReceiveBufferChunkSize/#text: 128 KiB {} -1711462090.567440 [0] python3: config: Domain/Compatibility/StandardsConformance/#text: lax {} -1711462090.567442 [0] python3: config: Domain/Compatibility/ExplicitlyPublishQosSetToDefault/#text: false {} -1711462090.567444 [0] python3: config: Domain/Compatibility/ManySocketsMode/#text: single {} -1711462090.567446 [0] python3: config: Domain/Compatibility/AssumeRtiHasPmdEndpoints/#text: false {} -1711462090.567448 [0] python3: config: Domain/Discovery/Tag/#text: {} -1711462090.567450 [0] python3: config: Domain/Discovery/ExternalDomainId/#text: 0 {} -1711462090.567452 [0] python3: config: Domain/Discovery/DSGracePeriod/#text: 30 s {} -1711462090.567455 [0] python3: config: Domain/Discovery/Peers/Peer[@Address]: localhost {1} -1711462090.567457 [0] python3: config: Domain/Discovery/ParticipantIndex/#text: auto {1} -1711462090.567459 [0] python3: config: Domain/Discovery/MaxAutoParticipantIndex/#text: 100 {1} -1711462090.567461 [0] python3: config: Domain/Discovery/SPDPMulticastAddress/#text: 239.255.0.1 {} -1711462090.567463 [0] python3: config: Domain/Discovery/SPDPInterval/#text: 30 s {} -1711462090.567465 [0] python3: config: Domain/Discovery/DefaultMulticastAddress/#text: auto {} -1711462090.567467 [0] python3: config: Domain/Discovery/Ports/Base/#text: 7400 {} -1711462090.567469 [0] python3: config: Domain/Discovery/Ports/DomainGain/#text: 250 {} -1711462090.567471 [0] python3: config: Domain/Discovery/Ports/ParticipantGain/#text: 2 {} -1711462090.567473 [0] python3: config: Domain/Discovery/Ports/MulticastMetaOffset/#text: 0 {} -1711462090.567475 [0] python3: config: Domain/Discovery/Ports/UnicastMetaOffset/#text: 10 {} -1711462090.567477 [0] python3: config: Domain/Discovery/Ports/MulticastDataOffset/#text: 1 {} -1711462090.567482 [0] python3: config: Domain/Discovery/Ports/UnicastDataOffset/#text: 11 {} -1711462090.567484 [0] python3: config: Domain/Discovery/EnableTopicDiscoveryEndpoints/#text: false {} -1711462090.567486 [0] python3: config: Domain/Discovery/LeaseDuration/#text: 10 s {} -1711462090.567490 [0] python3: config: Domain/Tracing/Category/#text: trace {1} -1711462090.567492 [0] python3: config: Domain/Tracing/OutputFile/#text: cyclonedds.log {1} -1711462090.567494 [0] python3: config: Domain/Tracing/AppendToFile/#text: false {} -1711462090.567496 [0] python3: config: Domain/Tracing/PacketCaptureFile/#text: {} -1711462090.567498 [0] python3: config: Domain/Internal/DeliveryQueueMaxSamples/#text: 256 {} -1711462090.567500 [0] python3: config: Domain/Internal/PrimaryReorderMaxSamples/#text: 128 {} -1711462090.567502 [0] python3: config: Domain/Internal/SecondaryReorderMaxSamples/#text: 128 {} -1711462090.567504 [0] python3: config: Domain/Internal/DefragUnreliableMaxSamples/#text: 4 {} -1711462090.567505 [0] python3: config: Domain/Internal/DefragReliableMaxSamples/#text: 16 {} -1711462090.567507 [0] python3: config: Domain/Internal/BuiltinEndpointSet/#text: writers {} -1711462090.567509 [0] python3: config: Domain/Internal/MeasureHbToAckLatency/#text: false {} -1711462090.567511 [0] python3: config: Domain/Internal/UnicastResponseToSPDPMessages/#text: true {} -1711462090.567520 [0] python3: config: Domain/Internal/SynchronousDeliveryPriorityThreshold/#text: 0 {} -1711462090.567522 [0] python3: config: Domain/Internal/SynchronousDeliveryLatencyBound/#text: inf {} -1711462090.567524 [0] python3: config: Domain/Internal/MaxParticipants/#text: 0 {} -1711462090.567526 [0] python3: config: Domain/Internal/AccelerateRexmitBlockSize/#text: 0 {} -1711462090.567529 [0] python3: config: Domain/Internal/RetransmitMerging/#text: never {} -1711462090.567531 [0] python3: config: Domain/Internal/RetransmitMergingPeriod/#text: 5 ms {} -1711462090.567533 [0] python3: config: Domain/Internal/HeartbeatInterval/#text: 100 ms {} -1711462090.567535 [0] python3: config: Domain/Internal/HeartbeatInterval[@min]: 5 ms {} -1711462090.567537 [0] python3: config: Domain/Internal/HeartbeatInterval[@minsched]: 20 ms {} -1711462090.567538 [0] python3: config: Domain/Internal/HeartbeatInterval[@max]: 8 s {} -1711462090.567540 [0] python3: config: Domain/Internal/MaxQueuedRexmitBytes/#text: 512 KiB {} -1711462090.567542 [0] python3: config: Domain/Internal/MaxQueuedRexmitMessages/#text: 200 {} -1711462090.567544 [0] python3: config: Domain/Internal/WriterLingerDuration/#text: 1 s {} -1711462090.567546 [0] python3: config: Domain/Internal/SocketReceiveBufferSize[@min]: 30 MiB {1} -1711462090.567547 [0] python3: config: Domain/Internal/SocketReceiveBufferSize[@max]: default {} -1711462090.567549 [0] python3: config: Domain/Internal/SocketSendBufferSize[@min]: 64 KiB {} -1711462090.567551 [0] python3: config: Domain/Internal/SocketSendBufferSize[@max]: default {} -1711462090.567552 [0] python3: config: Domain/Internal/NackDelay/#text: 100 ms {} -1711462090.567554 [0] python3: config: Domain/Internal/AckDelay/#text: 10 ms {} -1711462090.567557 [0] python3: config: Domain/Internal/AutoReschedNackDelay/#text: 3 s {} -1711462090.567559 [0] python3: config: Domain/Internal/PreEmptiveAckDelay/#text: 10 ms {} -1711462090.567561 [0] python3: config: Domain/Internal/ScheduleTimeRounding/#text: 0 s {} -1711462090.567562 [0] python3: config: Domain/Internal/DDSI2DirectMaxThreads/#text: 1 {} -1711462090.567564 [0] python3: config: Domain/Internal/SquashParticipants/#text: false {} -1711462090.567566 [0] python3: config: Domain/Internal/SPDPResponseMaxDelay/#text: 0 s {} -1711462090.567568 [0] python3: config: Domain/Internal/LateAckMode/#text: false {} -1711462090.567570 [0] python3: config: Domain/Internal/RetryOnRejectBestEffort/#text: false {} -1711462090.567572 [0] python3: config: Domain/Internal/GenerateKeyhash/#text: false {} -1711462090.567573 [0] python3: config: Domain/Internal/MaxSampleSize/#text: 2147483647 B {} -1711462090.567577 [0] python3: config: Domain/Internal/LivelinessMonitoring/#text: false {} -1711462090.567578 [0] python3: config: Domain/Internal/LivelinessMonitoring[@StackTraces]: true {} -1711462090.567580 [0] python3: config: Domain/Internal/LivelinessMonitoring[@Interval]: 1 s {} -1711462090.567582 [0] python3: config: Domain/Internal/MonitorPort/#text: -1 {} -1711462090.567583 [0] python3: config: Domain/Internal/PrioritizeRetransmit/#text: true {} -1711462090.567585 [0] python3: config: Domain/Internal/UseMulticastIfMreqn/#text: 0 {} -1711462090.567587 [0] python3: config: Domain/Internal/RediscoveryBlacklistDuration/#text: 0 s {} -1711462090.567588 [0] python3: config: Domain/Internal/RediscoveryBlacklistDuration[@enforce]: false {} -1711462090.567590 [0] python3: config: Domain/Internal/MultipleReceiveThreads/#text: default {} -1711462090.567592 [0] python3: config: Domain/Internal/MultipleReceiveThreads[@maxretries]: 4294967295 {} -1711462090.567594 [0] python3: config: Domain/Internal/Test/XmitLossiness/#text: 0 {} -1711462090.567596 [0] python3: config: Domain/Internal/Watermarks/WhcLow/#text: 1 KiB {} -1711462090.567598 [0] python3: config: Domain/Internal/Watermarks/WhcHigh/#text: 500 KiB {1} -1711462090.567599 [0] python3: config: Domain/Internal/Watermarks/WhcHighInit/#text: 30 KiB {} -1711462090.567601 [0] python3: config: Domain/Internal/Watermarks/WhcAdaptive/#text: true {} -1711462090.567603 [0] python3: config: Domain/Internal/BurstSize/MaxRexmit/#text: 1 MiB {} -1711462090.567605 [0] python3: config: Domain/Internal/BurstSize/MaxInitTransmit/#text: 4294967295 {} -1711462090.567608 [0] python3: config: Domain/Internal/EnableExpensiveChecks/#text: {} -1711462090.567610 [0] python3: config: Domain/TCP/NoDelay/#text: true {} -1711462090.567612 [0] python3: config: Domain/TCP/Port/#text: -1 {} -1711462090.567614 [0] python3: config: Domain/TCP/ReadTimeout/#text: 2 s {} -1711462090.567616 [0] python3: config: Domain/TCP/WriteTimeout/#text: 2 s {} -1711462090.567617 [0] python3: config: Domain/TCP/AlwaysUsePeeraddrForUnicast/#text: false {} -1711462090.567620 [0] python3: config: Domain/SSL/Enable/#text: false {} -1711462090.567623 [0] python3: config: Domain/SSL/CertificateVerification/#text: true {} -1711462090.567624 [0] python3: config: Domain/SSL/VerifyClient/#text: true {} -1711462090.567626 [0] python3: config: Domain/SSL/SelfSignedCertificates/#text: false {} -1711462090.567629 [0] python3: config: Domain/SSL/KeystoreFile/#text: keystore {} -1711462090.567630 [0] python3: config: Domain/SSL/KeyPassphrase/#text: secret {} -1711462090.567632 [0] python3: config: Domain/SSL/Ciphers/#text: ALL:!ADH:!LOW:!EXP:!MD5:@STRENGTH {} -1711462090.567634 [0] python3: config: Domain/SSL/EntropyFile/#text: {} -1711462090.567636 [0] python3: config: Domain/SSL/MinimumTLSVersion/#text: 1.3 {} -1711462090.567638 [0] python3: config: Domain/SharedMemory/Enable/#text: false {} -1711462090.567640 [0] python3: config: Domain/SharedMemory/Locator/#text: {} -1711462090.567643 [0] python3: config: Domain/SharedMemory/Prefix/#text: DDS_CYCLONE {} -1711462090.567645 [0] python3: config: Domain/SharedMemory/LogLevel/#text: info {} -1711462090.567647 [0] python3: config: Domain[@Id]: 0 {1} -1711462090.567660 [0] python3: started at 1711462090.06567651 -- 2024-03-26 14:08:10+00:00 -1711462090.567672 [0] python3: udp initialized -1711462090.569666 [0] python3: interfaces: lo udp/127.0.0.1(q1) wlp0s20f3 wireless udp/10.101.12.71(q9) br-d76075d5ea89 udp/172.18.0.1(q9) br-56769bca490c udp/172.20.0.1(q9) docker0 udp/172.17.0.1(q9) br-9ff433288a2a udp/172.19.0.1(q9) -1711462090.569671 [0] python3: selected interfaces: wlp0s20f3 (index 2 priority 0) -1711462090.569683 [0] python3: ownip: udp/10.101.12.71 -1711462090.569684 [0] python3: extmask: invalid/0 (not applicable) -1711462090.569685 [0] python3: SPDP MC: udp/239.255.0.1 -1711462090.569687 [0] python3: default MC: udp/239.255.0.1 -1711462090.569690 [0] python3: SSM support included -1711462090.569756 [0] python3: rtps_init: trying to find a free participant index -1711462090.569779 [0] python3: ddsi_udp_create_conn unicast socket 5 port 7418 -1711462090.569782 [0] python3: ddsi_udp_create_conn unicast socket 6 port 7419 -1711462090.569784 [0] python3: rtps_init: uc ports: disc 7418 data 7419 -1711462090.569785 [0] python3: rtps_init: domainid 0 participantid 5 -1711462090.569787 [0] python3: Unicast Ports: discovery 7418 data 7419 -1711462090.569793 [0] python3: ddsi_udp_create_conn multicast socket 7 port 7400 -1711462090.569797 [0] python3: ddsi_udp_create_conn multicast socket 8 port 7401 -1711462090.569798 [0] python3: Multicast Ports: discovery 7400 data 7401 -1711462090.569804 [0] python3: ddsi_udp_create_conn transmit(uc/mc) socket 9 port 52025 -1711462090.569807 [0] python3: interface wlp0s20f3: transmit port 52025 -1711462090.569813 [0] python3: join conn 0x653a1b46f210 for (udp/239.255.0.1, *) interface udp/10.101.12.71 -1711462090.569816 [0] python3: join conn 0x653a1b466f10 for (udp/239.255.0.1, *) interface udp/10.101.12.71 -1711462090.569819 [0] python3: join conn 0x653a1b46f210 for (udp/239.255.0.1, *) interface (default): already joined -1711462090.569820 [0] python3: join conn 0x653a1b466f10 for (udp/239.255.0.1, *) interface (default): already joined -1711462090.569913 [0] python3: add_peer_addresses: add udp/127.0.0.1:7410, :7412, :7414, :7416, :7418, :7420, :7422, :7424, :7426, :7428, :7430, :7432, :7434, :7436, :7438, :7440, :7442, :7444, :7446, :7448, :7450, :7452, :7454, :7456, :7458, :7460, :7462, :7464, :7466, :7468, :7470, :7472, :7474, :7476, :7478, :7480, :7482, :7484, :7486, :7488, :7490, :7492, :7494, :7496, :7498, :7500, :7502, :7504, :7506, :7508, :7510, :7512, :7514, :7516, :7518, :7520, :7522, :7524, :7526, :7528, :7530, :7532, :7534, :7536, :7538, :7540, :7542, :7544, :7546, :7548, :7550, :7552, :7554, :7556, :7558, :7560, :7562, :7564, :7566, :7568, :7570, :7572, :7574, :7576, :7578, :7580, :7582, :7584, :7586, :7588, :7590, :7592, :7594, :7596, :7598, :7600, :7602, :7604, :7606, :7608 -1711462090.569925 [0] python3: ddsi_new_local_orphan_writer(DCPSParticipant/org::eclipse::cyclonedds::builtin::DCPSParticipant) -1711462090.569942 [0] python3: WRITER 0:0:0:100c2 QOS={user_data=0<>,topic_name="DCPSParticipant",type_name="org::eclipse::cyclonedds::builtin::DCPSParticipant",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=1:0:0,partition={"__BUILT-IN PARTITION__"},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.569947 [0] python3: match_writer_with_readers(wr 0:0:0:100c2) scanning all rds of topic DCPSParticipant -1711462090.569948 [0] python3: ddsi_new_local_orphan_writer(DCPSTopic/org::eclipse::cyclonedds::builtin::DCPSTopic) -1711462090.569956 [0] python3: WRITER 0:0:0:2c2 QOS={user_data=0<>,topic_name="DCPSTopic",type_name="org::eclipse::cyclonedds::builtin::DCPSTopic",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=1:0:0,partition={"__BUILT-IN PARTITION__"},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.569959 [0] python3: match_writer_with_readers(wr 0:0:0:2c2) scanning all rds of topic DCPSTopic -1711462090.569960 [0] python3: ddsi_new_local_orphan_writer(DCPSPublication/org::eclipse::cyclonedds::builtin::DCPSPublication) -1711462090.569967 [0] python3: WRITER 0:0:0:3c2 QOS={user_data=0<>,topic_name="DCPSPublication",type_name="org::eclipse::cyclonedds::builtin::DCPSPublication",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=1:0:0,partition={"__BUILT-IN PARTITION__"},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.569970 [0] python3: match_writer_with_readers(wr 0:0:0:3c2) scanning all rds of topic DCPSPublication -1711462090.569972 [0] python3: ddsi_new_local_orphan_writer(DCPSSubscription/org::eclipse::cyclonedds::builtin::DCPSSubscription) -1711462090.569979 [0] python3: WRITER 0:0:0:4c2 QOS={user_data=0<>,topic_name="DCPSSubscription",type_name="org::eclipse::cyclonedds::builtin::DCPSSubscription",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=1:0:0,partition={"__BUILT-IN PARTITION__"},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.569981 [0] python3: match_writer_with_readers(wr 0:0:0:4c2) scanning all rds of topic DCPSSubscription -1711462090.569985 [0] python3: create_thread: gc: class 0 priority 0 stack 0 -1711462090.570018 [0] python3: create_thread: dq.builtins: class 0 priority 0 stack 0 -1711462090.570042 [0] python3: create_thread: dq.user: class 0 priority 0 stack 0 -1711462090.570054 [0] gc: started new thread 192809: gc -1711462090.570079 [0] dq.builtin: started new thread 192810: dq.builtins -1711462090.570080 [0] python3: create_thread: tev: class 0 priority 0 stack 0 -1711462090.570089 [0] dq.user: started new thread 192811: dq.user -1711462090.570104 [0] dq.builtin: thread_cputime 0.000000000 -1711462090.570109 [0] gc: thread_cputime 0.000000000 -1711462090.570111 [0] python3: create_thread: recv: class 0 priority 0 stack 0 -1711462090.570117 [0] tev: started new thread 192812: tev -1711462090.570112 [0] dq.user: thread_cputime 0.000000000 -1711462090.570136 [0] python3: create_thread: recvMC: class 0 priority 0 stack 0 -1711462090.570137 [0] tev: thread_cputime 0.000000000 -1711462090.570150 [0] recv: started new thread 192813: recv -1711462090.570161 [0] python3: create_thread: recvUC: class 0 priority 0 stack 0 -1711462090.570164 [0] recv: thread_cputime 0.000000000 -1711462090.570170 [0] recvMC: started new thread 192814: recvMC -1711462090.570179 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 332 from udp/10.101.12.71:44991 -1711462090.570185 [0] recvMC: thread_cputime 0.000000000 -1711462090.570190 [0] recvUC: started new thread 192815: recvUC -1711462090.570193 [0] recvUC: thread_cputime 0.000000000 -1711462090.570195 [0] python3: ddsi_new_participant(110aba1:7b19badd:ce0adb73:1c1, 0) -1711462090.570195 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 496 from udp/10.101.12.71:44991 -1711462090.570186 [0] recv: INFOTS(1711462090.569761244) -1711462090.570201 [0] recvMC: INFOTS(1711462090.569771719) -1711462090.570206 [0] gc: gc 0x653a1b47e890: not yet, shortsleep -1711462090.570207 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #9 110443d:bb7f10a5:18901533:403? -> 0:0:0:0) -1711462090.570206 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #7 110443d:bb7f10a5:18901533:4c2? -> 0:0:0:0) -1711462090.570207 [0] python3: PARTICIPANT 110aba1:7b19badd:ce0adb73:1c1 QOS={user_data=10<"enclave=/;">,liveliness=0:10000000000,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},adlink_entity_factory=0} -1711462090.570212 [0] recvMC: HEARTBEAT(F#14:9..9 110443d:bb7f10a5:18901533:403? -> 0:0:0:0) -1711462090.570220 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:100c2, (null).DCPSParticipant/ParticipantBuiltinTopicData) -1711462090.570225 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:100c2 @ 0x653a1b3e3734) user 1 builtin 1 -1711462090.570235 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:100c2 QOS={user_data=0<>,topic_name="DCPSParticipant",type_name="ParticipantBuiltinTopicData",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=0:0,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_reader_lifespan=0:9223372036854775807,adlink_writer_data_lifecycle=1,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570238 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:100c2) scanning proxy participants tgt=0 -1711462090.570241 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:4c2, (null).DCPSSubscription/SubscriptionBuiltinTopicData) -1711462090.570243 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:4c2 @ 0x653a1b3e3c14) user 1 builtin 2 -1711462090.570250 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:4c2 QOS={user_data=0<>,topic_name="DCPSSubscription",type_name="SubscriptionBuiltinTopicData",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.570252 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:4c2) scanning proxy participants tgt=4c7 -1711462090.570254 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:3c2, (null).DCPSPublication/PublicationBuiltinTopicData) -1711462090.570256 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:3c2 @ 0x653a1b3e4134) user 1 builtin 3 -1711462090.570262 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:3c2 QOS={user_data=0<>,topic_name="DCPSPublication",type_name="PublicationBuiltinTopicData",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.570264 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:3c2) scanning proxy participants tgt=3c7 -1711462090.570266 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:200c2, (null).DCPSParticipantMessage/ParticipantMessageData) -1711462090.570268 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:200c2 @ 0x653a1b3e4714) user 1 builtin 4 -1711462090.570274 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:200c2 QOS={user_data=0<>,topic_name="DCPSParticipantMessage",type_name="ParticipantMessageData",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.570276 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:200c2) scanning proxy participants tgt=200c7 -1711462090.570279 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:300c3, (null).DCPSTypeLookupRequest/DDS::Builtin::TypeLookup_Request) -1711462090.570281 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:300c3 @ 0x653a1b3e4ea4) user 1 builtin 5 -1711462090.570287 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:300c3 QOS={user_data=0<>,topic_name="DCPSTypeLookupRequest",type_name="DDS::Builtin::TypeLookup_Request",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=1:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.570289 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:300c3) scanning proxy participants tgt=300c4 -1711462090.570291 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:301c3, (null).DCPSTypeLookupReply/DDS::Builtin::TypeLookup_Reply) -1711462090.570293 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:301c3 @ 0x653a1b484114) user 1 builtin 6 -1711462090.570299 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:301c3 QOS={user_data=0<>,topic_name="DCPSTypeLookupReply",type_name="DDS::Builtin::TypeLookup_Reply",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=1:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_writer_data_lifecycle=1} -1711462090.570301 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:301c3) scanning proxy participants tgt=301c4 -1711462090.570303 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:100c7, (null).DCPSParticipant/ParticipantBuiltinTopicData) -1711462090.570305 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:100c7 @ 0x653a1af2dee4) user 1 builtin 7 -1711462090.570313 [0] python3: READER 110aba1:7b19badd:ce0adb73:100c7 QOS={user_data=0<>,topic_name="DCPSParticipant",type_name="ParticipantBuiltinTopicData",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=0:0,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_reader_lifespan=0:9223372036854775807,adlink_writer_data_lifecycle=1,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570315 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:100c7) scanning proxy participants tgt=0 -1711462090.570316 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:4c7, (null).DCPSSubscription/SubscriptionBuiltinTopicData) -1711462090.570318 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:4c7 @ 0x653a1af72b04) user 1 builtin 8 -1711462090.570325 [0] python3: READER 110aba1:7b19badd:ce0adb73:4c7 QOS={user_data=0<>,topic_name="DCPSSubscription",type_name="SubscriptionBuiltinTopicData",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570328 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:4c7) scanning proxy participants tgt=4c2 -1711462090.570329 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:3c7, (null).DCPSPublication/PublicationBuiltinTopicData) -1711462090.570331 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:3c7 @ 0x653a1af94554) user 1 builtin 9 -1711462090.570337 [0] python3: READER 110aba1:7b19badd:ce0adb73:3c7 QOS={user_data=0<>,topic_name="DCPSPublication",type_name="PublicationBuiltinTopicData",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570339 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:3c7) scanning proxy participants tgt=3c2 -1711462090.570341 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:200c7, (null).DCPSParticipantMessage/ParticipantMessageData) -1711462090.570343 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:200c7 @ 0x653a1af86bb4) user 1 builtin 10 -1711462090.570349 [0] python3: READER 110aba1:7b19badd:ce0adb73:200c7 QOS={user_data=0<>,topic_name="DCPSParticipantMessage",type_name="ParticipantMessageData",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570351 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:200c7) scanning proxy participants tgt=200c2 -1711462090.570353 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:300c4, (null).DCPSTypeLookupRequest/DDS::Builtin::TypeLookup_Request) -1711462090.570354 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:300c4 @ 0x653a1af22f84) user 1 builtin 11 -1711462090.570361 [0] python3: READER 110aba1:7b19badd:ce0adb73:300c4 QOS={user_data=0<>,topic_name="DCPSTypeLookupRequest",type_name="DDS::Builtin::TypeLookup_Request",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,destination_order=0,history=1:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570363 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:300c4) scanning proxy participants tgt=300c3 -1711462090.570364 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:301c4, (null).DCPSTypeLookupReply/DDS::Builtin::TypeLookup_Reply) -1711462090.570366 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:301c4 @ 0x653a1aeda794) user 1 builtin 12 -1711462090.570372 [0] python3: READER 110aba1:7b19badd:ce0adb73:301c4 QOS={user_data=0<>,topic_name="DCPSTypeLookupReply",type_name="DDS::Builtin::TypeLookup_Reply",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,destination_order=0,history=1:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570375 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:301c4) scanning proxy participants tgt=301c3 -1711462090.570379 [0] python3: => EVERYONE -1711462090.570381 [0] python3: spdp_write(110aba1:7b19badd:ce0adb73:1c1) -1711462090.570384 [0] python3: spdp_write(110aba1:7b19badd:ce0adb73:1c1) - internals: op-Alienware-x16-R1/0.10.4/Linux/Linux -1711462090.570406 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:100c2 #1: ST0 DCPSParticipant/ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.570410 [0] python3: non-timed queue now has 1 items -1711462090.570420 [0] tev: xpack_addmsg 0x770414001390 0x653a1aebe8d0 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462090.570426 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b2540c0 reg?yes refc 2 DCPSParticipant/org::eclipse::cyclonedds::builtin::DCPSParticipant) -1711462090.570427 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.570429 [0] python3: create_and_lock_ktopic: ktp 0x653a1b4966f0 -1711462090.570430 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b2540c0 -1711462090.570433 [0] python3: dds_create_topic_generic: new topic 865392310 -1711462090.570437 [0] tev: write_sample 110aba1:7b19badd:ce0adb73:200c2 #1: ST0 DCPSParticipantMessage/ParticipantMessageData:{110aba1:7b19badd:ce0adb73:1}:1<0> -1711462090.570439 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:107, __BUILT-IN PARTITION__.DCPSParticipant/org::eclipse::cyclonedds::builtin::DCPSParticipant) -1711462090.570445 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:107 @ 0x653a1b295fa4) user 2 builtin 12 -1711462090.570450 [0] tev: resched pmd(110aba1:7b19badd:ce0adb73:1c1): 8s -1711462090.570453 [0] python3: READER 110aba1:7b19badd:ce0adb73:107 QOS={user_data=0<>,topic_name="DCPSParticipant",type_name="org::eclipse::cyclonedds::builtin::DCPSParticipant",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=1:0:0,partition={"__BUILT-IN PARTITION__"},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570456 [0] python3: => EVERYONE -1711462090.570458 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:107) scanning all pwrs of topic DCPSParticipant -1711462090.570460 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:107) scanning all wrs of topic DCPSParticipant -1711462090.570464 [0] python3: reader_add_local_connection(wr 0:0:0:100c2 rd 110aba1:7b19badd:ce0adb73:107) -1711462090.570470 [0] python3: writer_add_local_connection(wr 0:0:0:100c2 rd 110aba1:7b19badd:ce0adb73:107) -1711462090.570474 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b2d50f0 reg?yes refc 2 DCPSSubscription/org::eclipse::cyclonedds::builtin::DCPSSubscription) -1711462090.570475 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.570476 [0] python3: create_and_lock_ktopic: ktp 0x653a1af95750 -1711462090.570478 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b2d50f0 -1711462090.570479 [0] python3: dds_create_topic_generic: new topic 531669467 -1711462090.570482 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462090.570485 [0] recv: INFOTS(1711462090.570393375) -1711462090.570483 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:207, __BUILT-IN PARTITION__.DCPSSubscription/org::eclipse::cyclonedds::builtin::DCPSSubscription) -1711462090.570494 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462090.570495 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:207 @ 0x653a1b3fe704) user 3 builtin 12 -1711462090.570504 [0] python3: READER 110aba1:7b19badd:ce0adb73:207 QOS={user_data=0<>,topic_name="DCPSSubscription",type_name="org::eclipse::cyclonedds::builtin::DCPSSubscription",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=1:0:0,partition={"__BUILT-IN PARTITION__"},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570507 [0] python3: => EVERYONE -1711462090.570509 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:207) scanning all pwrs of topic DCPSSubscription -1711462090.570512 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:207) scanning all wrs of topic DCPSSubscription -1711462090.570514 [0] python3: reader_add_local_connection(wr 0:0:0:4c2 rd 110aba1:7b19badd:ce0adb73:207) -1711462090.570524 [0] python3: writer_add_local_connection(wr 0:0:0:4c2 rd 110aba1:7b19badd:ce0adb73:207) -1711462090.570531 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b2d5150 reg?yes refc 2 DCPSPublication/org::eclipse::cyclonedds::builtin::DCPSPublication) -1711462090.570537 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.570539 [0] python3: create_and_lock_ktopic: ktp 0x653a1b2b1d50 -1711462090.570540 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b2d5150 -1711462090.570542 [0] python3: dds_create_topic_generic: new topic 859371661 -1711462090.570542 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462090.570546 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:307, __BUILT-IN PARTITION__.DCPSPublication/org::eclipse::cyclonedds::builtin::DCPSPublication) -1711462090.570550 [0] recv: INFOTS(1711462090.570393375) -1711462090.570552 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:307 @ 0x653a1b3ea574) user 4 builtin 12 -1711462090.570557 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462090.570564 [0] python3: READER 110aba1:7b19badd:ce0adb73:307 QOS={user_data=0<>,topic_name="DCPSPublication",type_name="org::eclipse::cyclonedds::builtin::DCPSPublication",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:100000000,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=1:0:0,partition={"__BUILT-IN PARTITION__"},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570567 [0] python3: => EVERYONE -1711462090.570568 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.570571 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:307) scanning all pwrs of topic DCPSPublication -1711462090.570575 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:307) scanning all wrs of topic DCPSPublication -1711462090.570575 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (local) -1711462090.570578 [0] python3: reader_add_local_connection(wr 0:0:0:3c2 rd 110aba1:7b19badd:ce0adb73:307) -1711462090.570582 [0] python3: writer_add_local_connection(wr 0:0:0:3c2 rd 110aba1:7b19badd:ce0adb73:307) -1711462090.570588 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.570590 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (local) -1711462090.570779 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b269990 reg?no refc 1 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_) -1711462090.570782 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.570785 [0] python3: create_and_lock_ktopic: ktp 0x653a1b3e5620 -1711462090.570789 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b269990 -1711462090.570792 [0] python3: dds_create_topic_generic: new topic 1225600693 -1711462090.570806 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:403, (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_) -1711462090.570810 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:403 @ 0x653a1b487c64) user 5 builtin 12 -1711462090.570821 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:403 QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.570826 [0] python3: => EVERYONE -1711462090.570830 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:403) scanning all prds of topic ros_discovery_info -1711462090.570835 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:403) scanning all rds of topic ros_discovery_info -1711462090.570845 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #1: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.570872 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1ae7deb0 reg?no refc 1 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_) -1711462090.570874 [0] python3: lookup_and_check_ktopic_may_unlock_pp: ktp 0x653a1b3e5620 reuse -1711462090.570877 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b269990 -1711462090.570881 [0] python3: dds_create_topic_generic: new topic 139754145 -1711462090.570887 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:504, (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_) -1711462090.570889 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:504 @ 0x653a1b2c9994) user 6 builtin 12 -1711462090.570898 [0] python3: READER 110aba1:7b19badd:ce0adb73:504 QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=1:-1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.570902 [0] python3: => EVERYONE -1711462090.570906 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:504) scanning all pwrs of topic ros_discovery_info -1711462090.570910 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:504) scanning all wrs of topic ros_discovery_info -1711462090.570919 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #1: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,history=1:-1,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:504},adlink_entity_factory=1} -1711462090.570970 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #1: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{},{}}}} -1711462090.570972 [0] python3: => EVERYONE -1711462090.571172 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 460 from udp/10.101.12.71:58212 -1711462090.571177 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.571180 [0] recv: INFOTS(1711462090.519442682) -1711462090.571186 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462090.571191 [0] tev: nn_xpack_send 444: 0x77041400139c:20 0x653a1b3ebde8:36 0x653a1b2b4514:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462090.571197 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.571202 [0] tev: traffic-xmit (101) 444 -1711462090.571213 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1ae7deb0 reg?no refc 1 rt/rosout/rcl_interfaces::msg::dds_::Log_) -1711462090.571214 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 bes fc3f NEW (0x00000000-0x0000002c-0x00000000-0x00000000-0x00000000 op-Alienware-x16-R1/0.10.4/Linux/Linux) (data udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 meta udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2) QOS={user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{}} -1711462090.571215 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.571222 [0] python3: create_and_lock_ktopic: ktp 0x653a1b46bca0 -1711462090.571219 [0] dq.builtin: lease_new(tdur 10000000000 guid 110e21c:d0762c48:a9f0fb28:1c1) @ 0x77041c001c10 -1711462090.571225 [0] python3: dds_create_topic_generic: register new sertype 0x653a1ae7deb0 -1711462090.571229 [0] dq.builtin: lease_new(tdur 10000000000 guid 110e21c:d0762c48:a9f0fb28:1c1) @ 0x77041c001c90 -1711462090.571232 [0] python3: dds_create_topic_generic: new topic 638628323 -1711462090.571239 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:100c7) scanning participants tgt=0 -1711462090.571241 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:603, (default).rt/rosout/rcl_interfaces::msg::dds_::Log_) -1711462090.571243 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:3c2) scanning participants tgt=3c7 -1711462090.571246 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:603 @ 0x653a1b2cb464) user 7 builtin 12 -1711462090.571252 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:3c7 init_acknack_count = 1 -1711462090.571257 [0] dq.builtin: reader_add_connection(pwr 110e21c:d0762c48:a9f0fb28:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.571261 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:603 QOS={user_data=0<>,topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1000:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=10000000000,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.571264 [0] gc: gc 0x653a1b47e890: deleting -1711462090.571262 [0] dq.builtin: proxy_writer_add_connection(pwr 110e21c:d0762c48:a9f0fb28:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) - out-of-sync -1711462090.571267 [0] python3: => EVERYONE -1711462090.571273 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:3c7) scanning participants tgt=3c2 -1711462090.571274 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:603) scanning all prds of topic rt/rosout -1711462090.571276 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110e21c:d0762c48:a9f0fb28:3c7) -1711462090.571280 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110e21c:d0762c48:a9f0fb28:3c7) - ack seq 1 -1711462090.571281 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:603) scanning all rds of topic rt/rosout -1711462090.571284 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 -1711462090.571289 [0] dq.builtin: reduced nlocs=2 -1711462090.571291 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571294 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571296 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462090.571299 [0] dq.builtin: a -1711462090.571302 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u } -1711462090.571304 [0] dq.builtin: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462090.571306 [0] dq.builtin: best = 0 -1711462090.571308 [0] dq.builtin: simple udp/10.101.12.71:7412@2 -1711462090.571312 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:3c2): udp/10.101.12.71:7412@2 (burst size 4294901760 rexmit 1048576) -1711462090.571316 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:4c2) scanning participants tgt=4c7 -1711462090.571317 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:4c7 init_acknack_count = 1 -1711462090.571319 [0] dq.builtin: reader_add_connection(pwr 110e21c:d0762c48:a9f0fb28:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.571319 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #2: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",durability=1,durability_service=0:0:1000:-1:-1:-1,reliability=1:9223372036854775807,lifespan=10000000000,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.571323 [0] dq.builtin: proxy_writer_add_connection(pwr 110e21c:d0762c48:a9f0fb28:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) - out-of-sync -1711462090.571329 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:4c7) scanning participants tgt=4c2 -1711462090.571332 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110e21c:d0762c48:a9f0fb28:4c7) -1711462090.571336 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110e21c:d0762c48:a9f0fb28:4c7) - ack seq 1 -1711462090.571329 [0] python3: non-timed queue now has 1 items -1711462090.571340 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 -1711462090.571345 [0] dq.builtin: reduced nlocs=2 -1711462090.571347 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571348 [0] tev: xpack_addmsg 0x770414001390 0x653a1b2cbe40 0(data(110aba1:7b19badd:ce0adb73:3c2:#2/1)): niov 0 sz 0 => now niov 3 sz 256 -1711462090.571349 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571355 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462090.571358 [0] dq.builtin: a -1711462090.571355 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #2: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}} -1711462090.571361 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u } -1711462090.571364 [0] tev: nn_xpack_send 256: 0x77041400139c:20 0x653a1af32538:36 0x653a1af8b024:200 [ udp/10.101.12.71:7412@2 ] -1711462090.571365 [0] python3: => EVERYONE -1711462090.571370 [0] tev: traffic-xmit (1) 256 -1711462090.571365 [0] dq.builtin: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462090.571381 [0] dq.builtin: best = 0 -1711462090.571383 [0] dq.builtin: simple udp/10.101.12.71:7412@2 -1711462090.571386 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:4c2): udp/10.101.12.71:7412@2 (burst size 4294901760 rexmit 1048576) -1711462090.571392 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:200c2) scanning participants tgt=200c7 -1711462090.571394 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:200c7 init_acknack_count = 1 -1711462090.571396 [0] dq.builtin: reader_add_connection(pwr 110e21c:d0762c48:a9f0fb28:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.571398 [0] dq.builtin: proxy_writer_add_connection(pwr 110e21c:d0762c48:a9f0fb28:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) - out-of-sync -1711462090.571401 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:200c7) scanning participants tgt=200c2 -1711462090.571405 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110e21c:d0762c48:a9f0fb28:200c7) -1711462090.571407 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110e21c:d0762c48:a9f0fb28:200c7) - ack seq 1 -1711462090.571411 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 -1711462090.571412 [0] dq.builtin: reduced nlocs=2 -1711462090.571414 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571417 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571418 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462090.571421 [0] dq.builtin: a -1711462090.571423 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u } -1711462090.571426 [0] dq.builtin: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462090.571428 [0] dq.builtin: best = 0 -1711462090.571429 [0] dq.builtin: simple udp/10.101.12.71:7412@2 -1711462090.571433 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:200c2): udp/10.101.12.71:7412@2 (burst size 4294901760 rexmit 1048576) -1711462090.571437 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:300c3) scanning participants tgt=300c4 -1711462090.571438 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:300c4 init_acknack_count = 1 -1711462090.571443 [0] dq.builtin: reader_add_connection(pwr 110e21c:d0762c48:a9f0fb28:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.571446 [0] dq.builtin: proxy_writer_add_connection(pwr 110e21c:d0762c48:a9f0fb28:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) - out-of-sync -1711462090.571454 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:300c4) scanning participants tgt=300c3 -1711462090.571458 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110e21c:d0762c48:a9f0fb28:300c4) -1711462090.571460 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110e21c:d0762c48:a9f0fb28:300c4) - ack seq 0 -1711462090.571455 [0] gc: gc 0x77041c005430: not yet, shortsleep -1711462090.571466 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 -1711462090.571468 [0] dq.builtin: reduced nlocs=2 -1711462090.571471 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571473 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571476 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462090.571477 [0] dq.builtin: a -1711462090.571479 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u } -1711462090.571482 [0] dq.builtin: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462090.571483 [0] dq.builtin: best = 0 -1711462090.571486 [0] dq.builtin: simple udp/10.101.12.71:7412@2 -1711462090.571488 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:300c3): udp/10.101.12.71:7412@2 (burst size 4294901760 rexmit 1048576) -1711462090.571492 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:301c3) scanning participants tgt=301c4 -1711462090.571494 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:301c4 init_acknack_count = 1 -1711462090.571496 [0] dq.builtin: reader_add_connection(pwr 110e21c:d0762c48:a9f0fb28:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.571498 [0] dq.builtin: proxy_writer_add_connection(pwr 110e21c:d0762c48:a9f0fb28:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) - out-of-sync -1711462090.571502 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:301c4) scanning participants tgt=301c3 -1711462090.571503 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110e21c:d0762c48:a9f0fb28:301c4) -1711462090.571506 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110e21c:d0762c48:a9f0fb28:301c4) - ack seq 0 -1711462090.571509 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 -1711462090.571511 [0] dq.builtin: reduced nlocs=2 -1711462090.571513 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571514 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571517 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462090.571518 [0] dq.builtin: a -1711462090.571521 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u } -1711462090.571523 [0] dq.builtin: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462090.571526 [0] dq.builtin: best = 0 -1711462090.571527 [0] dq.builtin: simple udp/10.101.12.71:7412@2 -1711462090.571529 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:301c3): udp/10.101.12.71:7412@2 (burst size 4294901760 rexmit 1048576) -1711462090.571538 [0] dq.builtin: => EVERYONE -1711462090.571544 [0] dq.builtin: lease_register(l 0x77041c001c90 guid 110e21c:d0762c48:a9f0fb28:1c1) -1711462090.571546 [0] dq.builtin: directed SPDP packet -> not responding -1711462090.571576 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 460 from udp/10.101.12.71:44991 -1711462090.571579 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.571584 [0] recv: INFOTS(1711462090.549308855) -1711462090.571589 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462090.571593 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 460 from udp/10.101.12.71:40473 -1711462090.571594 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.571596 [0] recv: INFOTS(1711462090.541682763) -1711462090.571597 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.571598 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462090.571606 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 bes fc3f NEW (0x00000000-0x0000002c-0x00000000-0x00000000-0x00000000 op-Alienware-x16-R1/0.10.4/Linux/Linux) (data udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 meta udp/239.255.0.1:7400@2 udp/10.101.12.71:7416@2) QOS={user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{}} -1711462090.571609 [0] dq.builtin: lease_new(tdur 10000000000 guid 110443d:bb7f10a5:18901533:1c1) @ 0x77041c006550 -1711462090.571611 [0] dq.builtin: lease_new(tdur 10000000000 guid 110443d:bb7f10a5:18901533:1c1) @ 0x77041c0065d0 -1711462090.571615 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:100c7) scanning participants tgt=0 -1711462090.571618 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:3c2) scanning participants tgt=3c7 -1711462090.571620 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:3c7 init_acknack_count = 1 -1711462090.571621 [0] dq.builtin: reader_add_connection(pwr 110443d:bb7f10a5:18901533:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.571624 [0] dq.builtin: proxy_writer_add_connection(pwr 110443d:bb7f10a5:18901533:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) - out-of-sync -1711462090.571627 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:3c7) scanning participants tgt=3c2 -1711462090.571629 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110443d:bb7f10a5:18901533:3c7) -1711462090.571630 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110443d:bb7f10a5:18901533:3c7) - ack seq 2 -1711462090.571633 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462090.571635 [0] dq.builtin: reduced nlocs=3 -1711462090.571637 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571639 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571641 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571642 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571645 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 1 -> 0 -1711462090.571647 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571648 [0] dq.builtin: a b -1711462090.571651 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462090.571653 [0] dq.builtin: loc 1 = udp/10.101.12.71:7416@2 1 { .. +u } -1711462090.571655 [0] dq.builtin: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462090.571658 [0] dq.builtin: best = 2 -1711462090.571661 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571663 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571666 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:4c2) scanning participants tgt=4c7 -1711462090.571668 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:4c7 init_acknack_count = 1 -1711462090.571670 [0] dq.builtin: reader_add_connection(pwr 110443d:bb7f10a5:18901533:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.571672 [0] dq.builtin: proxy_writer_add_connection(pwr 110443d:bb7f10a5:18901533:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) - out-of-sync -1711462090.571675 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:4c7) scanning participants tgt=4c2 -1711462090.571678 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110443d:bb7f10a5:18901533:4c7) -1711462090.571680 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110443d:bb7f10a5:18901533:4c7) - ack seq 1 -1711462090.571683 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462090.571686 [0] dq.builtin: reduced nlocs=3 -1711462090.571687 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571690 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571691 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571694 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571695 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 1 -> 0 -1711462090.571697 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571700 [0] dq.builtin: a b -1711462090.571702 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462090.571705 [0] dq.builtin: loc 1 = udp/10.101.12.71:7416@2 1 { .. +u } -1711462090.571708 [0] dq.builtin: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462090.571709 [0] dq.builtin: best = 2 -1711462090.571712 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571718 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571721 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:200c2) scanning participants tgt=200c7 -1711462090.571723 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:200c7 init_acknack_count = 1 -1711462090.571725 [0] dq.builtin: reader_add_connection(pwr 110443d:bb7f10a5:18901533:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.571727 [0] dq.builtin: proxy_writer_add_connection(pwr 110443d:bb7f10a5:18901533:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) - out-of-sync -1711462090.571730 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:200c7) scanning participants tgt=200c2 -1711462090.571733 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110443d:bb7f10a5:18901533:200c7) -1711462090.571735 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110443d:bb7f10a5:18901533:200c7) - ack seq 1 -1711462090.571740 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462090.571742 [0] dq.builtin: reduced nlocs=3 -1711462090.571743 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571745 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571748 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571751 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571753 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 1 -> 0 -1711462090.571754 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571756 [0] dq.builtin: a b -1711462090.571760 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462090.571762 [0] dq.builtin: loc 1 = udp/10.101.12.71:7416@2 1 { .. +u } -1711462090.571764 [0] dq.builtin: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462090.571766 [0] dq.builtin: best = 2 -1711462090.571769 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571771 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571775 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:300c3) scanning participants tgt=300c4 -1711462090.571776 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:300c4 init_acknack_count = 1 -1711462090.571778 [0] dq.builtin: reader_add_connection(pwr 110443d:bb7f10a5:18901533:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.571779 [0] dq.builtin: proxy_writer_add_connection(pwr 110443d:bb7f10a5:18901533:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) - out-of-sync -1711462090.571784 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:300c4) scanning participants tgt=300c3 -1711462090.571786 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110443d:bb7f10a5:18901533:300c4) -1711462090.571787 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110443d:bb7f10a5:18901533:300c4) - ack seq 0 -1711462090.571791 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462090.571793 [0] dq.builtin: reduced nlocs=3 -1711462090.571794 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571797 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571799 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571800 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571802 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 1 -> 0 -1711462090.571804 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571807 [0] dq.builtin: a b -1711462090.571809 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462090.571811 [0] dq.builtin: loc 1 = udp/10.101.12.71:7416@2 1 { .. +u } -1711462090.571813 [0] dq.builtin: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462090.571814 [0] dq.builtin: best = 2 -1711462090.571815 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571819 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571822 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:301c3) scanning participants tgt=301c4 -1711462090.571825 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:301c4 init_acknack_count = 1 -1711462090.571827 [0] dq.builtin: reader_add_connection(pwr 110443d:bb7f10a5:18901533:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.571833 [0] dq.builtin: proxy_writer_add_connection(pwr 110443d:bb7f10a5:18901533:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) - out-of-sync -1711462090.571835 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:301c4) scanning participants tgt=301c3 -1711462090.571837 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110443d:bb7f10a5:18901533:301c4) -1711462090.571839 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110443d:bb7f10a5:18901533:301c4) - ack seq 0 -1711462090.571842 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462090.571844 [0] dq.builtin: reduced nlocs=3 -1711462090.571845 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571849 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571851 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571853 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571855 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 1 -> 0 -1711462090.571857 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462090.571859 [0] dq.builtin: a b -1711462090.571862 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462090.571865 [0] dq.builtin: loc 1 = udp/10.101.12.71:7416@2 1 { .. +u } -1711462090.571867 [0] dq.builtin: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462090.571868 [0] dq.builtin: best = 2 -1711462090.571870 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571872 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571880 [0] dq.builtin: => EVERYONE -1711462090.571882 [0] dq.builtin: lease_register(l 0x77041c0065d0 guid 110443d:bb7f10a5:18901533:1c1) -1711462090.571883 [0] dq.builtin: directed SPDP packet -> not responding -1711462090.571892 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.571899 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 bes fc3f NEW (0x00000000-0x0000002c-0x00000000-0x00000000-0x00000000 op-Alienware-x16-R1/0.10.4/Linux/Linux) (data udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 meta udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2) QOS={user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{}} -1711462090.571901 [0] dq.builtin: lease_new(tdur 10000000000 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) @ 0x77041c00b710 -1711462090.571905 [0] dq.builtin: lease_new(tdur 10000000000 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) @ 0x77041c00b790 -1711462090.571910 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:100c7) scanning participants tgt=0 -1711462090.571915 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) scanning participants tgt=3c7 -1711462090.571916 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:3c7 init_acknack_count = 1 -1711462090.571919 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.571920 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) - out-of-sync -1711462090.571923 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3c7) scanning participants tgt=3c2 -1711462090.571925 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110d7b4:17c5b8c5:94bd0ff4:3c7) -1711462090.571927 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110d7b4:17c5b8c5:94bd0ff4:3c7) - ack seq 2 -1711462090.571931 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.571933 [0] dq.builtin: reduced nlocs=4 -1711462090.571934 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571937 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571940 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.571941 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571945 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571946 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.571948 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571950 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571952 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.571954 [0] dq.builtin: a b c -1711462090.571956 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462090.571958 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462090.571960 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462090.571963 [0] dq.builtin: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462090.571965 [0] dq.builtin: best = 3 -1711462090.571968 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571971 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571975 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) scanning participants tgt=4c7 -1711462090.571976 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:4c7 init_acknack_count = 1 -1711462090.571979 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.571980 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) - out-of-sync -1711462090.571983 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4c7) scanning participants tgt=4c2 -1711462090.571984 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110d7b4:17c5b8c5:94bd0ff4:4c7) -1711462090.571986 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110d7b4:17c5b8c5:94bd0ff4:4c7) - ack seq 1 -1711462090.571989 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.571991 [0] dq.builtin: reduced nlocs=4 -1711462090.571993 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571995 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571997 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.571998 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572000 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.572002 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572003 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 204 from udp/10.101.12.71:44991 -1711462090.572006 [0] recv: INFOTS(1711462090.571973111) -1711462090.572004 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572014 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.572018 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572011 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #10 L(:1c1 16938.231510) 110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.572021 [0] dq.builtin: a b c -1711462090.572028 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462090.572030 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462090.572032 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462090.572035 [0] dq.builtin: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462090.572038 [0] dq.builtin: best = 3 -1711462090.572039 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.572043 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.572046 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) scanning participants tgt=200c7 -1711462090.572049 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:200c7 init_acknack_count = 1 -1711462090.572051 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.572054 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) - out-of-sync -1711462090.572056 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:200c7) scanning participants tgt=200c2 -1711462090.572059 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110d7b4:17c5b8c5:94bd0ff4:200c7) -1711462090.572062 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110d7b4:17c5b8c5:94bd0ff4:200c7) - ack seq 1 -1711462090.572065 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.572066 [0] dq.builtin: reduced nlocs=4 -1711462090.572068 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572071 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.572075 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572086 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572090 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.572094 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572098 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572080 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 520 from udp/10.101.12.71:44991 -1711462090.572102 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.572115 [0] recvMC: INFOTS(1711462090.571985769) -1711462090.572119 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572123 [0] dq.builtin: a b c -1711462090.572127 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462090.572130 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462090.572130 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #10 110443d:bb7f10a5:18901533:403? -> 0:0:0:0) -1711462090.572135 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462090.572141 [0] recvMC: HEARTBEAT(F#15:10..10 110443d:bb7f10a5:18901533:403? -> 0:0:0:0) -1711462090.572143 [0] dq.builtin: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462090.572146 [0] dq.builtin: best = 3 -1711462090.572149 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.572152 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.572156 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:300c3) scanning participants tgt=300c4 -1711462090.572157 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:300c4 init_acknack_count = 1 -1711462090.572159 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.572161 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) - out-of-sync -1711462090.572164 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:300c4) scanning participants tgt=300c3 -1711462090.572166 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110d7b4:17c5b8c5:94bd0ff4:300c4) -1711462090.572170 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110d7b4:17c5b8c5:94bd0ff4:300c4) - ack seq 0 -1711462090.572176 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.572179 [0] dq.builtin: reduced nlocs=4 -1711462090.572180 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572182 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.572183 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572185 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572187 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.572188 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572190 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572192 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.572193 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572195 [0] dq.builtin: a b c -1711462090.572197 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462090.572199 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462090.572201 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462090.572204 [0] dq.builtin: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462090.572208 [0] dq.builtin: best = 3 -1711462090.572209 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.572211 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.572216 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:301c3) scanning participants tgt=301c4 -1711462090.572227 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:301c4 init_acknack_count = 1 -1711462090.572229 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.572230 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) - out-of-sync -1711462090.572234 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:301c4) scanning participants tgt=301c3 -1711462090.572235 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110d7b4:17c5b8c5:94bd0ff4:301c4) -1711462090.572237 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110d7b4:17c5b8c5:94bd0ff4:301c4) - ack seq 0 -1711462090.572240 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.572242 [0] dq.builtin: reduced nlocs=4 -1711462090.572244 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572245 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.572247 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572248 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572251 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.572252 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572253 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572255 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.572257 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462090.572259 [0] dq.builtin: a b c -1711462090.572261 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462090.572264 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462090.572267 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462090.572270 [0] dq.builtin: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462090.572271 [0] dq.builtin: best = 3 -1711462090.572273 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.572276 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.572282 [0] dq.builtin: => EVERYONE -1711462090.572284 [0] dq.builtin: lease_register(l 0x77041c00b790 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462090.572287 [0] dq.builtin: directed SPDP packet -> not responding -1711462090.572375 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:3c2 multicasting (rel-prd 3 seq-eq-max 0 seq 2 maxseq 2) -1711462090.572385 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:3c2) sent, resched in 0.1 s (min-ack 1!, avail-seq 2, xmit 2) -1711462090.572390 [0] tev: xpack_addmsg 0x770414001390 0x653a1b2cbe40 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462090.572395 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:4c2 multicasting (rel-prd 3 seq-eq-max 0 seq 1 maxseq 1) -1711462090.572399 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:4c2) sent, resched in 0.1 s (min-ack 1!, avail-seq 1, xmit 1) -1711462090.572406 [0] tev: xpack_addmsg 0x770414001390 0x653a1aebe8d0 0(control): niov 2 sz 52 => now niov 3 sz 84 -1711462090.572427 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 84 from udp/10.101.12.71:52025 -1711462090.572430 [0] recv: HEARTBEAT(#1:1..2 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.572435 [0] recv: HEARTBEAT(#1:1..1 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.572431 [0] tev: nn_xpack_send 84: 0x77041400139c:20 0x653a1af32538:32 0x653a1b3ebde8:32 [ udp/239.255.0.1:7400@2 ] -1711462090.572441 [0] tev: traffic-xmit (1) 84 -1711462090.572446 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:200c2 multicasting (rel-prd 3 seq-eq-max 0 seq 1 maxseq 1) -1711462090.572450 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:200c2) sent, resched in 0.1 s (min-ack 1!, avail-seq 1, xmit 1) -1711462090.572454 [0] tev: xpack_addmsg 0x770414001390 0x653a1b2cbe40 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462090.572473 [0] tev: nn_xpack_send 52: 0x77041400139c:20 0x653a1af32538:32 [ udp/239.255.0.1:7400@2 ] -1711462090.572479 [0] tev: traffic-xmit (1) 52 -1711462090.572479 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462090.572493 [0] recv: HEARTBEAT(#1:1..1 110aba1:7b19badd:ce0adb73:200c2? -> 0:0:0:0) -1711462090.572509 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 100 from udp/10.101.12.71:40473 -1711462090.572513 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:300c3 multicasting (rel-prd 3 seq-eq-max 0 seq 0 maxseq 0) -1711462090.572516 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572522 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:300c3) sent, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.572538 [0] tev: xpack_addmsg 0x770414001390 0x653a1b2cbe40 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462090.572539 [0] recv: ACKNACK(F#1:1/2:11 L(:1c1 16938.232019) 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 complying RX1non-timed queue now has 1 items -1711462090.572512 [0] gc: gc 0x77041c005430: deleting -1711462090.572549 [0] recv: RX2non-timed queue now has 2 items -1711462090.572560 [0] recv: rexmit#2 maxseq:2<2<=2defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:3c2 -> 110d7b4:17c5b8c5:94bd0ff4:3c7 - queue for transmit -1711462090.572563 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:301c3 multicasting (rel-prd 3 seq-eq-max 0 seq 0 maxseq 0) -1711462090.572555 [0] gc: gc 0x77041c006ae0: deleting -1711462090.572569 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:301c3) sent, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.572578 [0] tev: xpack_addmsg 0x770414001390 0x653a1aebe8d0 0(control): niov 2 sz 52 => now niov 3 sz 84 -1711462090.572565 [0] recv: ) -1711462090.572579 [0] gc: gc 0x77041c00b2e0: deleting -1711462090.572593 [0] recv: ACKNACK(F#1:1/1:1 110d7b4:17c5b8c5:94bd0ff4:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 complying RX1non-timed queue now has 2 items -1711462090.572593 [0] gc: gc 0x77041c00fb20: deleting -1711462090.572598 [0] recv: rexmit#1 maxseq:1<1<=1defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:4c2 -> 110d7b4:17c5b8c5:94bd0ff4:4c7 - queue for transmit -1711462090.572603 [0] recv: non-timed queue now has 3 items -1711462090.572608 [0] recv: ) -1711462090.572604 [0] tev: nn_xpack_send 84: 0x77041400139c:20 0x653a1af32538:32 0x653a1b3ebde8:32 [ udp/239.255.0.1:7400@2 ] -1711462090.572619 [0] tev: traffic-xmit (1) 84 -1711462090.572613 [0] recv: send_deferred_heartbeat: 34e8ac789b0d206c -> ab74337e0dc0d6b8 - queue for transmit -1711462090.572628 [0] tev: xpack_addmsg 0x770414001390 0x77040c001390 0(rexmit(110aba1:7b19badd:ce0adb73:3c2:#1/1)): niov 0 sz 0 => now niov 3 sz 244 -1711462090.572637 [0] recv: non-timed queue now has 4 items -1711462090.572649 [0] tev: xpack_addmsg 0x770414001390 0x77040c001560 0(rexmit(110aba1:7b19badd:ce0adb73:3c2:#2/1)): niov 3 sz 244 => now niov 5 sz 480 -1711462090.572650 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 68 from udp/10.101.12.71:40473 -1711462090.572654 [0] tev: xpack_addmsg 0x770414001390 0x77040c0018b0 0(rexmit(110aba1:7b19badd:ce0adb73:4c2:#1/1)): niov 5 sz 480 => now niov 7 sz 692 -1711462090.572661 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572666 [0] tev: xpack_addmsg 0x770414001390 0x77040c001730 0(control): niov 7 sz 692 => now niov 8 sz 724 -1711462090.572673 [0] recv: ACKNACK(F#1:1/1:1 L(:1c1 16938.232164) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 complying RX1non-timed queue now has 1 items -1711462090.572676 [0] recv: rexmit#1 maxseq:1<1<=1defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:200c2 -> 110d7b4:17c5b8c5:94bd0ff4:200c7 - queue for transmit -1711462090.572680 [0] recv: ) -1711462090.572674 [0] tev: xpack_addmsg 0x770414001390 0x77040c001ad0 0(control): niov 8 sz 724 => now niov 9 sz 756 -1711462090.572681 [0] recv: send_deferred_heartbeat: 83fc25c67f0bf70f -> a8ebede2376e2416 - queue for transmit -1711462090.572687 [0] tev: xpack_addmsg 0x770414001390 0x77040c001ca0 0(rexmit(110aba1:7b19badd:ce0adb73:200c2:#1/1)): niov 9 sz 756 => now niov 11 sz 820 -1711462090.572688 [0] recv: non-timed queue now has 1 items -1711462090.572698 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 84 from udp/10.101.12.71:52025 -1711462090.572700 [0] tev: xpack_addmsg 0x770414001390 0x77040c001e70 0(control): niov 11 sz 820 => now niov 12 sz 852 -1711462090.572701 [0] recv: HEARTBEAT(#1:1..0 110aba1:7b19badd:ce0adb73:300c3? -> 0:0:0:0) -1711462090.572706 [0] recv: HEARTBEAT(#1:1..0 110aba1:7b19badd:ce0adb73:301c3? -> 0:0:0:0) -1711462090.572711 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 132 from udp/10.101.12.71:44991 -1711462090.572712 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572716 [0] recv: ACKNACK(F#1:1/2:11 L(:1c1 16938.232216) 110443d:bb7f10a5:18901533:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 complying RX1non-timed queue now has 1 items -1711462090.572718 [0] recv: RX2non-timed queue now has 2 items -1711462090.572719 [0] tev: nn_xpack_send 852: 0x77041400139c:20 0x77040c001478:52 0x653a1b4036c4:172 0x77040c001658:36 0x653a1af8b024:200 0x77040c0019a8:36 0x653a1b2ca014:176 0x77040c001828:32 0x77040c001bc8:32 0x77040c001d98:36 0x7704140024ec:28 0x77040c001f68:32 [ udp/10.101.12.71:7414@2 ] -1711462090.572720 [0] recv: rexmit#2 maxseq:2<2<=2defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:3c2 -> 110443d:bb7f10a5:18901533:3c7 - queue for transmit -1711462090.572727 [0] recv: ) -1711462090.572724 [0] tev: traffic-xmit (1) 852 -1711462090.572730 [0] recv: ACKNACK(F#1:1/1:1 110443d:bb7f10a5:18901533:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 complying RX1non-timed queue now has 3 items -1711462090.572736 [0] recv: rexmit#1 maxseq:1<1<=1defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:4c2 -> 110443d:bb7f10a5:18901533:4c7 - queue for transmit -1711462090.572737 [0] recv: non-timed queue now has 4 items -1711462090.572741 [0] recv: ) -1711462090.572745 [0] recv: ACKNACK(F#1:1/1:1 110443d:bb7f10a5:18901533:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 complying RX1non-timed queue now has 4 items -1711462090.572748 [0] recv: rexmit#1 maxseq:1<1<=1defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:200c2 -> 110443d:bb7f10a5:18901533:200c7 - queue for transmit -1711462090.572750 [0] recv: non-timed queue now has 5 items -1711462090.572752 [0] recv: ) -1711462090.572745 [0] tev: xpack_addmsg 0x770414001390 0x77040c002040 0(rexmit(110aba1:7b19badd:ce0adb73:3c2:#1/1)): niov 0 sz 0 => now niov 3 sz 244 -1711462090.572754 [0] recv: send_deferred_heartbeat: 83fc25c67f0bf70f -> 1ba0b4e5d073b591 - queue for transmit -1711462090.572761 [0] recv: non-timed queue now has 5 items -1711462090.572759 [0] tev: xpack_addmsg 0x770414001390 0x77040c0021c0 0(rexmit(110aba1:7b19badd:ce0adb73:3c2:#2/1)): niov 3 sz 244 => now niov 5 sz 480 -1711462090.572766 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 132 from udp/10.101.12.71:58212 -1711462090.572769 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572772 [0] recv: ACKNACK(F#1:1/2:11 L(:1c1 16938.232273) 110e21c:d0762c48:a9f0fb28:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 complying RX1non-timed queue now has 5 items -1711462090.572775 [0] recv: RX2non-timed queue now has 6 items -1711462090.572777 [0] recv: rexmit#2 maxseq:2<2<=2defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:3c2 -> 110e21c:d0762c48:a9f0fb28:3c7 - queue for transmit -1711462090.572782 [0] recv: ) -1711462090.572773 [0] tev: xpack_addmsg 0x770414001390 0x77040c002510 0(rexmit(110aba1:7b19badd:ce0adb73:4c2:#1/1)): niov 5 sz 480 => now niov 7 sz 692 -1711462090.572784 [0] recv: ACKNACK(F#1:1/1:1 110e21c:d0762c48:a9f0fb28:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 complying RX1non-timed queue now has 7 items -1711462090.572790 [0] recv: rexmit#1 maxseq:1<1<=1defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:4c2 -> 110e21c:d0762c48:a9f0fb28:4c7 - queue for transmit -1711462090.572794 [0] recv: non-timed queue now has 7 items -1711462090.572796 [0] recv: ) -1711462090.572794 [0] tev: xpack_addmsg 0x770414001390 0x77040c002390 0(control): niov 7 sz 692 => now niov 8 sz 724 -1711462090.572799 [0] recv: ACKNACK(F#1:1/1:1 110e21c:d0762c48:a9f0fb28:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 complying RX1 (110aba1:7b19badd:ce0adb73:200c2#1/1:1+1->*) rexmit#1 maxseq:1<1<=1defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:200c2 -> 110e21c:d0762c48:a9f0fb28:200c7 - queue for transmit -1711462090.572803 [0] recv: non-timed queue now has 7 items -1711462090.572804 [0] recv: ) -1711462090.572806 [0] recv: send_deferred_heartbeat: 83fc25c67f0bf70f -> ff2b90d6377b7c1e - queue for transmit -1711462090.572808 [0] recv: non-timed queue now has 8 items -1711462090.572811 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 92 from udp/10.101.12.71:58212 -1711462090.572813 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572813 [0] tev: nn_xpack_send 724: 0x77041400139c:20 0x77040c002128:52 0x653a1b4036c4:172 0x77040c0022b8:36 0x653a1af8b024:200 0x77040c002608:36 0x653a1b2ca014:176 0x77040c002488:32 [ udp/10.101.12.71:7416@2 ] -1711462090.572817 [0] recv: ACKNACK(F#1:1/0: L(:1c1 16938.232318) 110e21c:d0762c48:a9f0fb28:300c4 -> 110aba1:7b19badd:ce0adb73:300c3 setting-has-replied-to-hb happy-now) -1711462090.572820 [0] recv: ACKNACK(F#1:1/0: 110e21c:d0762c48:a9f0fb28:301c4 -> 110aba1:7b19badd:ce0adb73:301c3 setting-has-replied-to-hb happy-now) -1711462090.572818 [0] tev: traffic-xmit (1) 724 -1711462090.572825 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 92 from udp/10.101.12.71:44991 -1711462090.572831 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572832 [0] tev: xpack_addmsg 0x770414001390 0x77040c0028b0 0(rexmit(110aba1:7b19badd:ce0adb73:200c2:#1/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462090.572835 [0] recv: ACKNACK(F#1:1/0: L(:1c1 16938.232336) 110443d:bb7f10a5:18901533:300c4 -> 110aba1:7b19badd:ce0adb73:300c3 setting-has-replied-to-hb happy-now) -1711462090.572837 [0] recv: ACKNACK(F#1:1/0: 110443d:bb7f10a5:18901533:301c4 -> 110aba1:7b19badd:ce0adb73:301c3 setting-has-replied-to-hb happy-now) -1711462090.572840 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 92 from udp/10.101.12.71:40473 -1711462090.572842 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572844 [0] recv: ACKNACK(F#1:1/0: L(:1c1 16938.232346) 110d7b4:17c5b8c5:94bd0ff4:300c4 -> 110aba1:7b19badd:ce0adb73:300c3 setting-has-replied-to-hb happy-now) -1711462090.572846 [0] recv: ACKNACK(F#1:1/0: 110d7b4:17c5b8c5:94bd0ff4:301c4 -> 110aba1:7b19badd:ce0adb73:301c3 setting-has-replied-to-hb happy-now) -1711462090.572850 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 804 from udp/10.101.12.71:50807 -1711462090.572852 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572852 [0] tev: nn_xpack_send 84: 0x77041400139c:20 0x77040c0029a8:36 0x7704140024ec:28 [ udp/239.255.0.1:7400@2 ] -1711462090.572857 [0] recv: ACKNACK(F#1:1/2:11 110cd0d:56a27910:57318171:3c7? -> 110aba1:7b19badd:ce0adb73:3c2) -1711462090.572863 [0] tev: traffic-xmit (1) 84 -1711462090.572864 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.572875 [0] recv: ACKNACK(F#12:3/0: 110cd0d:e3b81b8d:1ccb65b1:3c7? -> 110aba1:7b19badd:ce0adb73:3c2) -1711462090.572877 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.572880 [0] recv: ACKNACK(F#6:3/0: 110cd0d:79d6eeac:ea4a8907:3c7? -> 110aba1:7b19badd:ce0adb73:3c2) -1711462090.572882 [0] recv: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462090.572871 [0] tev: xpack_addmsg 0x770414001390 0x77040c002730 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.572885 [0] recv: ACKNACK(F#1:1/1:1 110cd0d:56a27910:57318171:4c7? -> 110aba1:7b19badd:ce0adb73:4c2) -1711462090.572892 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.572893 [0] tev: xpack_addmsg 0x770414001390 0x77040c002ad0 0(control): niov 2 sz 68 => now niov 3 sz 100 -1711462090.572895 [0] recv: ACKNACK(F#10:2/0: 110cd0d:e3b81b8d:1ccb65b1:4c7? -> 110aba1:7b19badd:ce0adb73:4c2) -1711462090.572899 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.572901 [0] tev: nn_xpack_send 100: 0x77041400139c:20 0x77040c002818:48 0x77040c002bc8:32 [ udp/10.101.12.71:7416@2 ] -1711462090.572905 [0] tev: traffic-xmit (1) 100 -1711462090.572902 [0] recv: ACKNACK(F#5:2/0: 110cd0d:79d6eeac:ea4a8907:4c7? -> 110aba1:7b19badd:ce0adb73:4c2) -1711462090.572909 [0] recv: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462090.572908 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(rexmit(110aba1:7b19badd:ce0adb73:3c2:#1/1)): niov 0 sz 0 => now niov 3 sz 244 -1711462090.572912 [0] recv: ACKNACK(F#1:1/1:1 110cd0d:56a27910:57318171:200c7? -> 110aba1:7b19badd:ce0adb73:200c2) -1711462090.572916 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.572919 [0] recv: ACKNACK(F#7:2/0: 110cd0d:e3b81b8d:1ccb65b1:200c7? -> 110aba1:7b19badd:ce0adb73:200c2) -1711462090.572923 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.572921 [0] tev: xpack_addmsg 0x770414001390 0x77040c002dd0 0(rexmit(110aba1:7b19badd:ce0adb73:3c2:#2/1)): niov 3 sz 244 => now niov 5 sz 480 -1711462090.572925 [0] recv: ACKNACK(F#5:2/0: 110cd0d:79d6eeac:ea4a8907:200c7? -> 110aba1:7b19badd:ce0adb73:200c2) -1711462090.572930 [0] tev: xpack_addmsg 0x770414001390 0x77040c003120 0(rexmit(110aba1:7b19badd:ce0adb73:4c2:#1/1)): niov 5 sz 480 => now niov 7 sz 692 -1711462090.572935 [0] recv: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462090.572938 [0] recv: ACKNACK(F#1:1/0: 110cd0d:56a27910:57318171:300c4? -> 110aba1:7b19badd:ce0adb73:300c3) -1711462090.572942 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.572939 [0] tev: xpack_addmsg 0x770414001390 0x77040c002fa0 0(control): niov 7 sz 692 => now niov 8 sz 724 -1711462090.572944 [0] recv: ACKNACK(F#5:1/0: 110cd0d:e3b81b8d:1ccb65b1:300c4? -> 110aba1:7b19badd:ce0adb73:300c3) -1711462090.572947 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 8 sz 724 => now niov 9 sz 756 -1711462090.572950 [0] tev: xpack_addmsg 0x770414001390 0x77040c003470 0(control): niov 9 sz 756 => now niov 10 sz 788 -1711462090.572948 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.572955 [0] recv: ACKNACK(F#3:1/0: 110cd0d:79d6eeac:ea4a8907:300c4? -> 110aba1:7b19badd:ce0adb73:300c3) -1711462090.572959 [0] recv: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462090.572958 [0] tev: nn_xpack_send 788: 0x77041400139c:20 0x77040c002d38:52 0x653a1b4036c4:172 0x77040c002ec8:36 0x653a1af8b024:200 0x77040c003218:36 0x653a1b2ca014:176 0x77040c003098:32 0x77040c0033e8:32 0x77040c003568:32 [ udp/10.101.12.71:7412@2 ] -1711462090.572961 [0] recv: ACKNACK(F#1:1/0: 110cd0d:56a27910:57318171:301c4? -> 110aba1:7b19badd:ce0adb73:301c3) -1711462090.572963 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.572965 [0] recv: ACKNACK(F#5:1/0: 110cd0d:e3b81b8d:1ccb65b1:301c4? -> 110aba1:7b19badd:ce0adb73:301c3) -1711462090.572966 [0] tev: traffic-xmit (1) 788 -1711462090.572967 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.572972 [0] recv: ACKNACK(F#3:1/0: 110cd0d:79d6eeac:ea4a8907:301c4? -> 110aba1:7b19badd:ce0adb73:301c3) -1711462090.572975 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 1356 from udp/10.101.12.71:50807 -1711462090.572977 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.572979 [0] recv: INFOTS(1711462090.143481252) -1711462090.572982 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462090.572983 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.572984 [0] recv: INFOTS(1711462067.905323275) -1711462090.572986 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462090.572988 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.572989 [0] recv: INFOTS(1711462069.653526636) -1711462090.572991 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462090.572992 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.572993 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 84 from udp/10.101.12.71:52025 -1711462090.572996 [0] recv: INFOTS(1711462090.570429633) -1711462090.572999 [0] recv: DATA(110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0 #1 110aba1:7b19badd:ce0adb73:200c2? -> 0:0:0:0) -1711462090.573001 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 bes fc3f NEW (0x00000000-0x0000002c-0x00000000-0x00000000-0x00000000 op-Alienware-x16-R1/0.10.4/Linux/Linux) (data udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 meta udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2) QOS={user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{}} -1711462090.573001 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 120 from udp/10.101.12.71:40473 -1711462090.573009 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.573007 [0] dq.builtin: lease_new(tdur 10000000000 guid 110cd0d:56a27910:57318171:1c1) @ 0x77041c00ff50 -1711462090.573013 [0] recv: ACKNACK(F#2:3/0: L(:1c1 16938.232513) 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 setting-has-replied-to-hb happy-now) -1711462090.573016 [0] dq.builtin: lease_new(tdur 10000000000 guid 110cd0d:56a27910:57318171:1c1) @ 0x77041c00ffd0 -1711462090.573018 [0] recv: ACKNACK(F#2:2/0: 110d7b4:17c5b8c5:94bd0ff4:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 setting-has-replied-to-hb happy-now) -1711462090.573022 [0] recv: ACKNACK(F#2:2/0: 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 setting-has-replied-to-hb happy-now) -1711462090.573024 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:100c7) scanning participants tgt=0 -1711462090.573025 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.573034 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.573031 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:3c2) scanning participants tgt=3c7 -1711462090.573037 [0] recv: ACKNACK(F#2:3/0: L(:1c1 16938.232538) 110443d:bb7f10a5:18901533:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 setting-has-replied-to-hb happy-now) -1711462090.573039 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:3c7 init_acknack_count = 1 -1711462090.573041 [0] dq.builtin: reader_add_connection(pwr 110cd0d:56a27910:57318171:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.573045 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 92 from udp/10.101.12.71:44991 -1711462090.573046 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:56a27910:57318171:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) - out-of-sync -1711462090.573046 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.573049 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:3c7) scanning participants tgt=3c2 -1711462090.573051 [0] recv: ACKNACK(F#2:2/0: L(:1c1 16938.232550) 110443d:bb7f10a5:18901533:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 setting-has-replied-to-hb happy-now) -1711462090.573052 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110cd0d:56a27910:57318171:3c7) -1711462090.573054 [0] recv: ACKNACK(F#2:2/0: 110443d:bb7f10a5:18901533:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 setting-has-replied-to-hb happy-now) -1711462090.573056 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110cd0d:56a27910:57318171:3c7) - ack seq 2 -1711462090.573064 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573066 [0] dq.builtin: reduced nlocs=5 -1711462090.573067 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573069 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573071 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573073 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573074 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573078 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573079 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573082 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573083 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573086 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573089 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573091 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573093 [0] dq.builtin: a b c d -1711462090.573095 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 1 { +u .. .. .. } -1711462090.573097 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. +u .. .. } -1711462090.573099 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.573101 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. +u .. } -1711462090.573103 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.573105 [0] dq.builtin: best = 4 -1711462090.573106 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 120 from udp/10.101.12.71:58212 -1711462090.573108 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.573106 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573114 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573120 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:4c2) scanning participants tgt=4c7 -1711462090.573121 [0] recv: ACKNACK(F#2:3/0: L(:1c1 16938.232613) 110e21c:d0762c48:a9f0fb28:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK1 RM0 setting-has-replied-to-hb happy-now) -1711462090.573121 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:4c7 init_acknack_count = 1 -1711462090.573126 [0] dq.builtin: reader_add_connection(pwr 110cd0d:56a27910:57318171:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.573128 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:56a27910:57318171:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) - out-of-sync -1711462090.573124 [0] recv: ACKNACK(F#2:2/0: 110e21c:d0762c48:a9f0fb28:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 setting-has-replied-to-hb happy-now) -1711462090.573132 [0] recv: ACKNACK(F#2:2/0: 110e21c:d0762c48:a9f0fb28:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 setting-has-replied-to-hb happy-now) -1711462090.573135 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:4c7) scanning participants tgt=4c2 -1711462090.573137 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110cd0d:56a27910:57318171:4c7) -1711462090.573139 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110cd0d:56a27910:57318171:4c7) - ack seq 1 -1711462090.573141 [0] gc: gc 0x77041c0123e0: not yet, shortsleep -1711462090.573144 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573150 [0] dq.builtin: reduced nlocs=5 -1711462090.573152 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573153 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573155 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573156 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573158 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573160 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573161 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573163 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573165 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573166 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573168 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573169 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573171 [0] dq.builtin: a b c d -1711462090.573175 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 1 { +u .. .. .. } -1711462090.573177 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. +u .. .. } -1711462090.573179 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.573181 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. +u .. } -1711462090.573184 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.573185 [0] dq.builtin: best = 4 -1711462090.573186 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573188 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573193 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:200c2) scanning participants tgt=200c7 -1711462090.573195 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:200c7 init_acknack_count = 1 -1711462090.573196 [0] dq.builtin: reader_add_connection(pwr 110cd0d:56a27910:57318171:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.573198 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:56a27910:57318171:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) - out-of-sync -1711462090.573202 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:200c7) scanning participants tgt=200c2 -1711462090.573203 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110cd0d:56a27910:57318171:200c7) -1711462090.573205 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110cd0d:56a27910:57318171:200c7) - ack seq 1 -1711462090.573209 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573210 [0] dq.builtin: reduced nlocs=5 -1711462090.573211 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573213 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573215 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573216 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573217 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573219 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573221 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573222 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573224 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573226 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573227 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573229 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573231 [0] dq.builtin: a b c d -1711462090.573233 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 1 { +u .. .. .. } -1711462090.573235 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. +u .. .. } -1711462090.573237 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.573238 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. +u .. } -1711462090.573240 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.573241 [0] dq.builtin: best = 4 -1711462090.573244 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573246 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573249 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:300c3) scanning participants tgt=300c4 -1711462090.573252 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:300c4 init_acknack_count = 1 -1711462090.573253 [0] dq.builtin: reader_add_connection(pwr 110cd0d:56a27910:57318171:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.573255 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:56a27910:57318171:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) - out-of-sync -1711462090.573257 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:300c4) scanning participants tgt=300c3 -1711462090.573259 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110cd0d:56a27910:57318171:300c4) -1711462090.573260 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110cd0d:56a27910:57318171:300c4) - ack seq 0 -1711462090.573264 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573266 [0] dq.builtin: reduced nlocs=5 -1711462090.573267 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573268 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573270 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573271 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573273 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573274 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573275 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573277 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573278 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573280 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573281 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573283 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573284 [0] dq.builtin: a b c d -1711462090.573286 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 1 { +u .. .. .. } -1711462090.573288 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. +u .. .. } -1711462090.573290 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.573292 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. +u .. } -1711462090.573294 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.573295 [0] dq.builtin: best = 4 -1711462090.573296 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573298 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573302 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:301c3) scanning participants tgt=301c4 -1711462090.573303 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:301c4 init_acknack_count = 1 -1711462090.573305 [0] dq.builtin: reader_add_connection(pwr 110cd0d:56a27910:57318171:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.573306 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:56a27910:57318171:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) - out-of-sync -1711462090.573308 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:301c4) scanning participants tgt=301c3 -1711462090.573310 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110cd0d:56a27910:57318171:301c4) -1711462090.573311 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110cd0d:56a27910:57318171:301c4) - ack seq 0 -1711462090.573315 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573316 [0] dq.builtin: reduced nlocs=5 -1711462090.573318 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573320 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573322 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573323 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573324 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573326 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573327 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573329 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573330 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573331 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573333 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573334 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573336 [0] dq.builtin: a b c d -1711462090.573338 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 1 { +u .. .. .. } -1711462090.573340 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. +u .. .. } -1711462090.573341 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.573343 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. +u .. } -1711462090.573345 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.573346 [0] dq.builtin: best = 4 -1711462090.573348 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573350 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573356 [0] dq.builtin: => EVERYONE -1711462090.573359 [0] dq.builtin: lease_register(l 0x77041c00ffd0 guid 110cd0d:56a27910:57318171:1c1) -1711462090.573360 [0] dq.builtin: directed SPDP packet -> not responding -1711462090.573370 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.573377 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 bes fc3f NEW (0x00000000-0x0000002c-0x00000000-0x00000000-0x00000000 op-Alienware-x16-R1/0.10.4/Linux/Linux) (data udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 meta udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2) QOS={user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{}} -1711462090.573380 [0] dq.builtin: lease_new(tdur 10000000000 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) @ 0x77041c015050 -1711462090.573381 [0] dq.builtin: lease_new(tdur 10000000000 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) @ 0x77041c0150d0 -1711462090.573385 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:100c7) scanning participants tgt=0 -1711462090.573388 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:3c2) scanning participants tgt=3c7 -1711462090.573389 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:3c7 init_acknack_count = 1 -1711462090.573390 [0] dq.builtin: reader_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.573392 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) - out-of-sync -1711462090.573397 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:3c7) scanning participants tgt=3c2 -1711462090.573399 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110cd0d:e3b81b8d:1ccb65b1:3c7) -1711462090.573401 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110cd0d:e3b81b8d:1ccb65b1:3c7) - ack seq 2 -1711462090.573405 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573406 [0] dq.builtin: reduced nlocs=5 -1711462090.573408 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573409 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573411 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573412 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573413 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573415 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573416 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573417 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573419 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573420 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573422 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573423 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573424 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573426 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573427 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573429 [0] dq.builtin: a b c d e -1711462090.573431 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 0 { +u +u .. .. .. } -1711462090.573433 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. +u .. .. } -1711462090.573435 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. +u } -1711462090.573437 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. +u .. } -1711462090.573439 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462090.573440 [0] dq.builtin: best = 4 -1711462090.573442 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573444 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573446 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:4c2) scanning participants tgt=4c7 -1711462090.573448 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:4c7 init_acknack_count = 1 -1711462090.573449 [0] dq.builtin: reader_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.573451 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) - out-of-sync -1711462090.573454 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:4c7) scanning participants tgt=4c2 -1711462090.573457 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110cd0d:e3b81b8d:1ccb65b1:4c7) -1711462090.573458 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110cd0d:e3b81b8d:1ccb65b1:4c7) - ack seq 1 -1711462090.573462 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573463 [0] dq.builtin: reduced nlocs=5 -1711462090.573466 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573468 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573470 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573471 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573473 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573474 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573476 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573477 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573478 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573480 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573481 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573483 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573484 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573485 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573487 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573488 [0] dq.builtin: a b c d e -1711462090.573491 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 0 { +u +u .. .. .. } -1711462090.573492 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. +u .. .. } -1711462090.573494 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. +u } -1711462090.573496 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. +u .. } -1711462090.573498 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462090.573499 [0] dq.builtin: best = 4 -1711462090.573501 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573503 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573506 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:200c2) scanning participants tgt=200c7 -1711462090.573507 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:200c7 init_acknack_count = 1 -1711462090.573508 [0] dq.builtin: reader_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.573510 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) - out-of-sync -1711462090.573512 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:200c7) scanning participants tgt=200c2 -1711462090.573514 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110cd0d:e3b81b8d:1ccb65b1:200c7) -1711462090.573515 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110cd0d:e3b81b8d:1ccb65b1:200c7) - ack seq 1 -1711462090.573519 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573520 [0] dq.builtin: reduced nlocs=5 -1711462090.573522 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573523 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573525 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573526 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573527 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573529 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573530 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573532 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573533 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573534 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573536 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573538 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573540 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573541 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573543 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573545 [0] dq.builtin: a b c d e -1711462090.573547 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 0 { +u +u .. .. .. } -1711462090.573549 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. +u .. .. } -1711462090.573551 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. +u } -1711462090.573553 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. +u .. } -1711462090.573555 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462090.573556 [0] dq.builtin: best = 4 -1711462090.573558 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573560 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573563 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:300c3) scanning participants tgt=300c4 -1711462090.573565 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:300c4 init_acknack_count = 1 -1711462090.573566 [0] dq.builtin: reader_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.573568 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) - out-of-sync -1711462090.573571 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:300c4) scanning participants tgt=300c3 -1711462090.573573 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110cd0d:e3b81b8d:1ccb65b1:300c4) -1711462090.573574 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110cd0d:e3b81b8d:1ccb65b1:300c4) - ack seq 0 -1711462090.573578 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573579 [0] dq.builtin: reduced nlocs=5 -1711462090.573581 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573582 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573584 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573585 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573586 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573588 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573589 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573591 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573592 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573593 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573596 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573597 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573598 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573600 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573601 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573603 [0] dq.builtin: a b c d e -1711462090.573605 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 0 { +u +u .. .. .. } -1711462090.573607 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. +u .. .. } -1711462090.573609 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. +u } -1711462090.573612 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. +u .. } -1711462090.573614 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462090.573615 [0] dq.builtin: best = 4 -1711462090.573616 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573619 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573621 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:301c3) scanning participants tgt=301c4 -1711462090.573623 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:301c4 init_acknack_count = 1 -1711462090.573625 [0] dq.builtin: reader_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.573627 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) - out-of-sync -1711462090.573630 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:301c4) scanning participants tgt=301c3 -1711462090.573632 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110cd0d:e3b81b8d:1ccb65b1:301c4) -1711462090.573638 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110cd0d:e3b81b8d:1ccb65b1:301c4) - ack seq 0 -1711462090.573645 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573646 [0] dq.builtin: reduced nlocs=5 -1711462090.573648 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573651 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573654 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573656 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573659 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573659 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 612 from udp/10.101.12.71:40473 -1711462090.573660 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573664 [0] recv: INFOTS(1711462090.573532356) -1711462090.573666 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573671 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573674 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 536 from udp/10.101.12.71:40473 -1711462090.573680 [0] recvMC: INFOTS(1711462090.573586880) -1711462090.573685 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #10 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.573675 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573691 [0] recvMC: HEARTBEAT(F#16:10..10 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.573700 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573705 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573709 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573712 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573712 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #10 L(:1c1 16938.233167) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.573714 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573726 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573723 [0] recv: INFOTS(1711462090.573575071) -1711462090.573728 [0] dq.builtin: a b c d e -1711462090.573733 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #8 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.573734 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 0 { +u +u .. .. .. } -1711462090.573754 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. +u .. .. } -1711462090.573767 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. +u } -1711462090.573771 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. +u .. } -1711462090.573776 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462090.573779 [0] dq.builtin: best = 4 -1711462090.573782 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573789 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573807 [0] dq.builtin: => EVERYONE -1711462090.573813 [0] dq.builtin: lease_register(l 0x77041c0150d0 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462090.573815 [0] dq.builtin: directed SPDP packet -> not responding -1711462090.573834 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.573849 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 bes fc3f NEW (0x00000000-0x0000002c-0x00000000-0x00000000-0x00000000 op-Alienware-x16-R1/0.10.4/Linux/Linux) (data udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 meta udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2) QOS={user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{}} -1711462090.573853 [0] dq.builtin: lease_new(tdur 10000000000 guid 110cd0d:79d6eeac:ea4a8907:1c1) @ 0x77041c019810 -1711462090.573855 [0] dq.builtin: lease_new(tdur 10000000000 guid 110cd0d:79d6eeac:ea4a8907:1c1) @ 0x77041c019890 -1711462090.573864 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:100c7) scanning participants tgt=0 -1711462090.573870 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:3c2) scanning participants tgt=3c7 -1711462090.573873 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:3c7 init_acknack_count = 1 -1711462090.573875 [0] dq.builtin: reader_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.573879 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:3c2 rd 110aba1:7b19badd:ce0adb73:3c7) - out-of-sync -1711462090.573884 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:3c7) scanning participants tgt=3c2 -1711462090.573886 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110cd0d:79d6eeac:ea4a8907:3c7) -1711462090.573890 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:3c2 prd 110cd0d:79d6eeac:ea4a8907:3c7) - ack seq 2 -1711462090.573898 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.573901 [0] dq.builtin: reduced nlocs=5 -1711462090.573904 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573907 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573910 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573912 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573916 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573918 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573924 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573927 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.573930 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573933 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573936 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.573939 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573941 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573943 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.573946 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573948 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.573950 [0] dq.builtin: rdidx 5 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.573952 [0] dq.builtin: rdidx 5 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.573956 [0] dq.builtin: a b c d e f -1711462090.573961 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. .. } -1711462090.573964 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. .. } -1711462090.573967 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. .. +u } -1711462090.573971 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u .. } -1711462090.573975 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -3 { +1 +1 +1 +1 +1 +1 } -1711462090.573977 [0] dq.builtin: best = 4 -1711462090.573979 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.573984 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.573989 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:4c2) scanning participants tgt=4c7 -1711462090.573992 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:4c7 init_acknack_count = 1 -1711462090.573995 [0] dq.builtin: reader_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.573997 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:4c2 rd 110aba1:7b19badd:ce0adb73:4c7) - out-of-sync -1711462090.574002 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:4c7) scanning participants tgt=4c2 -1711462090.574004 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110cd0d:79d6eeac:ea4a8907:4c7) -1711462090.574007 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:4c2 prd 110cd0d:79d6eeac:ea4a8907:4c7) - ack seq 1 -1711462090.574014 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.574016 [0] dq.builtin: reduced nlocs=5 -1711462090.574019 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574022 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574024 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574026 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574028 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574031 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574033 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574036 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574038 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574040 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574043 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.574045 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574049 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574052 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.574055 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574056 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574059 [0] dq.builtin: rdidx 5 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.574062 [0] dq.builtin: rdidx 5 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574065 [0] dq.builtin: a b c d e f -1711462090.574068 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. .. } -1711462090.574072 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. .. } -1711462090.574075 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. .. +u } -1711462090.574078 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u .. } -1711462090.574082 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -3 { +1 +1 +1 +1 +1 +1 } -1711462090.574084 [0] dq.builtin: best = 4 -1711462090.574087 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.574091 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.574096 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:200c2) scanning participants tgt=200c7 -1711462090.574098 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:200c7 init_acknack_count = 1 -1711462090.574100 [0] dq.builtin: reader_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.574103 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:200c2 rd 110aba1:7b19badd:ce0adb73:200c7) - out-of-sync -1711462090.574108 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:200c7) scanning participants tgt=200c2 -1711462090.574110 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110cd0d:79d6eeac:ea4a8907:200c7) -1711462090.574113 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:200c2 prd 110cd0d:79d6eeac:ea4a8907:200c7) - ack seq 1 -1711462090.574120 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.574122 [0] dq.builtin: reduced nlocs=5 -1711462090.574126 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574128 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574131 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574133 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574135 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574138 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574139 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574142 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574144 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574147 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574149 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.574152 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574154 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574156 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.574158 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574160 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574163 [0] dq.builtin: rdidx 5 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.574165 [0] dq.builtin: rdidx 5 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574168 [0] dq.builtin: a b c d e f -1711462090.574174 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. .. } -1711462090.574177 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. .. } -1711462090.574178 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:3c2) suppressed, resched in 0.1 s (min-ack 2!, avail-seq 2, xmit 2) -1711462090.574180 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. .. +u } -1711462090.574189 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u .. } -1711462090.574194 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -3 { +1 +1 +1 +1 +1 +1 } -1711462090.574195 [0] dq.builtin: best = 4 -1711462090.574198 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.574202 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.574203 [0] gc: gc 0x77041c0123e0: deleting -1711462090.574208 [0] gc: gc 0x77041c014c20: deleting -1711462090.574212 [0] gc: gc 0x77041c0193e0: deleting -1711462090.574209 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:300c3) scanning participants tgt=300c4 -1711462090.574218 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:300c4 init_acknack_count = 1 -1711462090.574220 [0] dq.builtin: reader_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.574223 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:300c3 rd 110aba1:7b19badd:ce0adb73:300c4) - out-of-sync -1711462090.574228 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:300c4) scanning participants tgt=300c3 -1711462090.574230 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110cd0d:79d6eeac:ea4a8907:300c4) -1711462090.574233 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:300c3 prd 110cd0d:79d6eeac:ea4a8907:300c4) - ack seq 0 -1711462090.574241 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.574243 [0] dq.builtin: reduced nlocs=5 -1711462090.574245 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574247 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574251 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574248 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:4c2) suppressed, resched in 0.1 s (min-ack 1!, avail-seq 1, xmit 1) -1711462090.574253 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574259 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:200c2) suppressed, resched in 0.1 s (min-ack 1!, avail-seq 1, xmit 1) -1711462090.574260 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574266 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574268 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574270 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574273 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574275 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574278 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.574280 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574282 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574285 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.574288 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574290 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574293 [0] dq.builtin: rdidx 5 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.574298 [0] dq.builtin: rdidx 5 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574301 [0] dq.builtin: a b c d e f -1711462090.574306 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. .. } -1711462090.574309 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. .. } -1711462090.574313 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. .. +u } -1711462090.574316 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u .. } -1711462090.574321 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -3 { +1 +1 +1 +1 +1 +1 } -1711462090.574323 [0] dq.builtin: best = 4 -1711462090.574326 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.574330 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.574336 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:301c3) scanning participants tgt=301c4 -1711462090.574338 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:301c4 init_acknack_count = 1 -1711462090.574341 [0] dq.builtin: reader_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.574344 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:301c3 rd 110aba1:7b19badd:ce0adb73:301c4) - out-of-sync -1711462090.574350 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:301c4) scanning participants tgt=301c3 -1711462090.574352 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110cd0d:79d6eeac:ea4a8907:301c4) -1711462090.574355 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:301c3 prd 110cd0d:79d6eeac:ea4a8907:301c4) - ack seq 0 -1711462090.574355 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:300c3) suppressed, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.574364 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462090.574366 [0] dq.builtin: reduced nlocs=5 -1711462090.574369 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574371 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574374 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574376 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574379 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574382 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574384 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574387 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462090.574390 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574392 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574395 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462090.574397 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574400 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574402 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7416@2 3 -> 0 -1711462090.574405 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574407 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.574410 [0] dq.builtin: rdidx 5 lidx udp/10.101.12.71:7414@2 2 -> 0 -1711462090.574413 [0] dq.builtin: rdidx 5 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.574416 [0] dq.builtin: a b c d e f -1711462090.574420 [0] dq.builtin: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. .. } -1711462090.574425 [0] dq.builtin: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. .. } -1711462090.574429 [0] dq.builtin: loc 2 = udp/10.101.12.71:7414@2 1 { .. .. .. .. .. +u } -1711462090.574433 [0] dq.builtin: loc 3 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u .. } -1711462090.574438 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -3 { +1 +1 +1 +1 +1 +1 } -1711462090.574439 [0] dq.builtin: best = 4 -1711462090.574442 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.574446 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.574452 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:301c3) suppressed, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.574457 [0] dq.builtin: => EVERYONE -1711462090.574463 [0] dq.builtin: lease_register(l 0x77041c019890 guid 110cd0d:79d6eeac:ea4a8907:1c1) -1711462090.574466 [0] dq.builtin: directed SPDP packet -> not responding -1711462090.574469 [0] gc: gc 0x77041c01d7a0: deleting -1711462090.575588 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 656 from udp/10.101.12.71:40473 -1711462090.575606 [0] recv: INFOTS(1711462090.575442461) -1711462090.575606 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 584 from udp/10.101.12.71:40473 -1711462090.575612 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #11 L(:1c1 16938.235109) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.575620 [0] recv: INFOTS(1711462090.575483556) -1711462090.575618 [0] recvMC: INFOTS(1711462090.575495060) -1711462090.575624 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #9 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.575632 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #11 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.575638 [0] recvMC: HEARTBEAT(F#17:11..11 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.575763 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 328 from udp/10.101.12.71:40473 -1711462090.575766 [0] recv: INFOTS(1711462090.575726889) -1711462090.575771 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #12 L(:1c1 16938.235270) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.575814 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 320 from udp/10.101.12.71:40473 -1711462090.575817 [0] recv: INFOTS(1711462090.575762290) -1711462090.575823 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #10 L(:1c1 16938.235321) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.575865 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 632 from udp/10.101.12.71:40473 -1711462090.575872 [0] recvMC: INFOTS(1711462090.575799187) -1711462090.575876 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #12 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.575880 [0] recvMC: HEARTBEAT(F#18:12..12 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.576029 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.576031 [0] recv: INFOTS(1711462090.575995325) -1711462090.576037 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #13 L(:1c1 16938.235535) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.576069 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.576072 [0] recv: INFOTS(1711462090.576028935) -1711462090.576078 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #11 L(:1c1 16938.235576) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.576125 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 680 from udp/10.101.12.71:40473 -1711462090.576132 [0] recvMC: INFOTS(1711462090.576052639) -1711462090.576140 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #13 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.576144 [0] recvMC: HEARTBEAT(F#19:13..13 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.576302 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.576305 [0] recv: INFOTS(1711462090.576266665) -1711462090.576311 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #14 L(:1c1 16938.235808) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.576338 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.576340 [0] recv: INFOTS(1711462090.576303013) -1711462090.576344 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #12 L(:1c1 16938.235844) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.576393 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 728 from udp/10.101.12.71:40473 -1711462090.576400 [0] recvMC: INFOTS(1711462090.576322325) -1711462090.576407 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #14 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.576411 [0] recvMC: HEARTBEAT(F#20:14..14 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.576565 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.576568 [0] recv: INFOTS(1711462090.576532468) -1711462090.576575 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #15 L(:1c1 16938.236072) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.576608 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.576610 [0] recv: INFOTS(1711462090.576567173) -1711462090.576617 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #13 L(:1c1 16938.236114) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.576654 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 776 from udp/10.101.12.71:40473 -1711462090.576661 [0] recvMC: INFOTS(1711462090.576579736) -1711462090.576666 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #15 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.576672 [0] recvMC: HEARTBEAT(F#21:15..15 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.576780 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.576790 [0] recv: INFOTS(1711462090.576742682) -1711462090.576798 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #16 L(:1c1 16938.236293) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.576842 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.576850 [0] recv: INFOTS(1711462090.576793854) -1711462090.576857 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #14 L(:1c1 16938.236353) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.576877 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 824 from udp/10.101.12.71:40473 -1711462090.576885 [0] recvMC: INFOTS(1711462090.576805226) -1711462090.576891 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #16 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.576906 [0] recvMC: HEARTBEAT(F#22:16..16 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.577431 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 652 from udp/10.101.12.71:40473 -1711462090.577439 [0] recv: INFOTS(1711462090.577330990) -1711462090.577444 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #17 L(:1c1 16938.236942) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.577448 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 872 from udp/10.101.12.71:40473 -1711462090.577449 [0] recv: INFOTS(1711462090.577371768) -1711462090.577456 [0] recvMC: INFOTS(1711462090.577381731) -1711462090.577467 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #15 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.577479 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #17 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.577492 [0] recvMC: HEARTBEAT(F#23:17..17 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.577629 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.577635 [0] recv: INFOTS(1711462090.577582984) -1711462090.577640 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #18 L(:1c1 16938.237139) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.577668 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.577674 [0] recv: INFOTS(1711462090.577626247) -1711462090.577680 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #16 L(:1c1 16938.237177) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.577719 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 920 from udp/10.101.12.71:40473 -1711462090.577727 [0] recvMC: INFOTS(1711462090.577649910) -1711462090.577731 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #18 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.577738 [0] recvMC: HEARTBEAT(F#24:18..18 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.577886 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.577890 [0] recv: INFOTS(1711462090.577848971) -1711462090.577895 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #19 L(:1c1 16938.237394) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.577927 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.577930 [0] recv: INFOTS(1711462090.577884615) -1711462090.577935 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #17 L(:1c1 16938.237434) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.577962 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 968 from udp/10.101.12.71:40473 -1711462090.577969 [0] recvMC: INFOTS(1711462090.577896758) -1711462090.577973 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #19 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.577977 [0] recvMC: HEARTBEAT(F#25:19..19 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.578076 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.578082 [0] recv: INFOTS(1711462090.578042902) -1711462090.578087 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #20 L(:1c1 16938.237585) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.578116 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.578123 [0] recv: INFOTS(1711462090.578086523) -1711462090.578128 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #18 L(:1c1 16938.237626) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.578156 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1016 from udp/10.101.12.71:40473 -1711462090.578163 [0] recvMC: INFOTS(1711462090.578098378) -1711462090.578168 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #20 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.578173 [0] recvMC: HEARTBEAT(F#26:20..20 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.578324 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.578329 [0] recv: INFOTS(1711462090.578286739) -1711462090.578333 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #21 L(:1c1 16938.237832) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.578363 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.578372 [0] recv: INFOTS(1711462090.578323981) -1711462090.578378 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #19 L(:1c1 16938.237875) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.578411 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1064 from udp/10.101.12.71:40473 -1711462090.578416 [0] recvMC: INFOTS(1711462090.578342953) -1711462090.578421 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #21 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.578426 [0] recvMC: HEARTBEAT(F#27:21..21 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.578566 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.578571 [0] recv: INFOTS(1711462090.578531397) -1711462090.578575 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #22 L(:1c1 16938.238074) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.578610 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.578618 [0] recv: INFOTS(1711462090.578575730) -1711462090.578622 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #20 L(:1c1 16938.238121) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.578661 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1112 from udp/10.101.12.71:40473 -1711462090.578667 [0] recvMC: INFOTS(1711462090.578589065) -1711462090.578674 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #22 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.578677 [0] recvMC: HEARTBEAT(F#28:22..22 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.578783 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.578787 [0] recv: INFOTS(1711462090.578749181) -1711462090.578793 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #23 L(:1c1 16938.238291) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.578821 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 328 from udp/10.101.12.71:40473 -1711462090.578828 [0] recv: INFOTS(1711462090.578795007) -1711462090.578833 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #21 L(:1c1 16938.238332) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.578865 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1160 from udp/10.101.12.71:40473 -1711462090.578869 [0] recvMC: INFOTS(1711462090.578803795) -1711462090.578876 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #23 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.578879 [0] recvMC: HEARTBEAT(F#29:23..23 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.579032 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.579036 [0] recv: INFOTS(1711462090.578997155) -1711462090.579042 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #24 L(:1c1 16938.238540) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.579078 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.579083 [0] recv: INFOTS(1711462090.579038346) -1711462090.579090 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #22 L(:1c1 16938.238587) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.579116 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1208 from udp/10.101.12.71:40473 -1711462090.579120 [0] recvMC: INFOTS(1711462090.579057465) -1711462090.579127 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #24 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.579131 [0] recvMC: HEARTBEAT(F#30:24..24 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.579281 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.579315 [0] recv: INFOTS(1711462090.579246849) -1711462090.579324 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #25 L(:1c1 16938.238818) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.579369 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 328 from udp/10.101.12.71:40473 -1711462090.579375 [0] recv: INFOTS(1711462090.579338744) -1711462090.579381 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #23 L(:1c1 16938.238878) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.579403 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1256 from udp/10.101.12.71:40473 -1711462090.579407 [0] recvMC: INFOTS(1711462090.579348653) -1711462090.579411 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #25 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.579416 [0] recvMC: HEARTBEAT(F#31:25..25 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.579565 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.579571 [0] recv: INFOTS(1711462090.579526903) -1711462090.579576 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #26 L(:1c1 16938.239075) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.579610 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.579613 [0] recv: INFOTS(1711462090.579582890) -1711462090.579617 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #24 L(:1c1 16938.239117) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.579654 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1304 from udp/10.101.12.71:40473 -1711462090.579660 [0] recvMC: INFOTS(1711462090.579592017) -1711462090.579665 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #26 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.579674 [0] recvMC: HEARTBEAT(F#32:26..26 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.579762 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.579763 [0] recv: INFOTS(1711462090.579726226) -1711462090.579767 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #27 L(:1c1 16938.239267) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.579805 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.579812 [0] recv: INFOTS(1711462090.579761832) -1711462090.579818 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #25 L(:1c1 16938.239316) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.579848 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1352 from udp/10.101.12.71:40473 -1711462090.579857 [0] recvMC: INFOTS(1711462090.579784458) -1711462090.579863 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #27 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.579868 [0] recvMC: HEARTBEAT(F#33:27..27 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580008 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.580014 [0] recv: INFOTS(1711462090.579973596) -1711462090.580020 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #28 L(:1c1 16938.239518) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580055 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.580061 [0] recv: INFOTS(1711462090.580027501) -1711462090.580067 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #26 L(:1c1 16938.239565) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580097 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1400 from udp/10.101.12.71:40473 -1711462090.580103 [0] recvMC: INFOTS(1711462090.580038123) -1711462090.580108 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #28 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580117 [0] recvMC: HEARTBEAT(F#34:28..28 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580221 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.580226 [0] recv: INFOTS(1711462090.580189530) -1711462090.580232 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #29 L(:1c1 16938.239730) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580266 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.580268 [0] recv: INFOTS(1711462090.580241067) -1711462090.580273 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #27 L(:1c1 16938.239772) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580310 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1448 from udp/10.101.12.71:40473 -1711462090.580319 [0] recvMC: INFOTS(1711462090.580250371) -1711462090.580323 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #29 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580327 [0] recvMC: HEARTBEAT(F#35:29..29 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580426 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.580427 [0] recv: INFOTS(1711462090.580393021) -1711462090.580431 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #30 L(:1c1 16938.239932) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580466 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.580468 [0] recv: INFOTS(1711462090.580434684) -1711462090.580472 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #28 L(:1c1 16938.239972) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580520 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1496 from udp/10.101.12.71:40473 -1711462090.580526 [0] recvMC: INFOTS(1711462090.580451532) -1711462090.580534 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #30 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580539 [0] recvMC: HEARTBEAT(F#36:30..30 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580692 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.580694 [0] recv: INFOTS(1711462090.580658980) -1711462090.580697 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #31 L(:1c1 16938.240198) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580740 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.580742 [0] recv: INFOTS(1711462090.580705816) -1711462090.580751 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #29 L(:1c1 16938.240246) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580789 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1544 from udp/10.101.12.71:40473 -1711462090.580797 [0] recvMC: INFOTS(1711462090.580722130) -1711462090.580802 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #31 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580806 [0] recvMC: HEARTBEAT(F#37:31..31 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580903 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.580908 [0] recv: INFOTS(1711462090.580867994) -1711462090.580915 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #32 L(:1c1 16938.240412) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580944 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.580950 [0] recv: INFOTS(1711462090.580901717) -1711462090.580955 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #30 L(:1c1 16938.240454) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.580989 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1592 from udp/10.101.12.71:40473 -1711462090.580990 [0] recvMC: INFOTS(1711462090.580918119) -1711462090.580992 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #32 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.580996 [0] recvMC: HEARTBEAT(F#38:32..32 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.581196 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.581200 [0] recv: INFOTS(1711462090.581146130) -1711462090.581205 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #33 L(:1c1 16938.240704) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.581236 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.581240 [0] recv: INFOTS(1711462090.581210847) -1711462090.581245 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #31 L(:1c1 16938.240744) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.581278 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1640 from udp/10.101.12.71:40473 -1711462090.581280 [0] recvMC: INFOTS(1711462090.581220310) -1711462090.581283 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #33 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.581285 [0] recvMC: HEARTBEAT(F#39:33..33 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.581299 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581315 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c002d38:44 [ udp/10.101.12.71:7412@2 ] -1711462090.581316 [0] tev: traffic-xmit (1) 64 -1711462090.581319 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581323 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c002d38:44 [ udp/10.101.12.71:7412@2 ] -1711462090.581324 [0] tev: traffic-xmit (1) 64 -1711462090.581362 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 68 from udp/10.101.12.71:58212 -1711462090.581366 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.581372 [0] recv: HEARTBEAT(#7:1..10 L(:1c1 16938.240870)110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:3c7 #10) 110aba1:7b19badd:ce0adb73:3c7@0) -1711462090.581376 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#1:1/10:1111111111 -1711462090.581378 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462090.581379 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 68 from udp/10.101.12.71:58212 -1711462090.581382 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.581384 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.581388 [0] recv: HEARTBEAT(#7:1..8 L(:1c1 16938.240888)110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:4c7 #8) 110aba1:7b19badd:ce0adb73:4c7@0) -1711462090.581390 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x77040c002d38:48 [ udp/10.101.12.71:7412@2 ] -1711462090.581391 [0] tev: traffic-xmit (1) 68 -1711462090.581394 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#1:1/8:11111111 -1711462090.581395 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462090.581397 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.581399 [0] tev: xpack_addmsg 0x770414001390 0x77040c002dd0 0(control): niov 2 sz 68 => now niov 3 sz 96 -1711462090.581405 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x77040c002d38:48 0x77040c002ec8:28 [ udp/10.101.12.71:7412@2 ] -1711462090.581406 [0] tev: traffic-xmit (1) 96 -1711462090.581462 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.581467 [0] recv: INFOTS(1711462090.581430143) -1711462090.581472 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #34 L(:1c1 16938.240971) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.581490 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581493 [0] tev: xpack_addmsg 0x770414001390 0x77040c002dd0 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.581497 [0] tev: nn_xpack_send 92: 0x77041400139c:20 0x77040c002d38:44 0x77040c002ec8:28 [ udp/10.101.12.71:7412@2 ] -1711462090.581499 [0] tev: traffic-xmit (1) 92 -1711462090.581517 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.581522 [0] recv: INFOTS(1711462090.581483917) -1711462090.581529 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #32 L(:1c1 16938.241026) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.581534 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 1316 from udp/10.101.12.71:58212 -1711462090.581538 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.581541 [0] recv: INFOTS(1711462090.520347756) -1711462090.581547 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #1 L(:1c1 16938.241042)) -1711462090.581549 [0] recv: INFOTS(1711462090.521064613) -1711462090.581552 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #2) -1711462090.581553 [0] recv: INFOTS(1711462090.539011681) -1711462090.581555 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #3) -1711462090.581557 [0] recv: INFOTS(1711462090.539559928) -1711462090.581559 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #4) -1711462090.581561 [0] recv: INFOTS(1711462090.539806527) -1711462090.581563 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #5) -1711462090.581566 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1688 from udp/10.101.12.71:40473 -1711462090.581566 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #1: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581575 [0] recvMC: INFOTS(1711462090.581502801) -1711462090.581581 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #34 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.581585 [0] recvMC: HEARTBEAT(F#40:34..34 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.581602 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:403 reliable transient-local writer unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581608 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 1316 from udp/10.101.12.71:58212 -1711462090.581612 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.581614 [0] recv: INFOTS(1711462090.540014801) -1711462090.581616 [0] dq.builtin: => EVERYONE -1711462090.581617 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #6 L(:1c1 16938.241116)) -1711462090.581621 [0] recv: INFOTS(1711462090.540228660) -1711462090.581623 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #7) -1711462090.581629 [0] recv: INFOTS(1711462090.540493479) -1711462090.581623 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:403) scanning all rds of topic ros_discovery_info -1711462090.581631 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #8) -1711462090.581635 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:504 init_acknack_count = 1 -1711462090.581635 [0] recv: INFOTS(1711462090.540726396) -1711462090.581648 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #9) -1711462090.581645 [0] dq.builtin: reader_add_connection(pwr 110e21c:d0762c48:a9f0fb28:403 rd 110aba1:7b19badd:ce0adb73:504) -1711462090.581670 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581673 [0] dq.builtin: proxy_writer_add_connection(pwr 110e21c:d0762c48:a9f0fb28:403 rd 110aba1:7b19badd:ce0adb73:504) - out-of-sync -1711462090.581675 [0] tev: xpack_addmsg 0x770414001390 0x77040c002dd0 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.581680 [0] tev: nn_xpack_send 92: 0x77041400139c:20 0x77040c002d38:44 0x77040c002ec8:28 [ udp/10.101.12.71:7416@2 ] -1711462090.581681 [0] tev: traffic-xmit (1) 92 -1711462090.581684 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #2: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",durability=1,durability_service=0:0:1000:-1:-1:-1,reliability=1:9223372036854775807,lifespan=10000000000,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581700 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:603 reliable transient-local writer unnamed: (default).rt/rosout/rcl_interfaces::msg::dds_::Log_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=0<>,topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1000:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=10000000000,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581708 [0] dq.builtin: => EVERYONE -1711462090.581712 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:603) scanning all rds of topic rt/rosout -1711462090.581719 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581732 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581736 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 1404 from udp/10.101.12.71:58212 -1711462090.581741 [0] dq.builtin: => EVERYONE -1711462090.581741 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.581746 [0] recv: INFOTS(1711462090.542833137) -1711462090.581746 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:703) scanning all rds of topic rt/parameter_events -1711462090.581750 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #10 L(:1c1 16938.241245) msr_in_sync(110aba1:7b19badd:ce0adb73:3c7 out-of-sync to tlcatchup)) -1711462090.581756 [0] recv: HEARTBEAT(#8:1..10 110e21c:d0762c48:a9f0fb28:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: 110aba1:7b19badd:ce0adb73:3c7@10(sync)) -1711462090.581758 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.1;">,topic_name="rr/topological_transform_publisher/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581759 [0] recv: INFOTS(1711462090.520486974) -1711462090.581765 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #1) -1711462090.581770 [0] recv: INFOTS(1711462090.539611271) -1711462090.581774 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:803 reliable volatile writer unnamed: (default).rr/topological_transform_publisher/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.1;">,topic_name="rr/topological_transform_publisher/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581772 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581779 [0] dq.builtin: => EVERYONE -1711462090.581774 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #2) -1711462090.581780 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#2:11/0: -1711462090.581783 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:803) scanning all rds of topic rr/topological_transform_publisher/describe_parametersReply -1711462090.581795 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462090.581790 [0] recv: INFOTS(1711462090.539853097) -1711462090.581803 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.2;">,topic_name="rr/topological_transform_publisher/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581806 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #3) -1711462090.581812 [0] recv: INFOTS(1711462090.540055318) -1711462090.581809 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c002d38:44 [ udp/10.101.12.71:7416@2 ] -1711462090.581816 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #4) -1711462090.581819 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:a03 reliable volatile writer unnamed: (default).rr/topological_transform_publisher/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.2;">,topic_name="rr/topological_transform_publisher/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581824 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 100 from udp/10.101.12.71:44991 -1711462090.581828 [0] dq.builtin: => EVERYONE -1711462090.581830 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.581833 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:a03) scanning all rds of topic rr/topological_transform_publisher/get_parametersReply -1711462090.581841 [0] recv: HEARTBEAT(#5:1..10 L(:1c1 16938.241333)110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:3c7 #10) 110aba1:7b19badd:ce0adb73:3c7@0) -1711462090.581844 [0] tev: traffic-xmit (1) 64 -1711462090.581847 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.3;">,topic_name="rr/topological_transform_publisher/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581849 [0] tev: xpack_addmsg 0x770414001390 0x77040c002dd0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581848 [0] recv: HEARTBEAT(#5:1..7 110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:4c7 #7) 110aba1:7b19badd:ce0adb73:4c7@0) -1711462090.581858 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 328 from udp/10.101.12.71:40473 -1711462090.581863 [0] recv: INFOTS(1711462090.581730334) -1711462090.581863 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:c03 reliable volatile writer unnamed: (default).rr/topological_transform_publisher/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.3;">,topic_name="rr/topological_transform_publisher/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581867 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c002eb8:44 [ udp/10.101.12.71:7412@2 ] -1711462090.581869 [0] dq.builtin: => EVERYONE -1711462090.581871 [0] tev: traffic-xmit (1) 64 -1711462090.581883 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581888 [0] tev: xpack_addmsg 0x770414001390 0x77040c002dd0 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.581865 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1736 from udp/10.101.12.71:40473 -1711462090.581879 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:c03) scanning all rds of topic rr/topological_transform_publisher/get_parameter_typesReply -1711462090.581895 [0] recvMC: INFOTS(1711462090.581796803) -1711462090.581897 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #35 L(:1c1 16938.241367) 110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.581893 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#1:1/10:1111111111 -1711462090.581901 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #35 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.581903 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462090.581902 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 1288 from udp/10.101.12.71:58212 -1711462090.581904 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.4;">,topic_name="rr/topological_transform_publisher/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581907 [0] tev: xpack_addmsg 0x770414001390 0x77040c003120 0(control): niov 3 sz 92 => now niov 4 sz 124 -1711462090.581908 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.581914 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#1:1/7:1111111 -1711462090.581906 [0] recvMC: HEARTBEAT(F#41:35..35 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.581917 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462090.581915 [0] recv: INFOTS(1711462090.540298409) -1711462090.581920 [0] tev: xpack_addmsg 0x770414001390 0x77040c002fa0 0(control): niov 4 sz 124 => now niov 5 sz 156 -1711462090.581922 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #5 L(:1c1 16938.241412)) -1711462090.581924 [0] recv: INFOTS(1711462090.540543814) -1711462090.581923 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:e03 reliable volatile writer unnamed: (default).rr/topological_transform_publisher/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.4;">,topic_name="rr/topological_transform_publisher/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581927 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #6) -1711462090.581929 [0] recv: INFOTS(1711462090.540764980) -1711462090.581928 [0] tev: nn_xpack_send 156: 0x77041400139c:20 0x77040c002d38:44 0x77040c002ec8:28 0x77040c003218:32 0x77040c003098:32 [ udp/10.101.12.71:7416@2 ] -1711462090.581931 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #7) -1711462090.581936 [0] tev: traffic-xmit (1) 156 -1711462090.581938 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581939 [0] recv: INFOTS(1711462090.543211087) -1711462090.581942 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #8 msr_in_sync(110aba1:7b19badd:ce0adb73:4c7 out-of-sync to tlcatchup)) -1711462090.581943 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c0033d8:44 [ udp/10.101.12.71:7414@2 ] -1711462090.581944 [0] tev: traffic-xmit (1) 64 -1711462090.581946 [0] recv: HEARTBEAT(#8:1..8 110e21c:d0762c48:a9f0fb28:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: 110aba1:7b19badd:ce0adb73:4c7@8(sync)) -1711462090.581939 [0] dq.builtin: => EVERYONE -1711462090.581947 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#2:9/0: -1711462090.581951 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462090.581953 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.581958 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#1:1/1:1 -1711462090.581949 [0] recv: HEARTBEAT(#7:1..1 110e21c:d0762c48:a9f0fb28:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:200c7 #1) 110aba1:7b19badd:ce0adb73:200c7@0) -1711462090.581959 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462090.581960 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:e03) scanning all rds of topic rr/topological_transform_publisher/list_parametersReply -1711462090.581962 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 2 sz 64 => now niov 3 sz 96 -1711462090.581967 [0] recv: HEARTBEAT(#4:1..0 110e21c:d0762c48:a9f0fb28:300c3 -> 110aba1:7b19badd:ce0adb73:300c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:300c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:300c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:300c4@0(sync)) -1711462090.581968 [0] tev: acknack 110aba1:7b19badd:ce0adb73:300c4 -> 110e21c:d0762c48:a9f0fb28:300c3: F#1:1/0: -1711462090.581970 [0] recv: HEARTBEAT(#4:1..0 110e21c:d0762c48:a9f0fb28:301c3 -> 110aba1:7b19badd:ce0adb73:301c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:301c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:301c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:301c4@0(sync)) -1711462090.581971 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:300c4 -> pwr 110e21c:d0762c48:a9f0fb28:300c3) -1711462090.581969 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.5;">,topic_name="rr/topological_transform_publisher/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581973 [0] tev: xpack_addmsg 0x770414001390 0x77040c002dd0 0(control): niov 3 sz 96 => now niov 4 sz 124 -1711462090.581973 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.581978 [0] tev: acknack 110aba1:7b19badd:ce0adb73:301c4 -> 110e21c:d0762c48:a9f0fb28:301c3: F#1:1/0: -1711462090.581979 [0] recv: INFOTS(1711462090.581783677) -1711462090.581980 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:301c4 -> pwr 110e21c:d0762c48:a9f0fb28:301c3) -1711462090.581982 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #33 L(:1c1 16938.241483) 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: no heartbeat seen yet) -1711462090.581985 [0] tev: xpack_addmsg 0x770414001390 0x77040c003120 0(control): niov 4 sz 124 => now niov 5 sz 152 -1711462090.581988 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:1003 reliable volatile writer unnamed: (default).rr/topological_transform_publisher/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.5;">,topic_name="rr/topological_transform_publisher/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581991 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 68 from udp/10.101.12.71:44991 -1711462090.581995 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.581995 [0] dq.builtin: => EVERYONE -1711462090.581999 [0] recv: HEARTBEAT(#5:1..1 L(:1c1 16938.241499)110443d:bb7f10a5:18901533:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:200c7 #1) 110aba1:7b19badd:ce0adb73:200c7@0) -1711462090.582005 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 68 from udp/10.101.12.71:44991 -1711462090.582006 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:1003) scanning all rds of topic rr/topological_transform_publisher/set_parametersReply -1711462090.582007 [0] tev: nn_xpack_send 152: 0x77041400139c:20 0x77040c0033d8:44 0x77040c002d48:32 0x77040c002ec8:28 0x77040c003218:28 [ udp/10.101.12.71:7412@2 ] -1711462090.582011 [0] tev: traffic-xmit (1) 152 -1711462090.582007 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582015 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.6;">,topic_name="rr/topological_transform_publisher/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582014 [0] tev: xpack_addmsg 0x770414001390 0x77040c002fa0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.582017 [0] recv: HEARTBEAT(#2:1..0 L(:1c1 16938.241511)110443d:bb7f10a5:18901533:300c3 -> 110aba1:7b19badd:ce0adb73:300c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:300c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:300c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:300c4@0(sync)) -1711462090.582021 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#1:1/1:1 -1711462090.582023 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 68 from udp/10.101.12.71:40473 -1711462090.582025 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462090.582025 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582030 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c003088:44 [ udp/10.101.12.71:7414@2 ] -1711462090.582031 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:1203 reliable volatile writer unnamed: (default).rr/topological_transform_publisher/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.6;">,topic_name="rr/topological_transform_publisher/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582035 [0] tev: traffic-xmit (1) 64 -1711462090.582031 [0] recv: HEARTBEAT(#6:1..35 L(:1c1 16938.241529)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:3c7 #35) 110aba1:7b19badd:ce0adb73:3c7@0) -1711462090.582038 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.582043 [0] dq.builtin: => EVERYONE -1711462090.582043 [0] tev: acknack 110aba1:7b19badd:ce0adb73:300c4 -> 110443d:bb7f10a5:18901533:300c3: F#1:1/0: -1711462090.582050 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:300c4 -> pwr 110443d:bb7f10a5:18901533:300c3) -1711462090.582048 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:1203) scanning all rds of topic rr/topological_transform_publisher/set_parameters_atomicallyReply -1711462090.582053 [0] tev: xpack_addmsg 0x770414001390 0x77040c002fa0 0(control): niov 2 sz 68 => now niov 3 sz 96 -1711462090.582056 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 100 from udp/10.101.12.71:58212 -1711462090.582062 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582059 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#1:1/35:11111111111111111111111111111111111 -1711462090.582064 [0] recv: INFOTS(1711462090.519500902) -1711462090.582066 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #10: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/tf_static",type_name="tf2_msgs::msg::dds_::TFMessage_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:1403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582068 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 #1 L(:1c1 16938.241566) msr_in_sync(110aba1:7b19badd:ce0adb73:200c7 out-of-sync to tlcatchup)) -1711462090.582071 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.582074 [0] recv: INFOTS(1711462090.582034642) -1711462090.582071 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462090.582083 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:1403 reliable transient-local writer unnamed: (default).rt/tf_static/tf2_msgs::msg::dds_::TFMessage_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=0<>,topic_name="rt/tf_static",type_name="tf2_msgs::msg::dds_::TFMessage_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582084 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #36 L(:1c1 16938.241578)) -1711462090.582086 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x77040c0033d8:48 0x77040c003098:28 [ udp/10.101.12.71:7416@2 ] -1711462090.582090 [0] tev: traffic-xmit (1) 96 -1711462090.582092 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 72 -1711462090.582093 [0] dq.builtin: => EVERYONE -1711462090.582097 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 2 sz 72 => now niov 3 sz 100 -1711462090.582095 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 68 from udp/10.101.12.71:58212 -1711462090.582108 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582105 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110e21c:d0762c48:a9f0fb28:1403) scanning all rds of topic rt/tf_static -1711462090.582111 [0] recv: HEARTBEAT(#8:1..1 L(:1c1 16938.241612)110e21c:d0762c48:a9f0fb28:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: 110aba1:7b19badd:ce0adb73:200c7@1(sync)) -1711462090.582105 [0] tev: nn_xpack_send 100: 0x77041400139c:20 0x77040c002d38:52 0x77040c0033e8:28 [ udp/10.101.12.71:7414@2 ] -1711462090.582117 [0] tev: traffic-xmit (1) 100 -1711462090.582120 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#2:2/0: -1711462090.582128 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462090.582130 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.582120 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 68 from udp/10.101.12.71:40473 -1711462090.582134 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c002d38:44 [ udp/10.101.12.71:7412@2 ] -1711462090.582121 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #1: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,history=1:-1,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:504},adlink_entity_factory=1} -1711462090.582137 [0] tev: traffic-xmit (1) 64 -1711462090.582136 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582148 [0] recv: HEARTBEAT(#6:1..33 L(:1c1 16938.241640)110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:4c7 #33) 110aba1:7b19badd:ce0adb73:4c7@0) -1711462090.582153 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#1:1/33:111111111111111111111111111111111 -1711462090.582155 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462090.582157 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 72 -1711462090.582155 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:504 reliable transient-local reader unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=1:-1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582159 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 2 sz 72 => now niov 3 sz 100 -1711462090.582164 [0] dq.builtin: => EVERYONE -1711462090.582164 [0] tev: nn_xpack_send 100: 0x77041400139c:20 0x77040c002d38:52 0x77040c0033e8:28 [ udp/10.101.12.71:7414@2 ] -1711462090.582169 [0] tev: traffic-xmit (1) 100 -1711462090.582169 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:504) scanning all wrs of topic ros_discovery_info -1711462090.582175 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1784 from udp/10.101.12.71:40473 -1711462090.582176 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110e21c:d0762c48:a9f0fb28:504) -1711462090.582177 [0] recvMC: INFOTS(1711462090.582110521) -1711462090.582180 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110e21c:d0762c48:a9f0fb28:504) - ack seq 2 -1711462090.582184 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #36 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.582189 [0] recvMC: HEARTBEAT(F#42:36..36 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.582182 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 1324 from udp/10.101.12.71:44991 -1711462090.582196 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582200 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 -1711462090.582203 [0] recv: HEARTBEAT(#2:1..0 L(:1c1 16938.241700)110443d:bb7f10a5:18901533:301c3 -> 110aba1:7b19badd:ce0adb73:301c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:301c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:301c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:301c4@0(sync)) -1711462090.582208 [0] recv: INFOTS(1711462090.550118373) -1711462090.582206 [0] tev: acknack 110aba1:7b19badd:ce0adb73:301c4 -> 110443d:bb7f10a5:18901533:301c3: F#1:1/0: -1711462090.582216 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:301c4 -> pwr 110443d:bb7f10a5:18901533:301c3) -1711462090.582221 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.582212 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #1) -1711462090.582204 [0] dq.builtin: reduced nlocs=2 -1711462090.582225 [0] recv: INFOTS(1711462090.551060212) -1711462090.582225 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c002d38:44 [ udp/10.101.12.71:7416@2 ] -1711462090.582230 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #2) -1711462090.582235 [0] recv: INFOTS(1711462090.567575352) -1711462090.582238 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #3) -1711462090.582240 [0] recv: INFOTS(1711462090.568216939) -1711462090.582243 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #4) -1711462090.582245 [0] recv: INFOTS(1711462090.568522519) -1711462090.582230 [0] tev: traffic-xmit (1) 64 -1711462090.582230 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.582247 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #5) -1711462090.582253 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462090.582249 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.582257 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 340 from udp/10.101.12.71:40473 -1711462090.582261 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 1 -> 8 -1711462090.582265 [0] recv: INFOTS(1711462090.582090581) -1711462090.582266 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c0033d8:44 [ udp/10.101.12.71:7414@2 ] -1711462090.582274 [0] tev: traffic-xmit (1) 64 -1711462090.582272 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #34 L(:1c1 16938.241769)) -1711462090.582269 [0] dq.builtin: a -1711462090.582292 [0] dq.builtin: loc 0 = udp/10.101.12.71:7413@2 1 { +u } -1711462090.582297 [0] dq.builtin: loc 1 = udp/239.255.0.1:7401@2 2 { +1 } -1711462090.582298 [0] dq.builtin: best = 0 -1711462090.582301 [0] dq.builtin: simple udp/10.101.12.71:7413@2 -1711462090.582305 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:403): udp/10.101.12.71:7413@2 (burst size 4294901760 rexmit 1048576) -1711462090.582306 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 1452 from udp/10.101.12.71:44991 -1711462090.582311 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582312 [0] recv: INFOTS(1711462090.568841289) -1711462090.582314 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #6 L(:1c1 16938.241815)) -1711462090.582316 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.1;">,topic_name="rq/topological_transform_publisher/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:904},adlink_entity_factory=1} -1711462090.582317 [0] recv: INFOTS(1711462090.569135598) -1711462090.582323 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #7) -1711462090.582324 [0] recv: INFOTS(1711462090.569441604) -1711462090.582327 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #8) -1711462090.582328 [0] recv: INFOTS(1711462090.569710842) -1711462090.582330 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #9) -1711462090.582331 [0] recv: INFOTS(1711462090.571973111) -1711462090.582333 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #10 msr_in_sync(110aba1:7b19badd:ce0adb73:3c7 out-of-sync to tlcatchup)) -1711462090.582336 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 1444 from udp/10.101.12.71:44991 -1711462090.582336 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:904 reliable volatile reader unnamed: (default).rq/topological_transform_publisher/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.1;">,topic_name="rq/topological_transform_publisher/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582337 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582341 [0] recv: INFOTS(1711462090.550298409) -1711462090.582343 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #1 L(:1c1 16938.241841)) -1711462090.582345 [0] recv: INFOTS(1711462090.568255806) -1711462090.582346 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #2) -1711462090.582349 [0] recv: INFOTS(1711462090.568565213) -1711462090.582347 [0] dq.builtin: => EVERYONE -1711462090.582351 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #3) -1711462090.582354 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:904) scanning all wrs of topic rq/topological_transform_publisher/describe_parametersRequest -1711462090.582354 [0] recv: INFOTS(1711462090.568893800) -1711462090.582359 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #4) -1711462090.582360 [0] recv: INFOTS(1711462090.569197394) -1711462090.582362 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #5) -1711462090.582364 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.2;">,topic_name="rq/topological_transform_publisher/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:b04},adlink_entity_factory=1} -1711462090.582366 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 800 from udp/10.101.12.71:44991 -1711462090.582371 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582373 [0] recv: INFOTS(1711462090.569489785) -1711462090.582375 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #6 L(:1c1 16938.241875)) -1711462090.582376 [0] recv: INFOTS(1711462090.569761244) -1711462090.582378 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #7 msr_in_sync(110aba1:7b19badd:ce0adb73:4c7 out-of-sync to tlcatchup)) -1711462090.582381 [0] recv: HEARTBEAT(#6:1..10 110443d:bb7f10a5:18901533:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: 110aba1:7b19badd:ce0adb73:3c7@10(sync)) -1711462090.582384 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#2:11/0: -1711462090.582384 [0] recv: HEARTBEAT(#6:1..7 110443d:bb7f10a5:18901533:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: 110aba1:7b19badd:ce0adb73:4c7@7(sync)) -1711462090.582386 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462090.582388 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.582391 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#2:8/0: -1711462090.582392 [0] recv: INFOTS(1711462090.549382637) -1711462090.582392 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462090.582395 [0] recv: DATA(110443d:bb7f10a5:18901533:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 #1 msr_in_sync(110aba1:7b19badd:ce0adb73:200c7 out-of-sync to tlcatchup)) -1711462090.582384 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:b04 reliable volatile reader unnamed: (default).rq/topological_transform_publisher/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.2;">,topic_name="rq/topological_transform_publisher/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582397 [0] recv: HEARTBEAT(#6:1..1 110443d:bb7f10a5:18901533:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: 110aba1:7b19badd:ce0adb73:200c7@1(sync)) -1711462090.582401 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.582402 [0] dq.builtin: => EVERYONE -1711462090.582406 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#2:2/0: -1711462090.582406 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:b04) scanning all wrs of topic rq/topological_transform_publisher/get_parametersRequest -1711462090.582409 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462090.582411 [0] tev: xpack_addmsg 0x770414001390 0x77040c002fa0 0(control): niov 3 sz 92 => now niov 4 sz 120 -1711462090.582415 [0] tev: nn_xpack_send 120: 0x77041400139c:20 0x77040c0033d8:44 0x77040c002d48:28 0x77040c003098:28 [ udp/10.101.12.71:7416@2 ] -1711462090.582417 [0] tev: traffic-xmit (1) 120 -1711462090.582419 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.3;">,topic_name="rq/topological_transform_publisher/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:d04},adlink_entity_factory=1} -1711462090.582442 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:d04 reliable volatile reader unnamed: (default).rq/topological_transform_publisher/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.3;">,topic_name="rq/topological_transform_publisher/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582454 [0] dq.builtin: => EVERYONE -1711462090.582458 [0] gc: gc 0x77041c025520: not yet, shortsleep -1711462090.582459 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:d04) scanning all wrs of topic rq/topological_transform_publisher/get_parameter_typesRequest -1711462090.582466 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1276 from udp/10.101.12.71:40473 -1711462090.582469 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582471 [0] recv: INFOTS(1711462090.542116783) -1711462090.582471 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.4;">,topic_name="rq/topological_transform_publisher/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:f04},adlink_entity_factory=1} -1711462090.582474 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #1 L(:1c1 16938.241973)) -1711462090.582476 [0] recv: INFOTS(1711462090.542538066) -1711462090.582477 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #2) -1711462090.582479 [0] recv: INFOTS(1711462090.557958448) -1711462090.582482 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #3) -1711462090.582483 [0] recv: INFOTS(1711462090.558506480) -1711462090.582487 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #4) -1711462090.582488 [0] recv: INFOTS(1711462090.558775861) -1711462090.582490 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #5) -1711462090.582490 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:f04 reliable volatile reader unnamed: (default).rq/topological_transform_publisher/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.4;">,topic_name="rq/topological_transform_publisher/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582502 [0] dq.builtin: => EVERYONE -1711462090.582506 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:f04) scanning all wrs of topic rq/topological_transform_publisher/list_parametersRequest -1711462090.582514 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.5;">,topic_name="rq/topological_transform_publisher/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:1104},adlink_entity_factory=1} -1711462090.582530 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:1104 reliable volatile reader unnamed: (default).rq/topological_transform_publisher/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.5;">,topic_name="rq/topological_transform_publisher/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582534 [0] dq.builtin: => EVERYONE -1711462090.582538 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:1104) scanning all wrs of topic rq/topological_transform_publisher/set_parametersRequest -1711462090.582542 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1832 from udp/10.101.12.71:40473 -1711462090.582545 [0] recvMC: INFOTS(1711462090.582480087) -1711462090.582545 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.6;">,topic_name="rq/topological_transform_publisher/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:1304},adlink_entity_factory=1} -1711462090.582551 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #37 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.582555 [0] recvMC: HEARTBEAT(F#43:37..37 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.582570 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:1304 reliable volatile reader unnamed: (default).rq/topological_transform_publisher/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.e2.1c.d0.76.2c.48.a9.f0.fb.28.0.0.0.6;">,topic_name="rq/topological_transform_publisher/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582579 [0] dq.builtin: => EVERYONE -1711462090.582583 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:1304) scanning all wrs of topic rq/topological_transform_publisher/set_parameters_atomicallyRequest -1711462090.582590 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110e21c:d0762c48:a9f0fb28:1504},adlink_entity_factory=1} -1711462090.582605 [0] dq.builtin: SEDP ST0 110e21c:d0762c48:a9f0fb28:1504 reliable transient-local reader unnamed: (default).rt/topological_map_2/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 ssm=0) QOS={user_data=0<>,topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582611 [0] dq.builtin: => EVERYONE -1711462090.582615 [0] dq.builtin: match_proxy_reader_with_writers(prd 110e21c:d0762c48:a9f0fb28:1504) scanning all wrs of topic rt/topological_map_2 -1711462090.582619 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:200c2 #1: ST0 DCPSParticipantMessage/ParticipantMessageData:{110e21c:d0762c48:a9f0fb28:1}:1<0> -1711462090.582623 [0] dq.builtin: PMD ST0 pp 110e21c:d0762c48:a9f0fb28 kind 1 data 1 -1711462090.582632 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #36: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rr/topological_map_manager2/update_fail_policyReply",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582649 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_fail_policyReply/topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rr/topological_map_manager2/update_fail_policyReply",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582657 [0] dq.builtin: => EVERYONE -1711462090.582660 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4803) scanning all rds of topic rr/topological_map_manager2/update_fail_policyReply -1711462090.582668 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #1: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582685 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:403 reliable transient-local writer unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582690 [0] dq.builtin: => EVERYONE -1711462090.582693 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:403) scanning all rds of topic ros_discovery_info -1711462090.582697 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:504 init_acknack_count = 1 -1711462090.582698 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1236 from udp/10.101.12.71:40473 -1711462090.582703 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582700 [0] dq.builtin: reader_add_connection(pwr 110443d:bb7f10a5:18901533:403 rd 110aba1:7b19badd:ce0adb73:504) -1711462090.582705 [0] recv: INFOTS(1711462090.559026057) -1711462090.582713 [0] dq.builtin: proxy_writer_add_connection(pwr 110443d:bb7f10a5:18901533:403 rd 110aba1:7b19badd:ce0adb73:504) - out-of-sync -1711462090.582715 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #6 L(:1c1 16938.242207)) -1711462090.582718 [0] recv: INFOTS(1711462090.559364009) -1711462090.582720 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #7) -1711462090.582721 [0] recv: INFOTS(1711462090.559658827) -1711462090.582722 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #8) -1711462090.582724 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #2: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",durability=1,durability_service=0:0:1000:-1:-1:-1,reliability=1:9223372036854775807,lifespan=10000000000,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582724 [0] recv: INFOTS(1711462090.559900367) -1711462090.582729 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #9) -1711462090.582740 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:603 reliable transient-local writer unnamed: (default).rt/rosout/rcl_interfaces::msg::dds_::Log_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=0<>,topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1000:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=10000000000,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582748 [0] dq.builtin: => EVERYONE -1711462090.582752 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:603) scanning all rds of topic rt/rosout -1711462090.582756 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1284 from udp/10.101.12.71:40473 -1711462090.582757 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582760 [0] recv: INFOTS(1711462090.573532356) -1711462090.582758 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582765 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #10 L(:1c1 16938.242261)) -1711462090.582770 [0] recv: INFOTS(1711462090.575442461) -1711462090.582772 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #11) -1711462090.582774 [0] recv: INFOTS(1711462090.575726889) -1711462090.582775 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #12) -1711462090.582776 [0] recv: INFOTS(1711462090.575995325) -1711462090.582779 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #13) -1711462090.582782 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582782 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 372 from udp/10.101.12.71:40473 -1711462090.582790 [0] dq.builtin: => EVERYONE -1711462090.582791 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582795 [0] recv: INFOTS(1711462090.576266665) -1711462090.582797 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #14 L(:1c1 16938.242295)) -1711462090.582797 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:703) scanning all rds of topic rt/parameter_events -1711462090.582811 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.1;">,topic_name="rr/static_transformer/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582820 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462090.582822 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582824 [0] recvUC: ACKNACK(#0:1/0: 110d7b4:17c5b8c5:94bd0ff4:504? -> 110aba1:7b19badd:ce0adb73:403) -1711462090.582828 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:803 reliable volatile writer unnamed: (default).rr/static_transformer/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.1;">,topic_name="rr/static_transformer/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582868 [0] dq.builtin: => EVERYONE -1711462090.582872 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:803) scanning all rds of topic rr/static_transformer/describe_parametersReply -1711462090.582880 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1344 from udp/10.101.12.71:40473 -1711462090.582881 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.2;">,topic_name="rr/static_transformer/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582882 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582889 [0] recv: INFOTS(1711462090.576532468) -1711462090.582887 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1880 from udp/10.101.12.71:40473 -1711462090.582902 [0] recvMC: INFOTS(1711462090.582807206) -1711462090.582899 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:a03 reliable volatile writer unnamed: (default).rr/static_transformer/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.2;">,topic_name="rr/static_transformer/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582904 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #38 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.582898 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #15 L(:1c1 16938.242386)) -1711462090.582909 [0] recv: INFOTS(1711462090.576742682) -1711462090.582910 [0] recvMC: HEARTBEAT(F#44:38..38 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.582914 [0] dq.builtin: => EVERYONE -1711462090.582914 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #16) -1711462090.582921 [0] recv: INFOTS(1711462090.577330990) -1711462090.582923 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #17) -1711462090.582925 [0] recv: INFOTS(1711462090.577582984) -1711462090.582918 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:a03) scanning all rds of topic rr/static_transformer/get_parametersReply -1711462090.582927 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #18) -1711462090.582932 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1320 from udp/10.101.12.71:40473 -1711462090.582934 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582937 [0] recv: INFOTS(1711462090.577848971) -1711462090.582935 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #34: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rq/topological_map_manager2/update_fail_policyRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4904},adlink_entity_factory=1} -1711462090.582939 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #19 L(:1c1 16938.242439)) -1711462090.582945 [0] recv: INFOTS(1711462090.578042902) -1711462090.582947 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #20) -1711462090.582949 [0] recv: INFOTS(1711462090.578286739) -1711462090.582950 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #21) -1711462090.582952 [0] recv: INFOTS(1711462090.578531397) -1711462090.582954 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #22) -1711462090.582957 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_fail_policyRequest/topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rq/topological_map_manager2/update_fail_policyRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582965 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.582965 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1316 from udp/10.101.12.71:40473 -1711462090.582971 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582968 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.582972 [0] recv: INFOTS(1711462090.578749181) -1711462090.582975 [0] recvUC: ACKNACK(#0:1/0: 110443d:bb7f10a5:18901533:504? -> 110aba1:7b19badd:ce0adb73:403) -1711462090.582968 [0] dq.builtin: => EVERYONE -1711462090.582981 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4904) scanning all wrs of topic rq/topological_map_manager2/update_fail_policyRequest -1711462090.582981 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #23 L(:1c1 16938.242475)) -1711462090.582985 [0] recv: INFOTS(1711462090.578997155) -1711462090.582987 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #24) -1711462090.582988 [0] recv: INFOTS(1711462090.579246849) -1711462090.582989 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #25) -1711462090.582992 [0] recv: INFOTS(1711462090.579526903) -1711462090.582990 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.3;">,topic_name="rr/static_transformer/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582994 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #26) -1711462090.582999 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1352 from udp/10.101.12.71:40473 -1711462090.583001 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583003 [0] recv: INFOTS(1711462090.579726226) -1711462090.583005 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #27 L(:1c1 16938.242505)) -1711462090.583006 [0] recv: INFOTS(1711462090.579973596) -1711462090.583008 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #28) -1711462090.583012 [0] recv: INFOTS(1711462090.580189530) -1711462090.583010 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:c03 reliable volatile writer unnamed: (default).rr/static_transformer/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.3;">,topic_name="rr/static_transformer/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583016 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #29) -1711462090.583020 [0] recv: INFOTS(1711462090.580393021) -1711462090.583022 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #30) -1711462090.583024 [0] dq.builtin: => EVERYONE -1711462090.583028 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:c03) scanning all rds of topic rr/static_transformer/get_parameter_typesReply -1711462090.583033 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1340 from udp/10.101.12.71:40473 -1711462090.583034 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583036 [0] recv: INFOTS(1711462090.580658980) -1711462090.583038 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #31 L(:1c1 16938.242538)) -1711462090.583040 [0] recv: INFOTS(1711462090.580867994) -1711462090.583036 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.4;">,topic_name="rr/static_transformer/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583042 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #32) -1711462090.583046 [0] recv: INFOTS(1711462090.581146130) -1711462090.583047 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #33) -1711462090.583049 [0] recv: INFOTS(1711462090.581430143) -1711462090.583051 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #34) -1711462090.583057 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:e03 reliable volatile writer unnamed: (default).rr/static_transformer/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.4;">,topic_name="rr/static_transformer/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583059 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.583062 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583063 [0] dq.builtin: => EVERYONE -1711462090.583063 [0] recv: INFOTS(1711462090.581730334) -1711462090.583073 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #35 L(:1c1 16938.242566) msr_in_sync(110aba1:7b19badd:ce0adb73:3c7 out-of-sync to tlcatchup)) -1711462090.583074 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:e03) scanning all rds of topic rr/static_transformer/list_parametersReply -1711462090.583081 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.5;">,topic_name="rr/static_transformer/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583084 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583087 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.583089 [0] recv: INFOTS(1711462090.582385748) -1711462090.583092 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #37 L(:1c1 16938.242594)) -1711462090.583094 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c0033d8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.583105 [0] tev: traffic-xmit (1) 64 -1711462090.583103 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462090.583116 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583096 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1003 reliable volatile writer unnamed: (default).rr/static_transformer/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.5;">,topic_name="rr/static_transformer/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583120 [0] recvUC: ACKNACK(#0:1/0: L(:1c1 16938.242620) 110e21c:d0762c48:a9f0fb28:504 -> 110aba1:7b19badd:ce0adb73:403 preemptive-nack rebase defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110e21c:d0762c48:a9f0fb28:504 - queue for transmit -1711462090.583122 [0] dq.builtin: => EVERYONE -1711462090.583124 [0] recvUC: non-timed queue now has 1 items -1711462090.583127 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:1003) scanning all rds of topic rr/static_transformer/set_parametersReply -1711462090.583130 [0] recvUC: ) -1711462090.583132 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583138 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c0033d8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.583140 [0] tev: traffic-xmit (1) 64 -1711462090.583142 [0] tev: xpack_addmsg 0x770414001390 0x770404001390 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.583143 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.6;">,topic_name="rr/static_transformer/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583148 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x770404001478:48 [ udp/10.101.12.71:7413@2 ] -1711462090.583150 [0] tev: traffic-xmit (1) 68 -1711462090.583152 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 100 from udp/10.101.12.71:40473 -1711462090.583160 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583161 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1203 reliable volatile writer unnamed: (default).rr/static_transformer/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.6;">,topic_name="rr/static_transformer/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583167 [0] recv: HEARTBEAT(#7:1..36 L(:1c1 16938.242664)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: 110aba1:7b19badd:ce0adb73:3c7@37(sync)) -1711462090.583169 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#2:38/0: -1711462090.583170 [0] recv: HEARTBEAT(#6:1..1 110d7b4:17c5b8c5:94bd0ff4:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:200c7 #1) 110aba1:7b19badd:ce0adb73:200c7@0) -1711462090.583173 [0] dq.builtin: => EVERYONE -1711462090.583175 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.583177 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:1203) scanning all rds of topic rr/static_transformer/set_parameters_atomicallyReply -1711462090.583177 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583171 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462090.583190 [0] tev: xpack_addmsg 0x770414001390 0x770404001390 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583187 [0] recv: HEARTBEAT(#7:1..9 L(:1c1 16938.242681)110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:3c7 #9) 110aba1:7b19badd:ce0adb73:3c7@0) -1711462090.583194 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#1:1/1:1 -1711462090.583187 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #10: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/tf_static",type_name="tf2_msgs::msg::dds_::TFMessage_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583205 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462090.583212 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 2 sz 64 => now niov 3 sz 96 -1711462090.583207 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.583218 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:56a27910:57318171:3c2: F#1:1/9:111111111 -1711462090.583223 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110cd0d:56a27910:57318171:3c2) -1711462090.583223 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1403 reliable transient-local writer unnamed: (default).rt/tf_static/tf2_msgs::msg::dds_::TFMessage_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=0<>,topic_name="rt/tf_static",type_name="tf2_msgs::msg::dds_::TFMessage_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583226 [0] recv: INFOTS(1711462090.582465762) -1711462090.583233 [0] dq.builtin: => EVERYONE -1711462090.583240 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #35 L(:1c1 16938.242729)) -1711462090.583243 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:1403) scanning all rds of topic rt/tf_static -1711462090.583227 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770404001478:44 0x77040c0033e8:32 [ udp/10.101.12.71:7414@2 ] -1711462090.583249 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.583252 [0] tev: traffic-xmit (1) 96 -1711462090.583253 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583255 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.583236 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 68 from udp/10.101.12.71:58212 -1711462090.583259 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1928 from udp/10.101.12.71:40473 -1711462090.583266 [0] recvMC: INFOTS(1711462090.583170500) -1711462090.583269 [0] tev: xpack_addmsg 0x770414001390 0x770404001390 0(control): niov 2 sz 68 => now niov 3 sz 96 -1711462090.583274 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 3 sz 96 => now niov 4 sz 124 -1711462090.583252 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #1: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,history=1:-1,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:504},adlink_entity_factory=1} -1711462090.583262 [0] recv: HEARTBEAT(#7:1..10 L(:1c1 16938.242756)110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:4c7 #10) 110aba1:7b19badd:ce0adb73:4c7@0) -1711462090.583278 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:56a27910:57318171:4c2: F#1:1/10:1111111111 -1711462090.583290 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110cd0d:56a27910:57318171:4c2) -1711462090.583263 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583291 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1416 from udp/10.101.12.71:40473 -1711462090.583292 [0] tev: xpack_addmsg 0x770414001390 0x77040c002fa0 0(control): niov 4 sz 124 => now niov 5 sz 156 -1711462090.583296 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583301 [0] recv: INFOTS(1711462090.542199398) -1711462090.583271 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #39 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.583297 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:504 reliable transient-local reader unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=1:-1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583302 [0] tev: nn_xpack_send 156: 0x77041400139c:20 0x77040c002d38:48 0x770404001488:28 0x77040c0033e8:28 0x77040c003098:32 [ udp/10.101.12.71:7410@2 ] -1711462090.583314 [0] tev: traffic-xmit (1) 156 -1711462090.583312 [0] recvMC: HEARTBEAT(F#45:39..39 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.583312 [0] recvUC: ACKNACK(F#1:2/1:1 L(:1c1 16938.242765) 110e21c:d0762c48:a9f0fb28:504 -> 110aba1:7b19badd:ce0adb73:403 complying RX2non-timed queue now has 1 items -1711462090.583321 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #1 L(:1c1 16938.242799)) -1711462090.583328 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583328 [0] recvUC: rexmit#1 maxseq:2<2<=2defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110e21c:d0762c48:a9f0fb28:504 - queue for transmit -1711462090.583332 [0] recv: INFOTS(1711462090.558560619) -1711462090.583340 [0] recvUC: ) -1711462090.583349 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #2) -1711462090.583357 [0] recvUC: send_deferred_heartbeat: 3aa8b43931dec43 -> 2b8c74ae6267e33e - queue for transmit -1711462090.583343 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 unicasting to prd 110e21c:d0762c48:a9f0fb28:504 (rel-prd 1 seq-eq-max 0 seq 2 maxseq 2) -1711462090.583342 [0] dq.builtin: => EVERYONE -1711462090.583358 [0] recv: INFOTS(1711462090.558823217) -1711462090.583368 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:504) scanning all wrs of topic ros_discovery_info -1711462090.583367 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) sent, resched in 0.1 s (min-ack 2!, avail-seq 2, xmit 2) -1711462090.583374 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110443d:bb7f10a5:18901533:504) -1711462090.583372 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #3) -1711462090.583382 [0] recv: INFOTS(1711462090.559079317) -1711462090.583362 [0] recvUC: non-timed queue now has 2 items -1711462090.583387 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #4) -1711462090.583396 [0] recv: INFOTS(1711462090.559428765) -1711462090.583396 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110443d:bb7f10a5:18901533:504) - ack seq 2 -1711462090.583396 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c002d38:44 [ udp/10.101.12.71:7410@2 ] -1711462090.583404 [0] tev: traffic-xmit (1) 64 -1711462090.583407 [0] tev: xpack_addmsg 0x770414001390 0x770404001390 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.583407 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7417@2 -1711462090.583413 [0] dq.builtin: reduced nlocs=3 -1711462090.583401 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #5) -1711462090.583414 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x770404001478:48 [ udp/10.101.12.71:7413@2 ] -1711462090.583417 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.583417 [0] tev: traffic-xmit (1) 68 -1711462090.583420 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583427 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462090.583427 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c002d38:44 [ udp/10.101.12.71:7410@2 ] -1711462090.583433 [0] tev: traffic-xmit (1) 64 -1711462090.583435 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1232 from udp/10.101.12.71:40473 -1711462090.583430 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462090.583440 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583448 [0] recv: INFOTS(1711462090.559691960) -1711462090.583436 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(rexmit(110aba1:7b19badd:ce0adb73:403:#2/1)): niov 0 sz 0 => now niov 3 sz 176 -1711462090.583450 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.583456 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #6 L(:1c1 16938.242944)) -1711462090.583462 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7417@2 1 -> 0 -1711462090.583465 [0] recv: INFOTS(1711462090.559945133) -1711462090.583471 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #7) -1711462090.583463 [0] tev: nn_xpack_send 176: 0x77041400139c:20 0x770404001648:52 0x653a1b479c90:104 [ udp/10.101.12.71:7413@2 ] -1711462090.583475 [0] recv: INFOTS(1711462090.573575071) -1711462090.583480 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #8) -1711462090.583484 [0] recv: INFOTS(1711462090.575483556) -1711462090.583472 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462090.583489 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #9) -1711462090.583494 [0] dq.builtin: a b -1711462090.583502 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1304 from udp/10.101.12.71:40473 -1711462090.583505 [0] dq.builtin: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. } -1711462090.583507 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583510 [0] dq.builtin: loc 1 = udp/10.101.12.71:7417@2 1 { .. +u } -1711462090.583511 [0] recv: INFOTS(1711462090.575762290) -1711462090.583481 [0] tev: traffic-xmit (1) 176 -1711462090.583517 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583519 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #10 L(:1c1 16938.243010)) -1711462090.583515 [0] dq.builtin: loc 2 = udp/239.255.0.1:7401@2 1 { +1 +1 } -1711462090.583523 [0] recv: INFOTS(1711462090.576028935) -1711462090.583520 [0] gc: gc 0x77041c025520: deleting -1711462090.583521 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.583534 [0] dq.builtin: best = 2 -1711462090.583532 [0] gc: gc 0x77041c02c7c0: deleting -1711462090.583537 [0] dq.builtin: simple udp/239.255.0.1:7401@2 -1711462090.583528 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #11) -1711462090.583538 [0] tev: nn_xpack_send 92: 0x77041400139c:20 0x77040c002d38:44 0x770404001658:28 [ udp/10.101.12.71:7410@2 ] -1711462090.583555 [0] tev: traffic-xmit (1) 92 -1711462090.583547 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462090.583558 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.583551 [0] recv: INFOTS(1711462090.576303013) -1711462090.583568 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #12) -1711462090.583572 [0] recv: INFOTS(1711462090.576567173) -1711462090.583565 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x7704040017c8:48 [ udp/10.101.12.71:7413@2 ] -1711462090.583577 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.1;">,topic_name="rq/static_transformer/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:904},adlink_entity_factory=1} -1711462090.583587 [0] tev: traffic-xmit (1) 68 -1711462090.583586 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #13) -1711462090.583601 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583607 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.583611 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:904 reliable volatile reader unnamed: (default).rq/static_transformer/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.1;">,topic_name="rq/static_transformer/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583610 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x7704040017c8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.583619 [0] dq.builtin: => EVERYONE -1711462090.583620 [0] tev: traffic-xmit (1) 64 -1711462090.583614 [0] recv: INFOTS(1711462090.582707552) -1711462090.583628 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583634 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #38 L(:1c1 16938.243117)) -1711462090.583626 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:904) scanning all wrs of topic rq/static_transformer/describe_parametersRequest -1711462090.583642 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1976 from udp/10.101.12.71:40473 -1711462090.583647 [0] recvMC: INFOTS(1711462090.583554164) -1711462090.583636 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x7704040017c8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.583650 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.2;">,topic_name="rq/static_transformer/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:b04},adlink_entity_factory=1} -1711462090.583658 [0] tev: traffic-xmit (1) 64 -1711462090.583643 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1308 from udp/10.101.12.71:40473 -1711462090.583667 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583653 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #40 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.583670 [0] recv: INFOTS(1711462090.576793854) -1711462090.583679 [0] recvMC: HEARTBEAT(F#46:40..40 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462090.583678 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:b04 reliable volatile reader unnamed: (default).rq/static_transformer/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.2;">,topic_name="rq/static_transformer/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583685 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #14 L(:1c1 16938.243170)) -1711462090.583697 [0] recv: INFOTS(1711462090.577371768) -1711462090.583697 [0] dq.builtin: => EVERYONE -1711462090.583703 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #15) -1711462090.583705 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:b04) scanning all wrs of topic rq/static_transformer/get_parametersRequest -1711462090.583709 [0] recv: INFOTS(1711462090.577626247) -1711462090.583714 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #16) -1711462090.583718 [0] recv: INFOTS(1711462090.577884615) -1711462090.583720 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.3;">,topic_name="rq/static_transformer/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:d04},adlink_entity_factory=1} -1711462090.583725 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #17) -1711462090.583732 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 964 from udp/10.101.12.71:40473 -1711462090.583737 [0] recv: INFOTS(1711462090.582794562) -1711462090.583739 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:d04 reliable volatile reader unnamed: (default).rq/static_transformer/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.3;">,topic_name="rq/static_transformer/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583883 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #36 L(:1c1 16938.243240)) -1711462090.583888 [0] recv: INFOTS(1711462090.583082734) -1711462090.583892 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #39) -1711462090.583894 [0] dq.builtin: => EVERYONE -1711462090.583898 [0] recv: INFOTS(1711462090.583152869) -1711462090.583904 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:d04) scanning all wrs of topic rq/static_transformer/get_parameter_typesRequest -1711462090.583911 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #37) -1711462090.583915 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.4;">,topic_name="rq/static_transformer/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:f04},adlink_entity_factory=1} -1711462090.583920 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1272 from udp/10.101.12.71:40473 -1711462090.583920 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.583928 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.583934 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:f04 reliable volatile reader unnamed: (default).rq/static_transformer/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.4;">,topic_name="rq/static_transformer/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583938 [0] recv: INFOTS(1711462090.578086523) -1711462090.583934 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x7704040017c8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.583946 [0] tev: traffic-xmit (1) 64 -1711462090.583946 [0] dq.builtin: => EVERYONE -1711462090.583944 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #18 L(:1c1 16938.243431)) -1711462090.583955 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:f04) scanning all wrs of topic rq/static_transformer/list_parametersRequest -1711462090.583960 [0] recv: INFOTS(1711462090.578323981) -1711462090.583965 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #19) -1711462090.583967 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.5;">,topic_name="rq/static_transformer/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1104},adlink_entity_factory=1} -1711462090.583968 [0] recv: INFOTS(1711462090.578575730) -1711462090.583980 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #20) -1711462090.583983 [0] recv: INFOTS(1711462090.578795007) -1711462090.583987 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #21) -1711462090.583987 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1104 reliable volatile reader unnamed: (default).rq/static_transformer/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.5;">,topic_name="rq/static_transformer/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583994 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 680 from udp/10.101.12.71:40473 -1711462090.583998 [0] recv: INFOTS(1711462090.583470556) -1711462090.584002 [0] dq.builtin: => EVERYONE -1711462090.584004 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #40 L(:1c1 16938.243501)) -1711462090.584006 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:1104) scanning all wrs of topic rq/static_transformer/set_parametersRequest -1711462090.584012 [0] recv: INFOTS(1711462090.583536280) -1711462090.584019 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #38) -1711462090.584022 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.6;">,topic_name="rq/static_transformer/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1304},adlink_entity_factory=1} -1711462090.584026 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1280 from udp/10.101.12.71:40473 -1711462090.584031 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584034 [0] recv: INFOTS(1711462090.579038346) -1711462090.584039 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584041 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #22 L(:1c1 16938.243535)) -1711462090.584041 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1304 reliable volatile reader unnamed: (default).rq/static_transformer/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.6;">,topic_name="rq/static_transformer/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.584044 [0] recv: INFOTS(1711462090.579338744) -1711462090.584052 [0] dq.builtin: => EVERYONE -1711462090.584056 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #23) -1711462090.584061 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:1304) scanning all wrs of topic rq/static_transformer/set_parameters_atomicallyRequest -1711462090.584046 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x7704040017c8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.584070 [0] tev: traffic-xmit (1) 64 -1711462090.584066 [0] recv: INFOTS(1711462090.579582890) -1711462090.584067 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:200c2 #1: ST0 DCPSParticipantMessage/ParticipantMessageData:{110443d:bb7f10a5:18901533:1}:1<0> -1711462090.584077 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #24) -1711462090.584079 [0] dq.builtin: PMD ST0 pp 110443d:bb7f10a5:18901533 kind 1 data 1 -1711462090.584081 [0] recv: INFOTS(1711462090.579761832) -1711462090.584089 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #1: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584091 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #25) -1711462090.584097 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 360 from udp/10.101.12.71:40473 -1711462090.584104 [0] recv: INFOTS(1711462090.583967232) -1711462090.584100 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2024 from udp/10.101.12.71:40473 -1711462090.584109 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:403 reliable transient-local writer unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584110 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #41 L(:1c1 16938.243607)) -1711462090.584115 [0] recvMC: INFOTS(1711462090.584035736) -1711462090.584125 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1344 from udp/10.101.12.71:40473 -1711462090.584125 [0] dq.builtin: => EVERYONE -1711462090.584129 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584134 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:403) scanning all rds of topic ros_discovery_info -1711462090.584135 [0] recv: INFOTS(1711462090.580027501) -1711462090.584141 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:504 init_acknack_count = 1 -1711462090.584147 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #26 L(:1c1 16938.243632)) -1711462090.584149 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:403 rd 110aba1:7b19badd:ce0adb73:504) -1711462090.584151 [0] recv: INFOTS(1711462090.580241067) -1711462090.584153 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:403 rd 110aba1:7b19badd:ce0adb73:504) - out-of-sync -1711462090.584155 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #27) -1711462090.584137 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #41 L(:1c1 16938.243618) 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: no readers) -1711462090.584158 [0] recv: INFOTS(1711462090.580434684) -1711462090.584165 [0] recvMC: HEARTBEAT(F#47:41..41 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:504 #41) 110aba1:7b19badd:ce0adb73:504@40) -1711462090.584168 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #28) -1711462090.584146 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584172 [0] recv: INFOTS(1711462090.580705816) -1711462090.584164 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #2: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",durability=1,durability_service=0:0:1000:-1:-1:-1,reliability=1:9223372036854775807,lifespan=10000000000,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584176 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #29) -1711462090.584182 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#1:41/1:1 -1711462090.584185 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462090.584182 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.584190 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x7704040017c8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.584195 [0] recv: INFOTS(1711462090.584023477) -1711462090.584195 [0] tev: traffic-xmit (1) 64 -1711462090.584192 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:603 reliable transient-local writer unnamed: (default).rt/rosout/rcl_interfaces::msg::dds_::Log_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1000:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=10000000000,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584205 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #39 L(:1c1 16938.243698)) -1711462090.584204 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.584212 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 1276 from udp/10.101.12.71:50807 -1711462090.584209 [0] dq.builtin: => EVERYONE -1711462090.584217 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x77040c002d38:48 [ udp/10.101.12.71:7415@2 ] -1711462090.584219 [0] tev: traffic-xmit (1) 68 -1711462090.584222 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584219 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584225 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x7704040017c8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.584226 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:603) scanning all rds of topic rt/rosout -1711462090.584227 [0] tev: traffic-xmit (1) 64 -1711462090.584231 [0] recv: INFOTS(1711462090.144360961) -1711462090.584235 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584236 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #1 L(:1c1 16938.243723)) -1711462090.584243 [0] recv: INFOTS(1711462090.144914546) -1711462090.584247 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #2) -1711462090.584250 [0] recv: INFOTS(1711462090.145397528) -1711462090.584253 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584256 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #3) -1711462090.584259 [0] recv: INFOTS(1711462090.146281233) -1711462090.584262 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #4) -1711462090.584266 [0] recv: INFOTS(1711462090.146691062) -1711462090.584263 [0] dq.builtin: => EVERYONE -1711462090.584270 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #5) -1711462090.584275 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2008 from udp/10.101.12.71:40473 -1711462090.584282 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1344 from udp/10.101.12.71:40473 -1711462090.584286 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584273 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:703) scanning all rds of topic rt/parameter_events -1711462090.584291 [0] recvUC: INFOTS(1711462090.584035736) -1711462090.584287 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584297 [0] recv: INFOTS(1711462090.580901717) -1711462090.584300 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1;">,topic_name="rr/map_manager/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584302 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #30 L(:1c1 16938.243790)) -1711462090.584305 [0] recv: INFOTS(1711462090.581210847) -1711462090.584309 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #31) -1711462090.584312 [0] recv: INFOTS(1711462090.581483917) -1711462090.584315 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #32) -1711462090.584317 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:803 reliable volatile writer unnamed: (default).rr/map_manager/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1;">,topic_name="rr/map_manager/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584318 [0] recv: INFOTS(1711462090.581783677) -1711462090.584323 [0] dq.builtin: => EVERYONE -1711462090.584326 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:803) scanning all rds of topic rr/map_manager/describe_parametersReply -1711462090.584326 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #33 msr_in_sync(110aba1:7b19badd:ce0adb73:4c7 out-of-sync to tlcatchup)) -1711462090.584335 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.2;">,topic_name="rr/map_manager/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584336 [0] recv: HEARTBEAT(#7:1..35 110d7b4:17c5b8c5:94bd0ff4:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: 110aba1:7b19badd:ce0adb73:4c7@39(sync)) -1711462090.584347 [0] recv: HEARTBEAT(#3:1..0 110d7b4:17c5b8c5:94bd0ff4:300c3 -> 110aba1:7b19badd:ce0adb73:300c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:300c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:300c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:300c4@0(sync)) -1711462090.584353 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:a03 reliable volatile writer unnamed: (default).rr/map_manager/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.2;">,topic_name="rr/map_manager/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584354 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 68 from udp/10.101.12.71:40473 -1711462090.584364 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584365 [0] dq.builtin: => EVERYONE -1711462090.584371 [0] recvUC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 110aba1:7b19badd:ce0adb73:504 #41 msr_in_sync(110aba1:7b19badd:ce0adb73:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #41: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3 =>110aba1:7b19badd:ce0adb73:504 -1711462090.584371 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:a03) scanning all rds of topic rr/map_manager/get_parametersReply -1711462090.584372 [0] recv: HEARTBEAT(#3:1..0 L(:1c1 16938.243867)110d7b4:17c5b8c5:94bd0ff4:301c3 -> 110aba1:7b19badd:ce0adb73:301c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:301c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:301c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:301c4@0(sync)) -1711462090.584385 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584388 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 360 from udp/10.101.12.71:40473 -1711462090.584394 [0] recv: INFOTS(1711462090.584293877) -1711462090.584383 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.3;">,topic_name="rr/map_manager/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584399 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #42 L(:1c1 16938.243898)) -1711462090.584386 [0] recvUC: ) -1711462090.584405 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 1328 from udp/10.101.12.71:50807 -1711462090.584390 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#2:40/0: -1711462090.584417 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462090.584409 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584415 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:c03 reliable volatile writer unnamed: (default).rr/map_manager/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.3;">,topic_name="rr/map_manager/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584425 [0] recv: INFOTS(1711462090.147101166) -1711462090.584422 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x7704040017c8:44 [ udp/10.101.12.71:7410@2 ] -1711462090.584431 [0] tev: traffic-xmit (1) 64 -1711462090.584410 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 68 from udp/10.101.12.71:40473 -1711462090.584434 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584431 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #6 L(:1c1 16938.243913)) -1711462090.584438 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584445 [0] recv: INFOTS(1711462090.147462171) -1711462090.584439 [0] dq.builtin: => EVERYONE -1711462090.584453 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #7) -1711462090.584458 [0] recv: INFOTS(1711462090.148077554) -1711462090.584442 [0] tev: acknack 110aba1:7b19badd:ce0adb73:300c4 -> 110d7b4:17c5b8c5:94bd0ff4:300c3: F#1:1/0: -1711462090.584463 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #8) -1711462090.584456 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:c03) scanning all rds of topic rr/map_manager/get_parameter_typesReply -1711462090.584467 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:300c4 -> pwr 110d7b4:17c5b8c5:94bd0ff4:300c3) -1711462090.584472 [0] recv: INFOTS(1711462090.148485952) -1711462090.584441 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2072 from udp/10.101.12.71:40473 -1711462090.584477 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.4;">,topic_name="rr/map_manager/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584476 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.584480 [0] recvUC: HEARTBEAT(#48:41..41 L(:1c1 16938.243941)110d7b4:17c5b8c5:94bd0ff4:403 -> 110aba1:7b19badd:ce0adb73:504: 110aba1:7b19badd:ce0adb73:504@41(sync)) -1711462090.584479 [0] recvMC: INFOTS(1711462090.584362470) -1711462090.584485 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #9 msr_in_sync(110aba1:7b19badd:ce0adb73:3c7 out-of-sync to tlcatchup)) -1711462090.584501 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:e03 reliable volatile writer unnamed: (default).rr/map_manager/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.4;">,topic_name="rr/map_manager/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584483 [0] tev: acknack 110aba1:7b19badd:ce0adb73:301c4 -> 110d7b4:17c5b8c5:94bd0ff4:301c3: F#1:1/0: -1711462090.584510 [0] recv: HEARTBEAT(#8:1..9 110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: 110aba1:7b19badd:ce0adb73:3c7@9(sync)) -1711462090.584515 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:301c4 -> pwr 110d7b4:17c5b8c5:94bd0ff4:301c3) -1711462090.584515 [0] dq.builtin: => EVERYONE -1711462090.584523 [0] recv: HEARTBEAT(#7:1..1 110cd0d:56a27910:57318171:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:200c7 #1) 110aba1:7b19badd:ce0adb73:200c7@0) -1711462090.584526 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:e03) scanning all rds of topic rr/map_manager/list_parametersReply -1711462090.584522 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 3 sz 92 => now niov 4 sz 120 -1711462090.584530 [0] recv: HEARTBEAT(#4:1..0 110cd0d:56a27910:57318171:300c3 -> 110aba1:7b19badd:ce0adb73:300c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:300c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:300c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:300c4@0(sync)) -1711462090.584532 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#2:42/0: -1711462090.584533 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462090.584535 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.5;">,topic_name="rr/map_manager/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584537 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.584538 [0] tev: nn_xpack_send 120: 0x77041400139c:20 0x77040c002d38:44 0x7704040017d8:28 0x770404001658:28 [ udp/10.101.12.71:7414@2 ] -1711462090.584541 [0] recv: INFOTS(1711462090.584335539) -1711462090.584542 [0] tev: traffic-xmit (1) 120 -1711462090.584547 [0] tev: xpack_addmsg 0x770414001390 0x770404001390 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584547 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #40 L(:1c1 16938.244044)) -1711462090.584549 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:56a27910:57318171:3c2: F#2:10/0: -1711462090.584544 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #42 L(:1c1 16938.243983) => EVERYONE -1711462090.584554 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110cd0d:56a27910:57318171:3c2) -1711462090.584552 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1003 reliable volatile writer unnamed: (default).rr/map_manager/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.5;">,topic_name="rr/map_manager/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584558 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001478:44 [ udp/10.101.12.71:7415@2 ] -1711462090.584560 [0] tev: traffic-xmit (1) 64 -1711462090.584556 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 132 from udp/10.101.12.71:40473 -1711462090.584563 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584567 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584566 [0] dq.builtin: => EVERYONE -1711462090.584571 [0] recv: INFOTS(1711462090.541742955) -1711462090.584574 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1003) scanning all rds of topic rr/map_manager/set_parametersReply -1711462090.584569 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2: F#1:1/1:1 -1711462090.584578 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 #1 L(:1c1 16938.244070) msr_in_sync(110aba1:7b19badd:ce0adb73:200c7 out-of-sync to tlcatchup)) -1711462090.584579 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:56a27910:57318171:200c2) -1711462090.584583 [0] tev: xpack_addmsg 0x770414001390 0x770404001390 0(control): niov 2 sz 64 => now niov 3 sz 96 -1711462090.584583 [0] recv: HEARTBEAT(#7:1..1 110d7b4:17c5b8c5:94bd0ff4:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: 110aba1:7b19badd:ce0adb73:200c7@1(sync)) -1711462090.584584 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.6;">,topic_name="rr/map_manager/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584585 [0] tev: acknack 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:56a27910:57318171:300c3: F#1:1/0: -1711462090.584590 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 1400 from udp/10.101.12.71:50807 -1711462090.584592 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:300c4 -> pwr 110cd0d:56a27910:57318171:300c3) -1711462090.584594 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 3 sz 96 => now niov 4 sz 124 -1711462090.584596 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584598 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) suppressed, resched in 0.1 s (min-ack 2!, avail-seq 2, xmit 2) -1711462090.584602 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#2:2/0: -1711462090.584600 [0] recv: INFOTS(1711462090.144576280) -1711462090.584605 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1203 reliable volatile writer unnamed: (default).rr/map_manager/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.6;">,topic_name="rr/map_manager/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584604 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462090.584609 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #1 L(:1c1 16938.244099)) -1711462090.584614 [0] tev: nn_xpack_send 124: 0x77041400139c:20 0x77040c002d38:44 0x770404001488:32 0x7704040017d8:28 [ udp/10.101.12.71:7410@2 ] -1711462090.584617 [0] dq.builtin: => EVERYONE -1711462090.584620 [0] gc: gc 0x77041c037f70: not yet, shortsleep -1711462090.584618 [0] recv: INFOTS(1711462090.146372602) -1711462090.584620 [0] tev: traffic-xmit (1) 124 -1711462090.584633 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1203) scanning all rds of topic rr/map_manager/set_parameters_atomicallyReply -1711462090.584634 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584634 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #2) -1711462090.584643 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001648:44 [ udp/10.101.12.71:7414@2 ] -1711462090.584644 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #10: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.7;">,topic_name="rr/topological_map_manager2/get_topological_mapReply",type_name="std_srvs::srv::dds_::Trigger_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584648 [0] tev: traffic-xmit (1) 64 -1711462090.584647 [0] recv: INFOTS(1711462090.146779354) -1711462090.584634 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #42: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.584656 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #3) -1711462090.584660 [0] recv: INFOTS(1711462090.147181889) -1711462090.584660 [0] recvMC: HEARTBEAT(F#49:42..42 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@42(sync)) -1711462090.584665 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #4) -1711462090.584668 [0] recv: INFOTS(1711462090.147550622) -1711462090.584671 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #5) -1711462090.584676 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_topological_mapReply/std_srvs::srv::dds_::Trigger_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.7;">,topic_name="rr/topological_map_manager2/get_topological_mapReply",type_name="std_srvs::srv::dds_::Trigger_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584678 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 1384 from udp/10.101.12.71:50807 -1711462090.584683 [0] dq.builtin: => EVERYONE -1711462090.584685 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584688 [0] recv: INFOTS(1711462090.148164095) -1711462090.584691 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1403) scanning all rds of topic rr/topological_map_manager2/get_topological_mapReply -1711462090.584692 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #6 L(:1c1 16938.244189)) -1711462090.584695 [0] recv: INFOTS(1711462090.148579939) -1711462090.584698 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #7) -1711462090.584699 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #11: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.8;">,topic_name="rr/topological_map_manager2/get_tagged_nodesReply",type_name="topological_navigation_msgs::srv::dds_::GetTaggedNodes_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584701 [0] recv: INFOTS(1711462090.148933169) -1711462090.584709 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #8) -1711462090.584711 [0] recv: INFOTS(1711462090.149246179) -1711462090.584715 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #9) -1711462090.584717 [0] recv: INFOTS(1711462090.149509963) -1711462090.584719 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1603 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_tagged_nodesReply/topological_navigation_msgs::srv::dds_::GetTaggedNodes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.8;">,topic_name="rr/topological_map_manager2/get_tagged_nodesReply",type_name="topological_navigation_msgs::srv::dds_::GetTaggedNodes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584721 [0] recv: DATA(110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #10 msr_in_sync(110aba1:7b19badd:ce0adb73:4c7 out-of-sync to tlcatchup)) -1711462090.584723 [0] dq.builtin: => EVERYONE -1711462090.584727 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1603) scanning all rds of topic rr/topological_map_manager2/get_tagged_nodesReply -1711462090.584729 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:56a27910:57318171:4c2: F#2:11/0: -1711462090.584727 [0] recv: HEARTBEAT(#8:1..10 110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: 110aba1:7b19badd:ce0adb73:4c7@10(sync)) -1711462090.584731 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110cd0d:56a27910:57318171:4c2) -1711462090.584735 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #12: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.9;">,topic_name="rr/topological_map_manager2/get_tagsReply",type_name="topological_navigation_msgs::srv::dds_::GetTags_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584736 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584743 [0] tev: acknack 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:56a27910:57318171:301c3: F#1:1/0: -1711462090.584744 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:301c4 -> pwr 110cd0d:56a27910:57318171:301c3) -1711462090.584740 [0] recv: HEARTBEAT(#4:1..0 110cd0d:56a27910:57318171:301c3 -> 110aba1:7b19badd:ce0adb73:301c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:301c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:301c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:301c4@0(sync)) -1711462090.584746 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.584750 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.584753 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_tagsReply/topological_navigation_msgs::srv::dds_::GetTags_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.9;">,topic_name="rr/topological_map_manager2/get_tagsReply",type_name="topological_navigation_msgs::srv::dds_::GetTags_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584754 [0] tev: nn_xpack_send 92: 0x77041400139c:20 0x770404001648:44 0x77040c002d48:28 [ udp/10.101.12.71:7410@2 ] -1711462090.584763 [0] tev: traffic-xmit (1) 92 -1711462090.584760 [0] recv: HEARTBEAT(#26:1..10 L(:1c1 16938.244189)110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:3c7 #10) 110aba1:7b19badd:ce0adb73:3c7@0) -1711462090.584766 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:e3b81b8d:1ccb65b1:3c2: F#1:1/10:1111111111 -1711462090.584768 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462090.584766 [0] dq.builtin: => EVERYONE -1711462090.584770 [0] recv: HEARTBEAT(#25:1..10 110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:4c7 #10) 110aba1:7b19badd:ce0adb73:4c7@0) -1711462090.584773 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1803) scanning all rds of topic rr/topological_map_manager2/get_tagsReply -1711462090.584770 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.584776 [0] recv: HEARTBEAT(#27:3..3 110cd0d:e3b81b8d:1ccb65b1:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:200c7 #3) 110aba1:7b19badd:ce0adb73:200c7@2) -1711462090.584779 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:e3b81b8d:1ccb65b1:4c2: F#1:1/10:1111111111 -1711462090.584781 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462090.584782 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 2 sz 68 => now niov 3 sz 100 -1711462090.584783 [0] recv: HEARTBEAT(#14:1..0 110cd0d:e3b81b8d:1ccb65b1:300c3 -> 110aba1:7b19badd:ce0adb73:300c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:300c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:300c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:300c4@0(sync)) -1711462090.584785 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2: F#1:3/1:1 -1711462090.584788 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462090.584782 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #13: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.a;">,topic_name="rr/topological_map_manager2/get_node_tagsReply",type_name="topological_navigation_msgs::srv::dds_::GetNodeTags_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584788 [0] recv: HEARTBEAT(#14:1..0 110cd0d:e3b81b8d:1ccb65b1:301c3 -> 110aba1:7b19badd:ce0adb73:301c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:301c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:301c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:301c4@0(sync)) -1711462090.584790 [0] tev: xpack_addmsg 0x770414001390 0x770404001390 0(control): niov 3 sz 100 => now niov 4 sz 132 -1711462090.584798 [0] tev: acknack 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:e3b81b8d:1ccb65b1:300c3: F#1:1/0: -1711462090.584800 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:300c4 -> pwr 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462090.584800 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.584801 [0] tev: xpack_addmsg 0x770414001390 0x7704040016e0 0(control): niov 4 sz 132 => now niov 5 sz 160 -1711462090.584805 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584807 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1a03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_node_tagsReply/topological_navigation_msgs::srv::dds_::GetNodeTags_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.a;">,topic_name="rr/topological_map_manager2/get_node_tagsReply",type_name="topological_navigation_msgs::srv::dds_::GetNodeTags_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584807 [0] tev: acknack 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:e3b81b8d:1ccb65b1:301c3: F#1:1/0: -1711462090.584812 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:301c4 -> pwr 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462090.584812 [0] recv: HEARTBEAT(#18:1..10 L(:1c1 16938.244308)110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:3c7 #10) 110aba1:7b19badd:ce0adb73:3c7@0) -1711462090.584812 [0] dq.builtin: => EVERYONE -1711462090.584813 [0] tev: xpack_addmsg 0x770414001390 0x77040c0032f0 0(control): niov 5 sz 160 => now niov 6 sz 188 -1711462090.584819 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.584823 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584824 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:79d6eeac:ea4a8907:3c2: F#1:1/10:1111111111 -1711462090.584823 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1a03) scanning all rds of topic rr/topological_map_manager2/get_node_tagsReply -1711462090.584826 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110cd0d:79d6eeac:ea4a8907:3c2) -1711462090.584828 [0] tev: xpack_addmsg 0x770414001390 0x77040c002fa0 0(control): niov 6 sz 188 => now niov 7 sz 236 -1711462090.584829 [0] recv: HEARTBEAT(#17:1..10 L(:1c1 16938.244326)110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:4c7 #10) 110aba1:7b19badd:ce0adb73:4c7@0) -1711462090.584831 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:79d6eeac:ea4a8907:4c2: F#1:1/10:1111111111 -1711462090.584840 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.584840 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110cd0d:79d6eeac:ea4a8907:4c2) -1711462090.584845 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #14: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.b;">,topic_name="rr/topological_map_manager2/get_edges_between_nodesReply",type_name="topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584847 [0] tev: xpack_addmsg 0x770414001390 0x77040c002dd0 0(control): niov 7 sz 236 => now niov 8 sz 268 -1711462090.584844 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584852 [0] tev: nn_xpack_send 268: 0x77041400139c:20 0x770404001648:48 0x77040c002d48:32 0x770404001488:32 0x7704040017d8:28 0x77040c0033e8:28 0x77040c003088:48 0x77040c002ec8:32 [ udp/10.101.12.71:7410@2 ] -1711462090.584853 [0] tev: traffic-xmit (1) 268 -1711462090.584855 [0] recv: HEARTBEAT(#19:3..3 L(:1c1 16938.244347)110cd0d:79d6eeac:ea4a8907:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:200c7 #3) 110aba1:7b19badd:ce0adb73:200c7@2) -1711462090.584855 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2: F#1:3/1:1 -1711462090.584860 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:79d6eeac:ea4a8907:200c2) -1711462090.584861 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_edges_between_nodesReply/topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.b;">,topic_name="rr/topological_map_manager2/get_edges_between_nodesReply",type_name="topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584862 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.584862 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.584868 [0] dq.builtin: => EVERYONE -1711462090.584872 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1c03) scanning all rds of topic rr/topological_map_manager2/get_edges_between_nodesReply -1711462090.584869 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x770404001648:48 [ udp/10.101.12.71:7410@2 ] -1711462090.584876 [0] tev: traffic-xmit (1) 68 -1711462090.584874 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584881 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #15: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.c;">,topic_name="rr/topological_map_manager2/write_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584886 [0] recv: HEARTBEAT(#9:1..0 L(:1c1 16938.244378)110cd0d:79d6eeac:ea4a8907:300c3 -> 110aba1:7b19badd:ce0adb73:300c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:300c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:300c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:300c4@0(sync)) -1711462090.584888 [0] tev: acknack 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:79d6eeac:ea4a8907:300c3: F#1:1/0: -1711462090.584893 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.584894 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:300c4 -> pwr 110cd0d:79d6eeac:ea4a8907:300c3) -1711462090.584899 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584900 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1e03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/write_topological_mapReply/topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.c;">,topic_name="rr/topological_map_manager2/write_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584897 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584902 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001648:44 [ udp/10.101.12.71:7410@2 ] -1711462090.584904 [0] tev: traffic-xmit (1) 64 -1711462090.584909 [0] dq.builtin: => EVERYONE -1711462090.584912 [0] recv: HEARTBEAT(#9:1..0 L(:1c1 16938.244401)110cd0d:79d6eeac:ea4a8907:301c3 -> 110aba1:7b19badd:ce0adb73:301c4: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:301c4 #0) msr_in_sync(110aba1:7b19badd:ce0adb73:301c4 out-of-sync to tlcatchup) 110aba1:7b19badd:ce0adb73:301c4@0(sync)) -1711462090.584913 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1e03) scanning all rds of topic rr/topological_map_manager2/write_topological_mapReply -1711462090.584920 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 132 from udp/10.101.12.71:50807 -1711462090.584914 [0] tev: acknack 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:79d6eeac:ea4a8907:301c3: F#1:1/0: -1711462090.584926 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #16: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.d;">,topic_name="rr/topological_map_manager2/switch_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584924 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584927 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:301c4 -> pwr 110cd0d:79d6eeac:ea4a8907:301c3) -1711462090.584933 [0] recv: INFOTS(1711462090.143586504) -1711462090.584934 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584940 [0] recv: DATA(110cd0d:56a27910:57318171:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 #1 L(:1c1 16938.244428) msr_in_sync(110aba1:7b19badd:ce0adb73:200c7 out-of-sync to tlcatchup)) -1711462090.584941 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001648:44 [ udp/10.101.12.71:7410@2 ] -1711462090.584948 [0] tev: traffic-xmit (1) 64 -1711462090.584951 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2: F#2:2/0: -1711462090.584953 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:56a27910:57318171:200c2) -1711462090.584955 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.584946 [0] recv: HEARTBEAT(#8:1..1 110cd0d:56a27910:57318171:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: 110aba1:7b19badd:ce0adb73:200c7@1(sync)) -1711462090.584944 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2003 reliable volatile writer unnamed: (default).rr/topological_map_manager2/switch_topological_mapReply/topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.d;">,topic_name="rr/topological_map_manager2/switch_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584964 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 1340 from udp/10.101.12.71:50807 -1711462090.584959 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001648:44 [ udp/10.101.12.71:7410@2 ] -1711462090.584968 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.584968 [0] dq.builtin: => EVERYONE -1711462090.584973 [0] recv: INFOTS(1711462067.906161391) -1711462090.584975 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2003) scanning all rds of topic rr/topological_map_manager2/switch_topological_mapReply -1711462090.584978 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #1 L(:1c1 16938.244472)) -1711462090.584970 [0] tev: traffic-xmit (1) 64 -1711462090.584982 [0] recv: INFOTS(1711462067.920967451) -1711462090.584984 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #17: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.e;">,topic_name="rr/topological_map_manager2/add_topological_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddNode_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584987 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #3) -1711462090.584990 [0] recv: INFOTS(1711462067.921934444) -1711462090.584994 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #4) -1711462090.584997 [0] recv: INFOTS(1711462067.922332717) -1711462090.584999 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2203 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_topological_nodeReply/topological_navigation_msgs::srv::dds_::AddNode_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.e;">,topic_name="rr/topological_map_manager2/add_topological_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddNode_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585001 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #5) -1711462090.585008 [0] recv: INFOTS(1711462067.922693393) -1711462090.585010 [0] dq.builtin: => EVERYONE -1711462090.585012 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #6) -1711462090.585014 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2203) scanning all rds of topic rr/topological_map_manager2/add_topological_nodeReply -1711462090.585022 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #18: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.f;">,topic_name="rr/topological_map_manager2/remove_topological_nodeReply",type_name="topological_navigation_msgs::srv::dds_::RmvNode_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585038 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/remove_topological_nodeReply/topological_navigation_msgs::srv::dds_::RmvNode_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.f;">,topic_name="rr/topological_map_manager2/remove_topological_nodeReply",type_name="topological_navigation_msgs::srv::dds_::RmvNode_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585043 [0] dq.builtin: => EVERYONE -1711462090.585047 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2403) scanning all rds of topic rr/topological_map_manager2/remove_topological_nodeReply -1711462090.585056 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #19: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.10;">,topic_name="rr/topological_map_manager2/add_edges_between_nodesReply",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585062 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 1180 from udp/10.101.12.71:50807 -1711462090.585066 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.585069 [0] recv: INFOTS(1711462067.923065472) -1711462090.585072 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2603 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_edges_between_nodesReply/topological_navigation_msgs::srv::dds_::AddEdge_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.10;">,topic_name="rr/topological_map_manager2/add_edges_between_nodesReply",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585074 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #7 L(:1c1 16938.244570)) -1711462090.585081 [0] recv: INFOTS(1711462067.923472842) -1711462090.585082 [0] dq.builtin: => EVERYONE -1711462090.585085 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #8) -1711462090.585089 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2603) scanning all rds of topic rr/topological_map_manager2/add_edges_between_nodesReply -1711462090.585092 [0] recv: INFOTS(1711462067.923854920) -1711462090.585099 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #9) -1711462090.585101 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #20: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.11;">,topic_name="rr/topological_map_manager2/remove_edgeReply",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585104 [0] recv: GAP(2..3/8 110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 msr_in_sync(110aba1:7b19badd:ce0adb73:3c7 out-of-sync to tlcatchup)) -1711462090.585110 [0] recv: INFOTS(1711462067.906312171) -1711462090.585114 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #1) -1711462090.585118 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/remove_edgeReply/topological_navigation_msgs::srv::dds_::AddEdge_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.11;">,topic_name="rr/topological_map_manager2/remove_edgeReply",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585120 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 1188 from udp/10.101.12.71:50807 -1711462090.585127 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.585131 [0] recv: INFOTS(1711462067.922001214) -1711462090.585128 [0] dq.builtin: => EVERYONE -1711462090.585136 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #2 L(:1c1 16938.244630)) -1711462090.585140 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2803) scanning all rds of topic rr/topological_map_manager2/remove_edgeReply -1711462090.585143 [0] recv: INFOTS(1711462067.922399199) -1711462090.585152 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #21: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.12;">,topic_name="rr/topological_map_manager2/add_content_to_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddContent_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585155 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #3) -1711462090.585158 [0] recv: INFOTS(1711462067.922751849) -1711462090.585161 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #4) -1711462090.585164 [0] recv: INFOTS(1711462067.923122963) -1711462090.585168 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #5) -1711462090.585169 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2a03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_content_to_nodeReply/topological_navigation_msgs::srv::dds_::AddContent_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.12;">,topic_name="rr/topological_map_manager2/add_content_to_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddContent_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585175 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 1264 from udp/10.101.12.71:50807 -1711462090.585177 [0] dq.builtin: => EVERYONE -1711462090.585178 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.585180 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2a03) scanning all rds of topic rr/topological_map_manager2/add_content_to_nodeReply -1711462090.585181 [0] recv: INFOTS(1711462067.923536312) -1711462090.585189 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #22: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.13;">,topic_name="rr/topological_map_manager2/update_node_nameReply",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeName_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585190 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #6 L(:1c1 16938.244681)) -1711462090.585197 [0] recv: INFOTS(1711462067.923914544) -1711462090.585201 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #7) -1711462090.585203 [0] recv: INFOTS(1711462067.925434417) -1711462090.585206 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2c03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_node_nameReply/topological_navigation_msgs::srv::dds_::UpdateNodeName_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.13;">,topic_name="rr/topological_map_manager2/update_node_nameReply",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeName_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585206 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #8) -1711462090.585213 [0] recv: INFOTS(1711462067.925734727) -1711462090.585214 [0] dq.builtin: => EVERYONE -1711462090.585217 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #9) -1711462090.585220 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2c03) scanning all rds of topic rr/topological_map_manager2/update_node_nameReply -1711462090.585223 [0] recv: INFOTS(1711462067.925980789) -1711462090.585230 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #10 msr_in_sync(110aba1:7b19badd:ce0adb73:4c7 out-of-sync to tlcatchup)) -1711462090.585232 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #23: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.14;">,topic_name="rr/topological_map_manager2/update_node_poseReply",type_name="topological_navigation_msgs::srv::dds_::AddNode_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585237 [0] recv: HEARTBEAT(#27:1..10 110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: 110aba1:7b19badd:ce0adb73:3c7@10(sync)) -1711462090.585239 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:e3b81b8d:1ccb65b1:3c2: F#2:11/0: -1711462090.585243 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462090.585241 [0] recv: INFOTS(1711462083.905805667) -1711462090.585245 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.585250 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 #3 msr_in_sync(110aba1:7b19badd:ce0adb73:200c7 out-of-sync to tlcatchup)) -1711462090.585252 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2e03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_node_poseReply/topological_navigation_msgs::srv::dds_::AddNode_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.14;">,topic_name="rr/topological_map_manager2/update_node_poseReply",type_name="topological_navigation_msgs::srv::dds_::AddNode_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585255 [0] recv: HEARTBEAT(#26:1..10 110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: 110aba1:7b19badd:ce0adb73:4c7@10(sync)) -1711462090.585258 [0] dq.builtin: => EVERYONE -1711462090.585253 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001648:44 [ udp/10.101.12.71:7410@2 ] -1711462090.585262 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2e03) scanning all rds of topic rr/topological_map_manager2/update_node_poseReply -1711462090.585265 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 1328 from udp/10.101.12.71:50807 -1711462090.585263 [0] tev: traffic-xmit (1) 64 -1711462090.585270 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.585271 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:e3b81b8d:1ccb65b1:4c2: F#2:11/0: -1711462090.585273 [0] recv: INFOTS(1711462069.653857118) -1711462090.585271 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #24: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.15;">,topic_name="rr/topological_map_manager2/update_node_toleranceReply",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585275 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462090.585280 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #1 L(:1c1 16938.244773)) -1711462090.585281 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.585284 [0] recv: INFOTS(1711462069.654474792) -1711462090.585288 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001648:44 [ udp/10.101.12.71:7410@2 ] -1711462090.585292 [0] tev: traffic-xmit (1) 64 -1711462090.585292 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #3) -1711462090.585297 [0] recv: INFOTS(1711462069.655041546) -1711462090.585292 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3003 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_node_toleranceReply/topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.15;">,topic_name="rr/topological_map_manager2/update_node_toleranceReply",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585303 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #4) -1711462090.585305 [0] recv: INFOTS(1711462069.655283557) -1711462090.585308 [0] dq.builtin: => EVERYONE -1711462090.585310 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #5) -1711462090.585312 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3003) scanning all rds of topic rr/topological_map_manager2/update_node_toleranceReply -1711462090.585312 [0] recv: INFOTS(1711462069.655486748) -1711462090.585320 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #6) -1711462090.585321 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #25: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.16;">,topic_name="rr/topological_map_manager2/modify_node_tagsReply",type_name="topological_navigation_msgs::srv::dds_::ModifyTag_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585327 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 1248 from udp/10.101.12.71:50807 -1711462090.585330 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.585332 [0] recv: INFOTS(1711462069.655692789) -1711462090.585338 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #7 L(:1c1 16938.244834)) -1711462090.585341 [0] recv: INFOTS(1711462069.655921271) -1711462090.585342 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3203 reliable volatile writer unnamed: (default).rr/topological_map_manager2/modify_node_tagsReply/topological_navigation_msgs::srv::dds_::ModifyTag_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.16;">,topic_name="rr/topological_map_manager2/modify_node_tagsReply",type_name="topological_navigation_msgs::srv::dds_::ModifyTag_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585344 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #8) -1711462090.585347 [0] dq.builtin: => EVERYONE -1711462090.585347 [0] recv: INFOTS(1711462069.656160198) -1711462090.585354 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3203) scanning all rds of topic rr/topological_map_manager2/modify_node_tagsReply -1711462090.585355 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 #9) -1711462090.585360 [0] recv: GAP(2..3/8 110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 msr_in_sync(110aba1:7b19badd:ce0adb73:3c7 out-of-sync to tlcatchup)) -1711462090.585362 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #26: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.17;">,topic_name="rr/topological_map_manager2/add_tag_to_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddTag_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585363 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.585373 [0] recv: HEARTBEAT(#28:3..3 L(:1c1 16938.244834)110cd0d:e3b81b8d:1ccb65b1:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: 110aba1:7b19badd:ce0adb73:200c7@3(sync)) -1711462090.585374 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2: F#2:4/0: -1711462090.585376 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.585376 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462090.585379 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_tag_to_nodeReply/topological_navigation_msgs::srv::dds_::AddTag_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.17;">,topic_name="rr/topological_map_manager2/add_tag_to_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddTag_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585381 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.585379 [0] recv: INFOTS(1711462069.653934558) -1711462090.585384 [0] dq.builtin: => EVERYONE -1711462090.585386 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #1) -1711462090.585385 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001648:44 [ udp/10.101.12.71:7410@2 ] -1711462090.585392 [0] tev: traffic-xmit (1) 64 -1711462090.585394 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 1452 from udp/10.101.12.71:50807 -1711462090.585388 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3403) scanning all rds of topic rr/topological_map_manager2/add_tag_to_nodeReply -1711462090.585398 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.585403 [0] recv: INFOTS(1711462069.655092444) -1711462090.585407 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #27: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.18;">,topic_name="rr/topological_map_manager2/rm_tag_from_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddTag_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585408 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #2 L(:1c1 16938.244901)) -1711462090.585414 [0] recv: INFOTS(1711462069.655331767) -1711462090.585418 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #3) -1711462090.585421 [0] recv: INFOTS(1711462069.655532459) -1711462090.585424 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #4) -1711462090.585425 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3603 reliable volatile writer unnamed: (default).rr/topological_map_manager2/rm_tag_from_nodeReply/topological_navigation_msgs::srv::dds_::AddTag_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.18;">,topic_name="rr/topological_map_manager2/rm_tag_from_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddTag_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585426 [0] recv: INFOTS(1711462069.655736184) -1711462090.585434 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #5) -1711462090.585438 [0] recv: INFOTS(1711462069.655967082) -1711462090.585435 [0] dq.builtin: => EVERYONE -1711462090.585444 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #6) -1711462090.585445 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3603) scanning all rds of topic rr/topological_map_manager2/rm_tag_from_nodeReply -1711462090.585451 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 1008 from udp/10.101.12.71:50807 -1711462090.585454 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.585457 [0] recv: INFOTS(1711462069.656205405) -1711462090.585457 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #28: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.19;">,topic_name="rr/topological_map_manager2/add_param_to_edge_configReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585461 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #7 L(:1c1 16938.244958)) -1711462090.585464 [0] recv: INFOTS(1711462069.656430660) -1711462090.585467 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #8) -1711462090.585470 [0] recv: INFOTS(1711462069.656644590) -1711462090.585473 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #9) -1711462090.585475 [0] recv: INFOTS(1711462069.656823224) -1711462090.585476 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_param_to_edge_configReply/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.19;">,topic_name="rr/topological_map_manager2/add_param_to_edge_configReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585480 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 #10 msr_in_sync(110aba1:7b19badd:ce0adb73:4c7 out-of-sync to tlcatchup)) -1711462090.585484 [0] dq.builtin: => EVERYONE -1711462090.585486 [0] recv: HEARTBEAT(#19:1..10 110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7: 110aba1:7b19badd:ce0adb73:3c7@10(sync)) -1711462090.585487 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3803) scanning all rds of topic rr/topological_map_manager2/add_param_to_edge_configReply -1711462090.585491 [0] recv: HEARTBEAT(#18:1..10 110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7: 110aba1:7b19badd:ce0adb73:4c7@10(sync)) -1711462090.585488 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:79d6eeac:ea4a8907:3c2: F#2:11/0: -1711462090.585497 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110cd0d:79d6eeac:ea4a8907:3c2) -1711462090.585498 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.585499 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #29: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1a;">,topic_name="rr/topological_map_manager2/rm_param_from_edge_configReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585495 [0] recv: INFOTS(1711462085.654169463) -1711462090.585501 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:79d6eeac:ea4a8907:4c2: F#2:11/0: -1711462090.585506 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110cd0d:79d6eeac:ea4a8907:4c2) -1711462090.585507 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 #3 msr_in_sync(110aba1:7b19badd:ce0adb73:200c7 out-of-sync to tlcatchup)) -1711462090.585508 [0] tev: xpack_addmsg 0x770414001390 0x77040c002c50 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.585513 [0] recv: HEARTBEAT(#20:3..3 110cd0d:79d6eeac:ea4a8907:200c2 -> 110aba1:7b19badd:ce0adb73:200c7: 110aba1:7b19badd:ce0adb73:200c7@3(sync)) -1711462090.585514 [0] tev: nn_xpack_send 92: 0x77041400139c:20 0x770404001648:44 0x77040c002d48:28 [ udp/10.101.12.71:7410@2 ] -1711462090.585517 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3a03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/rm_param_from_edge_configReply/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1a;">,topic_name="rr/topological_map_manager2/rm_param_from_edge_configReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585517 [0] tev: traffic-xmit (1) 92 -1711462090.585522 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2: F#2:4/0: -1711462090.585524 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:79d6eeac:ea4a8907:200c2) -1711462090.585526 [0] tev: xpack_addmsg 0x770414001390 0x770404001560 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.585526 [0] dq.builtin: => EVERYONE -1711462090.585530 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404001648:44 [ udp/10.101.12.71:7410@2 ] -1711462090.585531 [0] tev: traffic-xmit (1) 64 -1711462090.585532 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3a03) scanning all rds of topic rr/topological_map_manager2/rm_param_from_edge_configReply -1711462090.585540 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #30: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1b;">,topic_name="rr/topological_map_manager2/rm_param_from_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585557 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3c03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/rm_param_from_topological_mapReply/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1b;">,topic_name="rr/topological_map_manager2/rm_param_from_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585564 [0] dq.builtin: => EVERYONE -1711462090.585567 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3c03) scanning all rds of topic rr/topological_map_manager2/rm_param_from_topological_mapReply -1711462090.585575 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #31: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1c;">,topic_name="rr/topological_map_manager2/update_node_restrictionsReply",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585590 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3e03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_node_restrictionsReply/topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1c;">,topic_name="rr/topological_map_manager2/update_node_restrictionsReply",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585595 [0] dq.builtin: => EVERYONE -1711462090.585598 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3e03) scanning all rds of topic rr/topological_map_manager2/update_node_restrictionsReply -1711462090.585605 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #32: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1d;">,topic_name="rr/topological_map_manager2/update_edge_restrictionsReply",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585621 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4003 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_edge_restrictionsReply/topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1d;">,topic_name="rr/topological_map_manager2/update_edge_restrictionsReply",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585628 [0] dq.builtin: => EVERYONE -1711462090.585632 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4003) scanning all rds of topic rr/topological_map_manager2/update_edge_restrictionsReply -1711462090.585640 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #33: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1e;">,topic_name="rr/topological_map_manager2/update_edgeReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdge_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585656 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4203 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_edgeReply/topological_navigation_msgs::srv::dds_::UpdateEdge_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1e;">,topic_name="rr/topological_map_manager2/update_edgeReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdge_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585661 [0] dq.builtin: => EVERYONE -1711462090.585664 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4203) scanning all rds of topic rr/topological_map_manager2/update_edgeReply -1711462090.585671 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #34: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1f;">,topic_name="rr/topological_map_manager2/update_actionReply",type_name="topological_navigation_msgs::srv::dds_::UpdateAction_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585683 [0] gc: gc 0x77041c037f70: deleting -1711462090.585687 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_actionReply/topological_navigation_msgs::srv::dds_::UpdateAction_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1f;">,topic_name="rr/topological_map_manager2/update_actionReply",type_name="topological_navigation_msgs::srv::dds_::UpdateAction_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585699 [0] dq.builtin: => EVERYONE -1711462090.585702 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4403) scanning all rds of topic rr/topological_map_manager2/update_actionReply -1711462090.585710 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #35: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.20;">,topic_name="rr/topological_map_manager2/add_datumReply",type_name="topological_navigation_msgs::srv::dds_::AddDatum_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585726 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4603 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_datumReply/topological_navigation_msgs::srv::dds_::AddDatum_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.20;">,topic_name="rr/topological_map_manager2/add_datumReply",type_name="topological_navigation_msgs::srv::dds_::AddDatum_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585731 [0] dq.builtin: => EVERYONE -1711462090.585734 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4603) scanning all rds of topic rr/topological_map_manager2/add_datumReply -1711462090.585742 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #36: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rr/topological_map_manager2/update_fail_policyReply",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585746 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_fail_policyReply/topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_ p(open) known -1711462090.585760 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rr/topological_map_manager2/update_fail_policyReply",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585770 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #37: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rr/topological_map_manager2/set_node_influence_zoneReply",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585785 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4a03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/set_node_influence_zoneReply/topological_navigation_msgs::srv::dds_::SetInfluenceZone_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rr/topological_map_manager2/set_node_influence_zoneReply",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.585793 [0] dq.builtin: => EVERYONE -1711462090.585796 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4a03) scanning all rds of topic rr/topological_map_manager2/set_node_influence_zoneReply -1711462090.585803 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #35: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rq/topological_map_manager2/set_node_influence_zoneRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4b04},adlink_entity_factory=1} -1711462090.585820 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/set_node_influence_zoneRequest/topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rq/topological_map_manager2/set_node_influence_zoneRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.585826 [0] dq.builtin: => EVERYONE -1711462090.585829 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4b04) scanning all wrs of topic rq/topological_map_manager2/set_node_influence_zoneRequest -1711462090.585837 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #1: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,history=1:-1,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:504},adlink_entity_factory=1} -1711462090.585854 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:504 reliable transient-local reader unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=1:-1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.585861 [0] dq.builtin: => EVERYONE -1711462090.585864 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:504) scanning all wrs of topic ros_discovery_info -1711462090.585868 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110d7b4:17c5b8c5:94bd0ff4:504) -1711462090.585871 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110d7b4:17c5b8c5:94bd0ff4:504) - ack seq 2 -1711462090.585878 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 udp/10.101.12.71:7417@2 -1711462090.585880 [0] dq.builtin: reduced nlocs=4 -1711462090.585883 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.585886 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462090.585889 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462090.585891 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.585894 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7417@2 2 -> 0 -1711462090.585897 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462090.585899 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.585902 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462090.585904 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462090.585908 [0] dq.builtin: a b c -1711462090.585912 [0] dq.builtin: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. .. } -1711462090.585915 [0] dq.builtin: loc 1 = udp/10.101.12.71:7415@2 1 { .. .. +u } -1711462090.585918 [0] dq.builtin: loc 2 = udp/10.101.12.71:7417@2 1 { .. +u .. } -1711462090.585923 [0] dq.builtin: loc 3 = udp/239.255.0.1:7401@2 0 { +1 +1 +1 } -1711462090.585924 [0] dq.builtin: best = 3 -1711462090.585927 [0] dq.builtin: simple udp/239.255.0.1:7401@2 -1711462090.585932 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462090.585941 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1;">,topic_name="rq/map_manager/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:904},adlink_entity_factory=1} -1711462090.585959 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:904 reliable volatile reader unnamed: (default).rq/map_manager/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1;">,topic_name="rq/map_manager/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.585965 [0] dq.builtin: => EVERYONE -1711462090.585971 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:904) scanning all wrs of topic rq/map_manager/describe_parametersRequest -1711462090.585979 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.2;">,topic_name="rq/map_manager/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:b04},adlink_entity_factory=1} -1711462090.585996 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:b04 reliable volatile reader unnamed: (default).rq/map_manager/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.2;">,topic_name="rq/map_manager/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586001 [0] dq.builtin: => EVERYONE -1711462090.586004 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:b04) scanning all wrs of topic rq/map_manager/get_parametersRequest -1711462090.586011 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.3;">,topic_name="rq/map_manager/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:d04},adlink_entity_factory=1} -1711462090.586027 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:d04 reliable volatile reader unnamed: (default).rq/map_manager/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.3;">,topic_name="rq/map_manager/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586032 [0] dq.builtin: => EVERYONE -1711462090.586035 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:d04) scanning all wrs of topic rq/map_manager/get_parameter_typesRequest -1711462090.586043 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.4;">,topic_name="rq/map_manager/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:f04},adlink_entity_factory=1} -1711462090.586059 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:f04 reliable volatile reader unnamed: (default).rq/map_manager/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.4;">,topic_name="rq/map_manager/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586066 [0] dq.builtin: => EVERYONE -1711462090.586069 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:f04) scanning all wrs of topic rq/map_manager/list_parametersRequest -1711462090.586077 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.5;">,topic_name="rq/map_manager/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1104},adlink_entity_factory=1} -1711462090.586094 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1104 reliable volatile reader unnamed: (default).rq/map_manager/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.5;">,topic_name="rq/map_manager/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586099 [0] dq.builtin: => EVERYONE -1711462090.586102 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1104) scanning all wrs of topic rq/map_manager/set_parametersRequest -1711462090.586110 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.6;">,topic_name="rq/map_manager/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1304},adlink_entity_factory=1} -1711462090.586127 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1304 reliable volatile reader unnamed: (default).rq/map_manager/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.6;">,topic_name="rq/map_manager/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586136 [0] dq.builtin: => EVERYONE -1711462090.586139 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1304) scanning all wrs of topic rq/map_manager/set_parameters_atomicallyRequest -1711462090.586147 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.7;">,topic_name="rq/topological_map_manager2/get_topological_mapRequest",type_name="std_srvs::srv::dds_::Trigger_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1504},adlink_entity_factory=1} -1711462090.586163 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_topological_mapRequest/std_srvs::srv::dds_::Trigger_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.7;">,topic_name="rq/topological_map_manager2/get_topological_mapRequest",type_name="std_srvs::srv::dds_::Trigger_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586168 [0] dq.builtin: => EVERYONE -1711462090.586171 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1504) scanning all wrs of topic rq/topological_map_manager2/get_topological_mapRequest -1711462090.586178 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #9: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.8;">,topic_name="rq/topological_map_manager2/get_tagged_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::GetTaggedNodes_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1704},adlink_entity_factory=1} -1711462090.586194 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1704 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_tagged_nodesRequest/topological_navigation_msgs::srv::dds_::GetTaggedNodes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.8;">,topic_name="rq/topological_map_manager2/get_tagged_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::GetTaggedNodes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586202 [0] dq.builtin: => EVERYONE -1711462090.586206 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1704) scanning all wrs of topic rq/topological_map_manager2/get_tagged_nodesRequest -1711462090.586213 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #10: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.9;">,topic_name="rq/topological_map_manager2/get_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::GetTags_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1904},adlink_entity_factory=1} -1711462090.586229 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_tagsRequest/topological_navigation_msgs::srv::dds_::GetTags_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.9;">,topic_name="rq/topological_map_manager2/get_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::GetTags_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586234 [0] dq.builtin: => EVERYONE -1711462090.586238 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1904) scanning all wrs of topic rq/topological_map_manager2/get_tagsRequest -1711462090.586245 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #11: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.a;">,topic_name="rq/topological_map_manager2/get_node_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::GetNodeTags_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1b04},adlink_entity_factory=1} -1711462090.586262 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_node_tagsRequest/topological_navigation_msgs::srv::dds_::GetNodeTags_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.a;">,topic_name="rq/topological_map_manager2/get_node_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::GetNodeTags_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586269 [0] dq.builtin: => EVERYONE -1711462090.586272 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1b04) scanning all wrs of topic rq/topological_map_manager2/get_node_tagsRequest -1711462090.586280 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #12: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.b;">,topic_name="rq/topological_map_manager2/get_edges_between_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1d04},adlink_entity_factory=1} -1711462090.586296 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_edges_between_nodesRequest/topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.b;">,topic_name="rq/topological_map_manager2/get_edges_between_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586303 [0] dq.builtin: => EVERYONE -1711462090.586307 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1d04) scanning all wrs of topic rq/topological_map_manager2/get_edges_between_nodesRequest -1711462090.586314 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #13: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.c;">,topic_name="rq/topological_map_manager2/write_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1f04},adlink_entity_factory=1} -1711462090.586331 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/write_topological_mapRequest/topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.c;">,topic_name="rq/topological_map_manager2/write_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586335 [0] dq.builtin: => EVERYONE -1711462090.586338 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1f04) scanning all wrs of topic rq/topological_map_manager2/write_topological_mapRequest -1711462090.586346 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #38: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rr/topological_map_manager2/clear_topological_nodesReply",type_name="std_srvs::srv::dds_::Empty_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.586363 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4c03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/clear_topological_nodesReply/std_srvs::srv::dds_::Empty_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rr/topological_map_manager2/clear_topological_nodesReply",type_name="std_srvs::srv::dds_::Empty_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.586372 [0] dq.builtin: => EVERYONE -1711462090.586375 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4c03) scanning all rds of topic rr/topological_map_manager2/clear_topological_nodesReply -1711462090.586383 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #14: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.d;">,topic_name="rq/topological_map_manager2/switch_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2104},adlink_entity_factory=1} -1711462090.586400 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/switch_topological_mapRequest/topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.d;">,topic_name="rq/topological_map_manager2/switch_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586404 [0] dq.builtin: => EVERYONE -1711462090.586407 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2104) scanning all wrs of topic rq/topological_map_manager2/switch_topological_mapRequest -1711462090.586415 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #15: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.e;">,topic_name="rq/topological_map_manager2/add_topological_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddNode_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2304},adlink_entity_factory=1} -1711462090.586431 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_topological_nodeRequest/topological_navigation_msgs::srv::dds_::AddNode_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.e;">,topic_name="rq/topological_map_manager2/add_topological_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddNode_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586438 [0] dq.builtin: => EVERYONE -1711462090.586442 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2304) scanning all wrs of topic rq/topological_map_manager2/add_topological_nodeRequest -1711462090.586449 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #16: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.f;">,topic_name="rq/topological_map_manager2/remove_topological_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::RmvNode_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2504},adlink_entity_factory=1} -1711462090.586466 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/remove_topological_nodeRequest/topological_navigation_msgs::srv::dds_::RmvNode_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.f;">,topic_name="rq/topological_map_manager2/remove_topological_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::RmvNode_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586470 [0] dq.builtin: => EVERYONE -1711462090.586474 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2504) scanning all wrs of topic rq/topological_map_manager2/remove_topological_nodeRequest -1711462090.586480 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #17: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.10;">,topic_name="rq/topological_map_manager2/add_edges_between_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2704},adlink_entity_factory=1} -1711462090.586497 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2704 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_edges_between_nodesRequest/topological_navigation_msgs::srv::dds_::AddEdge_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.10;">,topic_name="rq/topological_map_manager2/add_edges_between_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586505 [0] dq.builtin: => EVERYONE -1711462090.586508 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2704) scanning all wrs of topic rq/topological_map_manager2/add_edges_between_nodesRequest -1711462090.586516 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #36: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rq/topological_map_manager2/clear_topological_nodesRequest",type_name="std_srvs::srv::dds_::Empty_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4d04},adlink_entity_factory=1} -1711462090.586533 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/clear_topological_nodesRequest/std_srvs::srv::dds_::Empty_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rq/topological_map_manager2/clear_topological_nodesRequest",type_name="std_srvs::srv::dds_::Empty_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586538 [0] dq.builtin: => EVERYONE -1711462090.586541 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4d04) scanning all wrs of topic rq/topological_map_manager2/clear_topological_nodesRequest -1711462090.586549 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #39: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rr/topological_map_manager2/add_topological_node_multiReply",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.586567 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4e03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_topological_node_multiReply/topological_navigation_msgs::srv::dds_::AddNodeArray_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rr/topological_map_manager2/add_topological_node_multiReply",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.586574 [0] dq.builtin: => EVERYONE -1711462090.586578 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4e03) scanning all rds of topic rr/topological_map_manager2/add_topological_node_multiReply -1711462090.586586 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #37: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rq/topological_map_manager2/add_topological_node_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4f04},adlink_entity_factory=1} -1711462090.586604 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_topological_node_multiRequest/topological_navigation_msgs::srv::dds_::AddNodeArray_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rq/topological_map_manager2/add_topological_node_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586611 [0] dq.builtin: => EVERYONE -1711462090.586614 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4f04) scanning all wrs of topic rq/topological_map_manager2/add_topological_node_multiRequest -1711462090.586621 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #18: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.11;">,topic_name="rq/topological_map_manager2/remove_edgeRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2904},adlink_entity_factory=1} -1711462090.586638 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/remove_edgeRequest/topological_navigation_msgs::srv::dds_::AddEdge_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.11;">,topic_name="rq/topological_map_manager2/remove_edgeRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586642 [0] dq.builtin: => EVERYONE -1711462090.586646 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2904) scanning all wrs of topic rq/topological_map_manager2/remove_edgeRequest -1711462090.586653 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #19: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.12;">,topic_name="rq/topological_map_manager2/add_content_to_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddContent_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2b04},adlink_entity_factory=1} -1711462090.586670 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_content_to_nodeRequest/topological_navigation_msgs::srv::dds_::AddContent_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.12;">,topic_name="rq/topological_map_manager2/add_content_to_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddContent_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586678 [0] dq.builtin: => EVERYONE -1711462090.586681 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2b04) scanning all wrs of topic rq/topological_map_manager2/add_content_to_nodeRequest -1711462090.586689 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #20: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.13;">,topic_name="rq/topological_map_manager2/update_node_nameRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeName_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2d04},adlink_entity_factory=1} -1711462090.586706 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_node_nameRequest/topological_navigation_msgs::srv::dds_::UpdateNodeName_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.13;">,topic_name="rq/topological_map_manager2/update_node_nameRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeName_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586710 [0] dq.builtin: => EVERYONE -1711462090.586713 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2d04) scanning all wrs of topic rq/topological_map_manager2/update_node_nameRequest -1711462090.586721 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #21: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.14;">,topic_name="rq/topological_map_manager2/update_node_poseRequest",type_name="topological_navigation_msgs::srv::dds_::AddNode_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2f04},adlink_entity_factory=1} -1711462090.586738 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_node_poseRequest/topological_navigation_msgs::srv::dds_::AddNode_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.14;">,topic_name="rq/topological_map_manager2/update_node_poseRequest",type_name="topological_navigation_msgs::srv::dds_::AddNode_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586745 [0] dq.builtin: => EVERYONE -1711462090.586748 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2f04) scanning all wrs of topic rq/topological_map_manager2/update_node_poseRequest -1711462090.586757 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #40: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rr/topological_map_manager2/add_edges_between_nodes_multiReply",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.586774 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5003 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_edges_between_nodes_multiReply/topological_navigation_msgs::srv::dds_::AddEdgeArray_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rr/topological_map_manager2/add_edges_between_nodes_multiReply",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.586779 [0] dq.builtin: => EVERYONE -1711462090.586783 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5003) scanning all rds of topic rr/topological_map_manager2/add_edges_between_nodes_multiReply -1711462090.586790 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #38: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rq/topological_map_manager2/add_edges_between_nodes_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5104},adlink_entity_factory=1} -1711462090.586807 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_edges_between_nodes_multiRequest/topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rq/topological_map_manager2/add_edges_between_nodes_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586814 [0] dq.builtin: => EVERYONE -1711462090.586817 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:5104) scanning all wrs of topic rq/topological_map_manager2/add_edges_between_nodes_multiRequest -1711462090.586826 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #22: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.15;">,topic_name="rq/topological_map_manager2/update_node_toleranceRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3104},adlink_entity_factory=1} -1711462090.586842 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_node_toleranceRequest/topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.15;">,topic_name="rq/topological_map_manager2/update_node_toleranceRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586851 [0] dq.builtin: => EVERYONE -1711462090.586857 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3104) scanning all wrs of topic rq/topological_map_manager2/update_node_toleranceRequest -1711462090.586858 [0] gc: gc 0x77041c058960: not yet, shortsleep -1711462090.586865 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #23: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.16;">,topic_name="rq/topological_map_manager2/modify_node_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::ModifyTag_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3304},adlink_entity_factory=1} -1711462090.586887 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/modify_node_tagsRequest/topological_navigation_msgs::srv::dds_::ModifyTag_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.16;">,topic_name="rq/topological_map_manager2/modify_node_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::ModifyTag_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586892 [0] dq.builtin: => EVERYONE -1711462090.586895 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3304) scanning all wrs of topic rq/topological_map_manager2/modify_node_tagsRequest -1711462090.586902 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #24: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.17;">,topic_name="rq/topological_map_manager2/add_tag_to_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddTag_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3504},adlink_entity_factory=1} -1711462090.586920 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_tag_to_nodeRequest/topological_navigation_msgs::srv::dds_::AddTag_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.17;">,topic_name="rq/topological_map_manager2/add_tag_to_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddTag_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586925 [0] dq.builtin: => EVERYONE -1711462090.586928 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3504) scanning all wrs of topic rq/topological_map_manager2/add_tag_to_nodeRequest -1711462090.586936 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #25: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.18;">,topic_name="rq/topological_map_manager2/rm_tag_from_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddTag_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3704},adlink_entity_factory=1} -1711462090.586953 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3704 reliable volatile reader unnamed: (default).rq/topological_map_manager2/rm_tag_from_nodeRequest/topological_navigation_msgs::srv::dds_::AddTag_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.18;">,topic_name="rq/topological_map_manager2/rm_tag_from_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddTag_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.586961 [0] dq.builtin: => EVERYONE -1711462090.586964 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3704) scanning all wrs of topic rq/topological_map_manager2/rm_tag_from_nodeRequest -1711462090.586973 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #41: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rr/topological_map_manager2/add_param_to_edge_config_multiReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.586989 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) suppressed, resched in 0.1 s (min-ack 2!, avail-seq 2, xmit 2) -1711462090.586990 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5203 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_param_to_edge_config_multiReply/topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rr/topological_map_manager2/add_param_to_edge_config_multiReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587005 [0] dq.builtin: => EVERYONE -1711462090.587009 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5203) scanning all rds of topic rr/topological_map_manager2/add_param_to_edge_config_multiReply -1711462090.587017 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #26: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.19;">,topic_name="rq/topological_map_manager2/add_param_to_edge_configRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3904},adlink_entity_factory=1} -1711462090.587034 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_param_to_edge_configRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.19;">,topic_name="rq/topological_map_manager2/add_param_to_edge_configRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587039 [0] dq.builtin: => EVERYONE -1711462090.587043 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3904) scanning all wrs of topic rq/topological_map_manager2/add_param_to_edge_configRequest -1711462090.587051 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #27: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1a;">,topic_name="rq/topological_map_manager2/rm_param_from_edge_configRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3b04},adlink_entity_factory=1} -1711462090.587068 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/rm_param_from_edge_configRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1a;">,topic_name="rq/topological_map_manager2/rm_param_from_edge_configRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587075 [0] dq.builtin: => EVERYONE -1711462090.587079 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3b04) scanning all wrs of topic rq/topological_map_manager2/rm_param_from_edge_configRequest -1711462090.587087 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #28: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1b;">,topic_name="rq/topological_map_manager2/rm_param_from_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3d04},adlink_entity_factory=1} -1711462090.587104 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/rm_param_from_topological_mapRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1b;">,topic_name="rq/topological_map_manager2/rm_param_from_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587109 [0] dq.builtin: => EVERYONE -1711462090.587112 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3d04) scanning all wrs of topic rq/topological_map_manager2/rm_param_from_topological_mapRequest -1711462090.587121 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #29: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1c;">,topic_name="rq/topological_map_manager2/update_node_restrictionsRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3f04},adlink_entity_factory=1} -1711462090.587138 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_node_restrictionsRequest/topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1c;">,topic_name="rq/topological_map_manager2/update_node_restrictionsRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587145 [0] dq.builtin: => EVERYONE -1711462090.587148 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3f04) scanning all wrs of topic rq/topological_map_manager2/update_node_restrictionsRequest -1711462090.587156 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #39: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rq/topological_map_manager2/add_param_to_edge_config_multiRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5304},adlink_entity_factory=1} -1711462090.587173 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_param_to_edge_config_multiRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rq/topological_map_manager2/add_param_to_edge_config_multiRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587178 [0] dq.builtin: => EVERYONE -1711462090.587181 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:5304) scanning all wrs of topic rq/topological_map_manager2/add_param_to_edge_config_multiRequest -1711462090.587190 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #1: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587206 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:403 reliable transient-local writer unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587214 [0] dq.builtin: => EVERYONE -1711462090.587217 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:403) scanning all rds of topic ros_discovery_info -1711462090.587221 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:504 init_acknack_count = 1 -1711462090.587223 [0] dq.builtin: reader_add_connection(pwr 110cd0d:56a27910:57318171:403 rd 110aba1:7b19badd:ce0adb73:504) -1711462090.587231 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:56a27910:57318171:403 rd 110aba1:7b19badd:ce0adb73:504) - out-of-sync -1711462090.587240 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #2: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",durability=1,durability_service=0:0:1000:-1:-1:-1,reliability=1:9223372036854775807,lifespan=10000000000,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587255 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:603 reliable transient-local writer unnamed: (default).rt/rosout/rcl_interfaces::msg::dds_::Log_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1000:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=10000000000,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587260 [0] dq.builtin: => EVERYONE -1711462090.587267 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:603) scanning all rds of topic rt/rosout -1711462090.587275 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587290 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587295 [0] dq.builtin: => EVERYONE -1711462090.587298 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:703) scanning all rds of topic rt/parameter_events -1711462090.587305 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.1;">,topic_name="rr/test_node/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587321 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:803 reliable volatile writer unnamed: (default).rr/test_node/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.1;">,topic_name="rr/test_node/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587329 [0] dq.builtin: => EVERYONE -1711462090.587332 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:803) scanning all rds of topic rr/test_node/describe_parametersReply -1711462090.587340 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.2;">,topic_name="rr/test_node/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587354 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:a03 reliable volatile writer unnamed: (default).rr/test_node/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.2;">,topic_name="rr/test_node/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587358 [0] dq.builtin: => EVERYONE -1711462090.587361 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:a03) scanning all rds of topic rr/test_node/get_parametersReply -1711462090.587368 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #30: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1d;">,topic_name="rq/topological_map_manager2/update_edge_restrictionsRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4104},adlink_entity_factory=1} -1711462090.587386 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_edge_restrictionsRequest/topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1d;">,topic_name="rq/topological_map_manager2/update_edge_restrictionsRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587396 [0] dq.builtin: => EVERYONE -1711462090.587399 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4104) scanning all wrs of topic rq/topological_map_manager2/update_edge_restrictionsRequest -1711462090.587407 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #31: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1e;">,topic_name="rq/topological_map_manager2/update_edgeRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdge_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4304},adlink_entity_factory=1} -1711462090.587421 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_edgeRequest/topological_navigation_msgs::srv::dds_::UpdateEdge_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1e;">,topic_name="rq/topological_map_manager2/update_edgeRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdge_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587426 [0] dq.builtin: => EVERYONE -1711462090.587429 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4304) scanning all wrs of topic rq/topological_map_manager2/update_edgeRequest -1711462090.587435 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #32: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1f;">,topic_name="rq/topological_map_manager2/update_actionRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateAction_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4504},adlink_entity_factory=1} -1711462090.587449 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_actionRequest/topological_navigation_msgs::srv::dds_::UpdateAction_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1f;">,topic_name="rq/topological_map_manager2/update_actionRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateAction_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587459 [0] dq.builtin: => EVERYONE -1711462090.587462 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4504) scanning all wrs of topic rq/topological_map_manager2/update_actionRequest -1711462090.587468 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #33: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.20;">,topic_name="rq/topological_map_manager2/add_datumRequest",type_name="topological_navigation_msgs::srv::dds_::AddDatum_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4704},adlink_entity_factory=1} -1711462090.587482 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4704 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_datumRequest/topological_navigation_msgs::srv::dds_::AddDatum_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.20;">,topic_name="rq/topological_map_manager2/add_datumRequest",type_name="topological_navigation_msgs::srv::dds_::AddDatum_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587486 [0] dq.builtin: => EVERYONE -1711462090.587492 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4704) scanning all wrs of topic rq/topological_map_manager2/add_datumRequest -1711462090.587499 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #34: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rq/topological_map_manager2/update_fail_policyRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4904},adlink_entity_factory=1} -1711462090.587503 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_fail_policyRequest/topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_ p(open) known -1711462090.587518 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rq/topological_map_manager2/update_fail_policyRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587526 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #35: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rq/topological_map_manager2/set_node_influence_zoneRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4b04},adlink_entity_factory=1} -1711462090.587530 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/set_node_influence_zoneRequest/topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_ p(open) known -1711462090.587543 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rq/topological_map_manager2/set_node_influence_zoneRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587549 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #36: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rq/topological_map_manager2/clear_topological_nodesRequest",type_name="std_srvs::srv::dds_::Empty_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4d04},adlink_entity_factory=1} -1711462090.587553 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/clear_topological_nodesRequest/std_srvs::srv::dds_::Empty_Request_ p(open) known -1711462090.587566 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rq/topological_map_manager2/clear_topological_nodesRequest",type_name="std_srvs::srv::dds_::Empty_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587573 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #37: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rq/topological_map_manager2/add_topological_node_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4f04},adlink_entity_factory=1} -1711462090.587576 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_topological_node_multiRequest/topological_navigation_msgs::srv::dds_::AddNodeArray_Request_ p(open) known -1711462090.587588 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rq/topological_map_manager2/add_topological_node_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587596 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #38: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rq/topological_map_manager2/add_edges_between_nodes_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5104},adlink_entity_factory=1} -1711462090.587600 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_edges_between_nodes_multiRequest/topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_ p(open) known -1711462090.587613 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rq/topological_map_manager2/add_edges_between_nodes_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587621 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #39: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rq/topological_map_manager2/add_param_to_edge_config_multiRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5304},adlink_entity_factory=1} -1711462090.587625 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_param_to_edge_config_multiRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_ p(open) known -1711462090.587638 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rq/topological_map_manager2/add_param_to_edge_config_multiRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587645 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #42: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.27;">,topic_name="rr/topological_map_manager2/set_node_influence_zone_multiReply",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587660 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/set_node_influence_zone_multiReply/topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.27;">,topic_name="rr/topological_map_manager2/set_node_influence_zone_multiReply",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587666 [0] dq.builtin: => EVERYONE -1711462090.587670 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5403) scanning all rds of topic rr/topological_map_manager2/set_node_influence_zone_multiReply -1711462090.587678 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.3;">,topic_name="rr/test_node/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587692 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:c03 reliable volatile writer unnamed: (default).rr/test_node/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.3;">,topic_name="rr/test_node/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587700 [0] dq.builtin: => EVERYONE -1711462090.587703 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:c03) scanning all rds of topic rr/test_node/get_parameter_typesReply -1711462090.587711 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.4;">,topic_name="rr/test_node/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587725 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:e03 reliable volatile writer unnamed: (default).rr/test_node/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.4;">,topic_name="rr/test_node/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587729 [0] dq.builtin: => EVERYONE -1711462090.587732 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:e03) scanning all rds of topic rr/test_node/list_parametersReply -1711462090.587739 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.5;">,topic_name="rr/test_node/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587753 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:1003 reliable volatile writer unnamed: (default).rr/test_node/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.5;">,topic_name="rr/test_node/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587760 [0] dq.builtin: => EVERYONE -1711462090.587763 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:1003) scanning all rds of topic rr/test_node/set_parametersReply -1711462090.587771 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.6;">,topic_name="rr/test_node/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587786 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:1203 reliable volatile writer unnamed: (default).rr/test_node/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.6;">,topic_name="rr/test_node/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.587791 [0] dq.builtin: => EVERYONE -1711462090.587794 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:56a27910:57318171:1203) scanning all rds of topic rr/test_node/set_parameters_atomicallyReply -1711462090.587802 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #40: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.27;">,topic_name="rq/topological_map_manager2/set_node_influence_zone_multiRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5504},adlink_entity_factory=1} -1711462090.587817 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/set_node_influence_zone_multiRequest/topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.27;">,topic_name="rq/topological_map_manager2/set_node_influence_zone_multiRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587825 [0] dq.builtin: => EVERYONE -1711462090.587828 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:5504) scanning all wrs of topic rq/topological_map_manager2/set_node_influence_zone_multiRequest -1711462090.587833 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:200c2 #1: ST0 DCPSParticipantMessage/ParticipantMessageData:{110d7b4:17c5b8c5:94bd0ff4:1}:1<0> -1711462090.587836 [0] dq.builtin: PMD ST0 pp 110d7b4:17c5b8c5:94bd0ff4 kind 1 data 1 -1711462090.587843 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #1: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,history=1:-1,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:504},adlink_entity_factory=1} -1711462090.587859 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:504 reliable transient-local reader unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=1:-1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587864 [0] dq.builtin: => EVERYONE -1711462090.587868 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:504) scanning all wrs of topic ros_discovery_info -1711462090.587871 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110cd0d:56a27910:57318171:504) -1711462090.587874 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110cd0d:56a27910:57318171:504) - ack seq 2 -1711462090.587882 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 udp/10.101.12.71:7417@2 -1711462090.587887 [0] dq.builtin: reduced nlocs=5 -1711462090.587890 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.587893 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462090.587895 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.587898 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.587900 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7413@2 1 -> 0 -1711462090.587903 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.587905 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.587907 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7417@2 3 -> 0 -1711462090.587909 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.587911 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.587914 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7415@2 2 -> 0 -1711462090.587917 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.587920 [0] dq.builtin: a b c d -1711462090.587924 [0] dq.builtin: loc 0 = udp/10.101.12.71:7411@2 1 { +u .. .. .. } -1711462090.587927 [0] dq.builtin: loc 1 = udp/10.101.12.71:7413@2 1 { .. +u .. .. } -1711462090.587931 [0] dq.builtin: loc 2 = udp/10.101.12.71:7415@2 1 { .. .. .. +u } -1711462090.587934 [0] dq.builtin: loc 3 = udp/10.101.12.71:7417@2 1 { .. .. +u .. } -1711462090.587938 [0] dq.builtin: loc 4 = udp/239.255.0.1:7401@2 -1 { +1 +1 +1 +1 } -1711462090.587939 [0] dq.builtin: best = 4 -1711462090.587942 [0] dq.builtin: simple udp/239.255.0.1:7401@2 -1711462090.587946 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462090.587957 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.1;">,topic_name="rq/test_node/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:904},adlink_entity_factory=1} -1711462090.587965 [0] gc: gc 0x77041c058960: deleting -1711462090.587972 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:904 reliable volatile reader unnamed: (default).rq/test_node/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.1;">,topic_name="rq/test_node/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.587978 [0] dq.builtin: => EVERYONE -1711462090.587981 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:904) scanning all wrs of topic rq/test_node/describe_parametersRequest -1711462090.587988 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.2;">,topic_name="rq/test_node/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:b04},adlink_entity_factory=1} -1711462090.588005 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:b04 reliable volatile reader unnamed: (default).rq/test_node/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.2;">,topic_name="rq/test_node/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588010 [0] dq.builtin: => EVERYONE -1711462090.588013 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:b04) scanning all wrs of topic rq/test_node/get_parametersRequest -1711462090.588020 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.3;">,topic_name="rq/test_node/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:d04},adlink_entity_factory=1} -1711462090.588038 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:d04 reliable volatile reader unnamed: (default).rq/test_node/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.3;">,topic_name="rq/test_node/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588042 [0] dq.builtin: => EVERYONE -1711462090.588045 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:d04) scanning all wrs of topic rq/test_node/get_parameter_typesRequest -1711462090.588053 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.4;">,topic_name="rq/test_node/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:f04},adlink_entity_factory=1} -1711462090.588069 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:f04 reliable volatile reader unnamed: (default).rq/test_node/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.4;">,topic_name="rq/test_node/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588079 [0] dq.builtin: => EVERYONE -1711462090.588084 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:f04) scanning all wrs of topic rq/test_node/list_parametersRequest -1711462090.588090 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.5;">,topic_name="rq/test_node/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:1104},adlink_entity_factory=1} -1711462090.588103 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:1104 reliable volatile reader unnamed: (default).rq/test_node/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.5;">,topic_name="rq/test_node/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588107 [0] dq.builtin: => EVERYONE -1711462090.588113 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:1104) scanning all wrs of topic rq/test_node/set_parametersRequest -1711462090.588119 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.6;">,topic_name="rq/test_node/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:1304},adlink_entity_factory=1} -1711462090.588136 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:1304 reliable volatile reader unnamed: (default).rq/test_node/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.56.a2.79.10.57.31.81.71.0.0.0.6;">,topic_name="rq/test_node/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588150 [0] dq.builtin: => EVERYONE -1711462090.588168 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:1304) scanning all wrs of topic rq/test_node/set_parameters_atomicallyRequest -1711462090.588180 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:1404},adlink_entity_factory=1} -1711462090.588195 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:1404 reliable volatile reader unnamed: (default).rt/topological_map_2/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588207 [0] dq.builtin: => EVERYONE -1711462090.588211 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:1404) scanning all wrs of topic rt/topological_map_2 -1711462090.588217 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #9: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/closest_node",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:1504},adlink_entity_factory=1} -1711462090.588228 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:1504 reliable volatile reader unnamed: (default).rt/closest_node/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/closest_node",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588231 [0] dq.builtin: => EVERYONE -1711462090.588234 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:1504) scanning all wrs of topic rt/closest_node -1711462090.588240 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:4c2 #10: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/current_node",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:56a27910:57318171:1604},adlink_entity_factory=1} -1711462090.588249 [0] dq.builtin: SEDP ST0 110cd0d:56a27910:57318171:1604 reliable volatile reader unnamed: (default).rt/current_node/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/current_node",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588252 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1ae885c0 reg?no refc 1 rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_) -1711462090.588254 [0] dq.builtin: => EVERYONE -1711462090.588259 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.588262 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:56a27910:57318171:1604) scanning all wrs of topic rt/current_node -1711462090.588264 [0] python3: create_and_lock_ktopic: ktp 0x653a1b4aa9b0 -1711462090.588267 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:200c2 #1: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:56a27910:57318171:1}:1<0> -1711462090.588268 [0] python3: dds_create_topic_generic: register new sertype 0x653a1ae885c0 -1711462090.588269 [0] dq.builtin: PMD ST0 pp 110cd0d:56a27910:57318171 kind 1 data 1 -1711462090.588275 [0] python3: dds_create_topic_generic: new topic 287716525 -1711462090.588277 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:3c2 #1: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588289 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:703, (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_) -1711462090.588290 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:403 reliable transient-local writer unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588296 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:703 @ 0x653a1b3ea6c4) user 8 builtin 12 -1711462090.588298 [0] dq.builtin: => EVERYONE -1711462090.588302 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:403) scanning all rds of topic ros_discovery_info -1711462090.588306 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:504 init_acknack_count = 1 -1711462090.588308 [0] dq.builtin: reader_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:403 rd 110aba1:7b19badd:ce0adb73:504) -1711462090.588311 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:e3b81b8d:1ccb65b1:403 rd 110aba1:7b19badd:ce0adb73:504) - out-of-sync -1711462090.588314 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:703 QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588316 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588322 [0] python3: => EVERYONE -1711462090.588325 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588329 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:703) scanning all prds of topic rt/parameter_events -1711462090.588332 [0] dq.builtin: => EVERYONE -1711462090.588334 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:703) scanning all rds of topic rt/parameter_events -1711462090.588335 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:703) scanning all rds of topic rt/parameter_events -1711462090.588342 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.1;">,topic_name="rr/test_node/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588352 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:803 reliable volatile writer unnamed: (default).rr/test_node/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.1;">,topic_name="rr/test_node/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588353 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588355 [0] dq.builtin: => EVERYONE -1711462090.588358 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:803) scanning all rds of topic rr/test_node/describe_parametersReply -1711462090.588360 [0] python3: non-timed queue now has 1 items -1711462090.588365 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.2;">,topic_name="rr/test_node/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588367 [0] tev: xpack_addmsg 0x770414001390 0x653a1b43cf70 0(data(110aba1:7b19badd:ce0adb73:3c2:#3/1)): niov 0 sz 0 => now niov 3 sz 224 -1711462090.588374 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:a03 reliable volatile writer unnamed: (default).rr/test_node/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.2;">,topic_name="rr/test_node/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588380 [0] dq.builtin: => EVERYONE -1711462090.588382 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:a03) scanning all rds of topic rr/test_node/get_parametersReply -1711462090.588386 [0] tev: nn_xpack_send 224: 0x77041400139c:20 0x653a1aec0a18:36 0x653a1b380e74:168 [ udp/239.255.0.1:7400@2 ] -1711462090.588388 [0] tev: traffic-xmit (1) 224 -1711462090.588390 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.3;">,topic_name="rr/test_node/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588391 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #3: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}}}}} -1711462090.588391 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 224 from udp/10.101.12.71:52025 -1711462090.588400 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:c03 reliable volatile writer unnamed: (default).rr/test_node/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.3;">,topic_name="rr/test_node/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588409 [0] recv: INFOTS(1711462090.588344046) -1711462090.588413 [0] dq.builtin: => EVERYONE -1711462090.588404 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b380cc0 0(data(110aba1:7b19badd:ce0adb73:403:#3/1)): niov 0 sz 0 => now niov 3 sz 184 -1711462090.588416 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #3 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.588422 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 4 seq-eq-max 0 seq 3 maxseq 2) -1711462090.588418 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:c03) scanning all rds of topic rr/test_node/get_parameter_typesReply -1711462090.588433 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) piggybacked, resched in 0.000571083 s (min-ack 2!, avail-seq 3, xmit 2) -1711462090.588437 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b382310 0(control): niov 3 sz 184 => now niov 4 sz 216 -1711462090.588439 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.4;">,topic_name="rr/test_node/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588457 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:e03 reliable volatile writer unnamed: (default).rr/test_node/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.4;">,topic_name="rr/test_node/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588461 [0] dq.builtin: => EVERYONE -1711462090.588465 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:e03) scanning all rds of topic rr/test_node/list_parametersReply -1711462090.588471 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 216 from udp/10.101.12.71:52025 -1711462090.588473 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.5;">,topic_name="rr/test_node/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588473 [0] python3: nn_xpack_send 216: 0x653a1add92cc:20 0x653a1b381278:36 0x653a1b3811c0:128 0x653a1b382408:32 [ udp/239.255.0.1:7401@2 ] -1711462090.588477 [0] recvMC: INFOTS(1711462090.588369545) -1711462090.588487 [0] python3: traffic-xmit (1) 216 -1711462090.588489 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:1003 reliable volatile writer unnamed: (default).rr/test_node/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.5;">,topic_name="rr/test_node/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588494 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #3 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.588499 [0] python3: => EVERYONE -1711462090.588511 [0] recvMC: HEARTBEAT(#4:3..3 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.588516 [0] dq.builtin: => EVERYONE -1711462090.588520 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:1003) scanning all rds of topic rr/test_node/set_parametersReply -1711462090.588528 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.6;">,topic_name="rr/test_node/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588540 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 68 from udp/10.101.12.71:44991 -1711462090.588543 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:1203 reliable volatile writer unnamed: (default).rr/test_node/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.6;">,topic_name="rr/test_node/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588545 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.588547 [0] dq.builtin: => EVERYONE -1711462090.588549 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:e3b81b8d:1ccb65b1:1203) scanning all rds of topic rr/test_node/set_parameters_atomicallyReply -1711462090.588554 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #1: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,history=1:-1,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:504},adlink_entity_factory=1} -1711462090.588555 [0] recvUC: ACKNACK(F#1:3/1:1 L(:1c1 16938.248048) 110443d:bb7f10a5:18901533:504 -> 110aba1:7b19badd:ce0adb73:403 complying RX3non-timed queue now has 1 items -1711462090.588561 [0] recvUC: rexmit#1 maxseq:3<3<=3defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110443d:bb7f10a5:18901533:504 - queue for transmit -1711462090.588564 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:504 reliable transient-local reader unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=1:-1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588565 [0] recvUC: ) -1711462090.588562 [0] tev: xpack_addmsg 0x770414001390 0x770404001910 0(rexmit(110aba1:7b19badd:ce0adb73:403:#3/1)): niov 0 sz 0 => now niov 3 sz 200 -1711462090.588568 [0] recvUC: send_deferred_heartbeat: 3aa8b43931dec43 -> 92e6e35aff0ad2b8 - queue for transmit -1711462090.588571 [0] dq.builtin: => EVERYONE -1711462090.588572 [0] recvUC: non-timed queue now has 1 items -1711462090.588574 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:504) scanning all wrs of topic ros_discovery_info -1711462090.588576 [0] tev: nn_xpack_send 200: 0x77041400139c:20 0x7704040019f8:52 0x653a1b3811c0:128 [ udp/10.101.12.71:7417@2 ] -1711462090.588577 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110cd0d:e3b81b8d:1ccb65b1:504) -1711462090.588577 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462090.588579 [0] tev: traffic-xmit (1) 200 -1711462090.588582 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.588585 [0] tev: xpack_addmsg 0x770414001390 0x770404001a90 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.588585 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110cd0d:e3b81b8d:1ccb65b1:504) - ack seq 3 -1711462090.588589 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x770404001b78:48 [ udp/10.101.12.71:7417@2 ] -1711462090.588591 [0] tev: traffic-xmit (1) 68 -1711462090.588593 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 udp/10.101.12.71:7417@2 -1711462090.588595 [0] dq.builtin: reduced nlocs=5 -1711462090.588597 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.588599 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462090.588601 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.588602 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.588604 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462090.588605 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.588607 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.588608 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7413@2 1 -> 0 -1711462090.588610 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.588611 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.588613 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7417@2 3 -> 0 -1711462090.588615 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.588616 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.588618 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7415@2 2 -> 0 -1711462090.588619 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.588622 [0] dq.builtin: a b c d e -1711462090.588625 [0] dq.builtin: loc 0 = udp/10.101.12.71:7411@2 0 { +u +u .. .. .. } -1711462090.588629 [0] dq.builtin: loc 1 = udp/10.101.12.71:7413@2 1 { .. .. +u .. .. } -1711462090.588633 [0] dq.builtin: loc 2 = udp/10.101.12.71:7415@2 1 { .. .. .. .. +u } -1711462090.588637 [0] dq.builtin: loc 3 = udp/10.101.12.71:7417@2 1 { .. .. .. +u .. } -1711462090.588642 [0] dq.builtin: loc 4 = udp/239.255.0.1:7401@2 -2 { +1 +1 +1 +1 +1 } -1711462090.588644 [0] dq.builtin: best = 4 -1711462090.588646 [0] dq.builtin: simple udp/239.255.0.1:7401@2 -1711462090.588649 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462090.588654 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.1;">,topic_name="rq/test_node/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:904},adlink_entity_factory=1} -1711462090.588657 [0] recvUC: ACKNACK(F#2:4/0: L(:1c1 16938.248085) 110e21c:d0762c48:a9f0fb28:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0 setting-has-replied-to-hb happy-now) -1711462090.588663 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.588664 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:904 reliable volatile reader unnamed: (default).rq/test_node/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.1;">,topic_name="rq/test_node/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588667 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.588675 [0] dq.builtin: => EVERYONE -1711462090.588676 [0] recvUC: ACKNACK(F#2:4/0: L(:1c1 16938.248170) 110443d:bb7f10a5:18901533:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0 setting-has-replied-to-hb happy-now) -1711462090.588678 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:904) scanning all wrs of topic rq/test_node/describe_parametersRequest -1711462090.588685 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.2;">,topic_name="rq/test_node/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:b04},adlink_entity_factory=1} -1711462090.588694 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:b04 reliable volatile reader unnamed: (default).rq/test_node/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.2;">,topic_name="rq/test_node/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588697 [0] dq.builtin: => EVERYONE -1711462090.588700 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:b04) scanning all wrs of topic rq/test_node/get_parametersRequest -1711462090.588704 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.3;">,topic_name="rq/test_node/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:d04},adlink_entity_factory=1} -1711462090.588712 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:d04 reliable volatile reader unnamed: (default).rq/test_node/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.3;">,topic_name="rq/test_node/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588715 [0] dq.builtin: => EVERYONE -1711462090.588719 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:d04) scanning all wrs of topic rq/test_node/get_parameter_typesRequest -1711462090.588724 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.4;">,topic_name="rq/test_node/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:f04},adlink_entity_factory=1} -1711462090.588732 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:f04 reliable volatile reader unnamed: (default).rq/test_node/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.4;">,topic_name="rq/test_node/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588736 [0] dq.builtin: => EVERYONE -1711462090.588738 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:f04) scanning all wrs of topic rq/test_node/list_parametersRequest -1711462090.588742 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.5;">,topic_name="rq/test_node/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1104},adlink_entity_factory=1} -1711462090.588751 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:1104 reliable volatile reader unnamed: (default).rq/test_node/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.5;">,topic_name="rq/test_node/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588761 [0] dq.builtin: => EVERYONE -1711462090.588763 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:1104) scanning all wrs of topic rq/test_node/set_parametersRequest -1711462090.588765 [0] gc: gc 0x77041c074ce0: not yet, shortsleep -1711462090.588768 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.6;">,topic_name="rq/test_node/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1304},adlink_entity_factory=1} -1711462090.588779 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:1304 reliable volatile reader unnamed: (default).rq/test_node/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.cd.d.e3.b8.1b.8d.1c.cb.65.b1.0.0.0.6;">,topic_name="rq/test_node/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588782 [0] dq.builtin: => EVERYONE -1711462090.588784 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:1304) scanning all wrs of topic rq/test_node/set_parameters_atomicallyRequest -1711462090.588788 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1404},adlink_entity_factory=1} -1711462090.588797 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:1404 reliable volatile reader unnamed: (default).rt/topological_map_2/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588802 [0] dq.builtin: => EVERYONE -1711462090.588804 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:1404) scanning all wrs of topic rt/topological_map_2 -1711462090.588808 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #9: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/closest_node",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1504},adlink_entity_factory=1} -1711462090.588816 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:1504 reliable volatile reader unnamed: (default).rt/closest_node/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/closest_node",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588819 [0] dq.builtin: => EVERYONE -1711462090.588821 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:1504) scanning all wrs of topic rt/closest_node -1711462090.588825 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:4c2 #10: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/current_node",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1604},adlink_entity_factory=1} -1711462090.588833 [0] dq.builtin: SEDP ST0 110cd0d:e3b81b8d:1ccb65b1:1604 reliable volatile reader unnamed: (default).rt/current_node/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/current_node",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.588838 [0] dq.builtin: => EVERYONE -1711462090.588840 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:e3b81b8d:1ccb65b1:1604) scanning all wrs of topic rt/current_node -1711462090.588843 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:e3b81b8d:1ccb65b1:1}:1<0> -1711462090.588845 [0] dq.builtin: PMD ST0 pp 110cd0d:e3b81b8d:1ccb65b1 kind 1 data 1 -1711462090.588849 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:3c2 #1: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588857 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:403 reliable transient-local writer unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588861 [0] dq.builtin: => EVERYONE -1711462090.588863 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:403) scanning all rds of topic ros_discovery_info -1711462090.588865 [0] dq.builtin: reader 110aba1:7b19badd:ce0adb73:504 init_acknack_count = 1 -1711462090.588866 [0] dq.builtin: reader_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:403 rd 110aba1:7b19badd:ce0adb73:504) -1711462090.588869 [0] dq.builtin: proxy_writer_add_connection(pwr 110cd0d:79d6eeac:ea4a8907:403 rd 110aba1:7b19badd:ce0adb73:504) - out-of-sync -1711462090.588873 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588881 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588887 [0] dq.builtin: => EVERYONE -1711462090.588889 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:703) scanning all rds of topic rt/parameter_events -1711462090.588894 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.1;">,topic_name="rr/test_node/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588906 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:803 reliable volatile writer unnamed: (default).rr/test_node/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.1;">,topic_name="rr/test_node/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588910 [0] dq.builtin: => EVERYONE -1711462090.588912 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:803) scanning all rds of topic rr/test_node/describe_parametersReply -1711462090.588916 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.2;">,topic_name="rr/test_node/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588924 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:a03 reliable volatile writer unnamed: (default).rr/test_node/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.2;">,topic_name="rr/test_node/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588929 [0] dq.builtin: => EVERYONE -1711462090.588930 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:703 #1: ST0 rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_:{{1711462090,588851954},"/topological_navigation_core",{{"use_sim_time",{1,0,0,0.000000,"",{{}},{{}},{{}},{{}},{{}}}}},{},{}} -1711462090.588931 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:a03) scanning all rds of topic rr/test_node/get_parametersReply -1711462090.588938 [0] python3: => EVERYONE -1711462090.588945 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.3;">,topic_name="rr/test_node/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588957 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:c03 reliable volatile writer unnamed: (default).rr/test_node/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.3;">,topic_name="rr/test_node/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588960 [0] dq.builtin: => EVERYONE -1711462090.588962 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:c03) scanning all rds of topic rr/test_node/get_parameter_typesReply -1711462090.588967 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.4;">,topic_name="rr/test_node/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588975 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:e03 reliable volatile writer unnamed: (default).rr/test_node/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.4;">,topic_name="rr/test_node/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588980 [0] dq.builtin: => EVERYONE -1711462090.588982 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:e03) scanning all rds of topic rr/test_node/list_parametersReply -1711462090.588986 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.5;">,topic_name="rr/test_node/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588994 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:1003 reliable volatile writer unnamed: (default).rr/test_node/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.5;">,topic_name="rr/test_node/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588999 [0] dq.builtin: => EVERYONE -1711462090.589001 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:1003) scanning all rds of topic rr/test_node/set_parametersReply -1711462090.589006 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.6;">,topic_name="rr/test_node/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589014 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:1203 reliable volatile writer unnamed: (default).rr/test_node/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.6;">,topic_name="rr/test_node/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589017 [0] dq.builtin: => EVERYONE -1711462090.589019 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110cd0d:79d6eeac:ea4a8907:1203) scanning all rds of topic rr/test_node/set_parameters_atomicallyReply -1711462090.589023 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #1: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,history=1:-1,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:504},adlink_entity_factory=1} -1711462090.589032 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:504 reliable transient-local reader unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=1:-1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589037 [0] dq.builtin: => EVERYONE -1711462090.589036 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) suppressed, resched in 0.1 s (min-ack 2!, avail-seq 3, xmit 3) -1711462090.589041 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:504) scanning all wrs of topic ros_discovery_info -1711462090.589046 [0] dq.builtin: proxy_reader_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110cd0d:79d6eeac:ea4a8907:504) -1711462090.589047 [0] dq.builtin: writer_add_connection(wr 110aba1:7b19badd:ce0adb73:403 prd 110cd0d:79d6eeac:ea4a8907:504) - ack seq 3 -1711462090.589052 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 udp/10.101.12.71:7417@2 -1711462090.589054 [0] dq.builtin: reduced nlocs=5 -1711462090.589055 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.589057 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462090.589058 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.589059 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.589061 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462090.589063 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.589064 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.589066 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462090.589067 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.589068 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.589070 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7413@2 1 -> 0 -1711462090.589071 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.589073 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.589074 [0] dq.builtin: rdidx 4 lidx udp/10.101.12.71:7417@2 3 -> 0 -1711462090.589076 [0] dq.builtin: rdidx 4 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.589077 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.589078 [0] dq.builtin: rdidx 5 lidx udp/10.101.12.71:7415@2 2 -> 0 -1711462090.589080 [0] dq.builtin: rdidx 5 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.589082 [0] dq.builtin: a b c d e f -1711462090.589084 [0] dq.builtin: loc 0 = udp/10.101.12.71:7411@2 -1 { +u +u +u .. .. .. } -1711462090.589086 [0] dq.builtin: loc 1 = udp/10.101.12.71:7413@2 1 { .. .. .. +u .. .. } -1711462090.589088 [0] dq.builtin: loc 2 = udp/10.101.12.71:7415@2 1 { .. .. .. .. .. +u } -1711462090.589090 [0] dq.builtin: loc 3 = udp/10.101.12.71:7417@2 1 { .. .. .. .. +u .. } -1711462090.589092 [0] dq.builtin: loc 4 = udp/239.255.0.1:7401@2 -3 { +1 +1 +1 +1 +1 +1 } -1711462090.589094 [0] dq.builtin: best = 4 -1711462090.589095 [0] dq.builtin: simple udp/239.255.0.1:7401@2 -1711462090.589098 [0] dq.builtin: ddsi_rebuild_writer_addrset(110aba1:7b19badd:ce0adb73:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462090.589103 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.1;">,topic_name="rq/test_node/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:904},adlink_entity_factory=1} -1711462090.589111 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:904 reliable volatile reader unnamed: (default).rq/test_node/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.1;">,topic_name="rq/test_node/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589114 [0] dq.builtin: => EVERYONE -1711462090.589116 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:904) scanning all wrs of topic rq/test_node/describe_parametersRequest -1711462090.589121 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.2;">,topic_name="rq/test_node/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:b04},adlink_entity_factory=1} -1711462090.589129 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:b04 reliable volatile reader unnamed: (default).rq/test_node/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.2;">,topic_name="rq/test_node/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589135 [0] dq.builtin: => EVERYONE -1711462090.589137 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:b04) scanning all wrs of topic rq/test_node/get_parametersRequest -1711462090.589141 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.3;">,topic_name="rq/test_node/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:d04},adlink_entity_factory=1} -1711462090.589149 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:d04 reliable volatile reader unnamed: (default).rq/test_node/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.3;">,topic_name="rq/test_node/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589152 [0] dq.builtin: => EVERYONE -1711462090.589154 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:d04) scanning all wrs of topic rq/test_node/get_parameter_typesRequest -1711462090.589158 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.4;">,topic_name="rq/test_node/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:f04},adlink_entity_factory=1} -1711462090.589167 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:f04 reliable volatile reader unnamed: (default).rq/test_node/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.4;">,topic_name="rq/test_node/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589172 [0] dq.builtin: => EVERYONE -1711462090.589174 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:f04) scanning all wrs of topic rq/test_node/list_parametersRequest -1711462090.589179 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.5;">,topic_name="rq/test_node/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:1104},adlink_entity_factory=1} -1711462090.589187 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:1104 reliable volatile reader unnamed: (default).rq/test_node/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.5;">,topic_name="rq/test_node/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589190 [0] dq.builtin: => EVERYONE -1711462090.589192 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:1104) scanning all wrs of topic rq/test_node/set_parametersRequest -1711462090.589196 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.6;">,topic_name="rq/test_node/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:1304},adlink_entity_factory=1} -1711462090.589205 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:1304 reliable volatile reader unnamed: (default).rq/test_node/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=52<"serviceid= 1.10.cd.d.79.d6.ee.ac.ea.4a.89.7.0.0.0.6;">,topic_name="rq/test_node/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589210 [0] dq.builtin: => EVERYONE -1711462090.589212 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:1304) scanning all wrs of topic rq/test_node/set_parameters_atomicallyRequest -1711462090.589213 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b3841d0 reg?no refc 1 rr/topological_navigation_core/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_) -1711462090.589216 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:1404},adlink_entity_factory=1} -1711462090.589220 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.589225 [0] python3: create_and_lock_ktopic: ktp 0x653a1b3837e0 -1711462090.589228 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b3841d0 -1711462090.589229 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:1404 reliable volatile reader unnamed: (default).rt/topological_map_2/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589235 [0] dq.builtin: => EVERYONE -1711462090.589239 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:1404) scanning all wrs of topic rt/topological_map_2 -1711462090.589235 [0] python3: dds_create_topic_generic: new topic 1185260289 -1711462090.589242 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #9: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/closest_node",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:1504},adlink_entity_factory=1} -1711462090.589249 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b386cd0 reg?no refc 1 rq/topological_navigation_core/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_) -1711462090.589251 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:1504 reliable volatile reader unnamed: (default).rt/closest_node/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/closest_node",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589253 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.589259 [0] dq.builtin: => EVERYONE -1711462090.589260 [0] python3: create_and_lock_ktopic: ktp 0x653a1b387450 -1711462090.589262 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:1504) scanning all wrs of topic rt/closest_node -1711462090.589264 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b386cd0 -1711462090.589268 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:4c2 #10: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="rt/current_node",type_name="std_msgs::msg::dds_::String_",reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110cd0d:79d6eeac:ea4a8907:1604},adlink_entity_factory=1} -1711462090.589269 [0] python3: dds_create_topic_generic: new topic 308518741 -1711462090.589277 [0] dq.builtin: SEDP ST0 110cd0d:79d6eeac:ea4a8907:1604 reliable volatile reader unnamed: (default).rt/current_node/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 ssm=0) QOS={user_data=0<>,topic_name="rt/current_node",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589280 [0] dq.builtin: => EVERYONE -1711462090.589282 [0] dq.builtin: match_proxy_reader_with_writers(prd 110cd0d:79d6eeac:ea4a8907:1604) scanning all wrs of topic rt/current_node -1711462090.589285 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:79d6eeac:ea4a8907:1}:1<0> -1711462090.589286 [0] dq.builtin: PMD ST0 pp 110cd0d:79d6eeac:ea4a8907 kind 1 data 1 -1711462090.589292 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:803, (default).rr/topological_navigation_core/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_) -1711462090.589298 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:803 @ 0x653a1b2ebe84) user 9 builtin 12 -1711462090.589314 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:803 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rr/topological_navigation_core/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589322 [0] python3: => EVERYONE -1711462090.589329 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:803) scanning all prds of topic rr/topological_navigation_core/describe_parametersReply -1711462090.589333 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:803) scanning all rds of topic rr/topological_navigation_core/describe_parametersReply -1711462090.589351 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rr/topological_navigation_core/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589358 [0] python3: non-timed queue now has 1 items -1711462090.589363 [0] tev: xpack_addmsg 0x770414001390 0x653a1b382310 0(data(110aba1:7b19badd:ce0adb73:3c2:#4/1)): niov 0 sz 0 => now niov 3 sz 336 -1711462090.589369 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:904, (default).rq/topological_navigation_core/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_) -1711462090.589372 [0] tev: nn_xpack_send 336: 0x77041400139c:20 0x653a1b382408:36 0x653a1ad693c4:280 [ udp/239.255.0.1:7400@2 ] -1711462090.589375 [0] tev: traffic-xmit (1) 336 -1711462090.589376 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:904 @ 0x653a1b3ded14) user 10 builtin 12 -1711462090.589380 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 336 from udp/10.101.12.71:52025 -1711462090.589384 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 68 from udp/10.101.12.71:40473 -1711462090.589392 [0] python3: READER 110aba1:7b19badd:ce0adb73:904 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rq/topological_navigation_core/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589394 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.589399 [0] python3: => EVERYONE -1711462090.589387 [0] recv: INFOTS(1711462090.589341876) -1711462090.589405 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:904) scanning all pwrs of topic rq/topological_navigation_core/describe_parametersRequest -1711462090.589405 [0] recvUC: ACKNACK(F#1:3/1:1 L(:1c1 16938.248898) 110d7b4:17c5b8c5:94bd0ff4:504 -> 110aba1:7b19badd:ce0adb73:403 complying RX3non-timed queue now has 1 items -1711462090.589410 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #4 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.589413 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:904) scanning all wrs of topic rq/topological_navigation_core/describe_parametersRequest -1711462090.589418 [0] recvUC: rexmit#1 maxseq:3<3<=3defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110d7b4:17c5b8c5:94bd0ff4:504 - queue for transmit -1711462090.589418 [0] tev: xpack_addmsg 0x770414001390 0x770404001c10 0(rexmit(110aba1:7b19badd:ce0adb73:403:#3/1)): niov 0 sz 0 => now niov 3 sz 200 -1711462090.589424 [0] recvUC: ) -1711462090.589429 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rq/topological_navigation_core/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:904},adlink_entity_factory=1} -1711462090.589431 [0] recvUC: send_deferred_heartbeat: 3aa8b43931dec43 -> ba3f356d8b9e85c4 - queue for transmit -1711462090.589429 [0] tev: nn_xpack_send 200: 0x77041400139c:20 0x770404001cf8:52 0x653a1b3811c0:128 [ udp/10.101.12.71:7415@2 ] -1711462090.589434 [0] recvUC: non-timed queue now has 1 items -1711462090.589435 [0] tev: traffic-xmit (1) 200 -1711462090.589441 [0] python3: non-timed queue now has 2 items -1711462090.589451 [0] tev: xpack_addmsg 0x770414001390 0x770404001d90 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.589456 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x770404001e78:48 [ udp/10.101.12.71:7415@2 ] -1711462090.589457 [0] tev: traffic-xmit (1) 68 -1711462090.589459 [0] tev: xpack_addmsg 0x770414001390 0x653a1b380cc0 0(data(110aba1:7b19badd:ce0adb73:4c2:#2/1)): niov 0 sz 0 => now niov 3 sz 332 -1711462090.589467 [0] tev: nn_xpack_send 332: 0x77041400139c:20 0x653a1b381278:36 0x653a1b430db4:276 [ udp/239.255.0.1:7400@2 ] -1711462090.589468 [0] tev: traffic-xmit (1) 332 -1711462090.589479 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 332 from udp/10.101.12.71:52025 -1711462090.589484 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #4: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}}}}} -1711462090.589486 [0] recv: INFOTS(1711462090.589420872) -1711462090.589496 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b389500 0(data(110aba1:7b19badd:ce0adb73:403:#4/1)): niov 0 sz 0 => now niov 3 sz 232 -1711462090.589497 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #2 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.589501 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 2 seq 4 maxseq 3) -1711462090.589507 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.000634423 s (min-ack 2!, avail-seq 4, xmit 3) -1711462090.589512 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b389680 0(control): niov 3 sz 232 => now niov 4 sz 264 -1711462090.589539 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 264 from udp/10.101.12.71:52025 -1711462090.589543 [0] python3: nn_xpack_send 264: 0x653a1add92cc:20 0x653a1b3895f8:36 0x653a1b436780:176 0x653a1b389778:32 [ udp/239.255.0.1:7401@2 ] -1711462090.589544 [0] recvMC: INFOTS(1711462090.589459043) -1711462090.589549 [0] python3: traffic-xmit (1) 264 -1711462090.589557 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #4 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.589561 [0] recvMC: HEARTBEAT(F#7:4..4 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.589566 [0] python3: => EVERYONE -1711462090.589599 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462090.589605 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.589613 [0] recvUC: ACKNACK(F#2:4/0: L(:1c1 16938.249108) 110d7b4:17c5b8c5:94bd0ff4:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0 setting-has-replied-to-hb happy-now) -1711462090.589738 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b38ac30 reg?no refc 1 rr/topological_navigation_core/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_) -1711462090.589745 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.589748 [0] python3: create_and_lock_ktopic: ktp 0x653a1b389f70 -1711462090.589752 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b38ac30 -1711462090.589757 [0] python3: dds_create_topic_generic: new topic 584095406 -1711462090.589765 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b38dc70 reg?no refc 1 rq/topological_navigation_core/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_) -1711462090.589768 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.589770 [0] python3: create_and_lock_ktopic: ktp 0x653a1b38e410 -1711462090.589773 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b38dc70 -1711462090.589777 [0] python3: dds_create_topic_generic: new topic 857333311 -1711462090.589790 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:a03, (default).rr/topological_navigation_core/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_) -1711462090.589796 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:a03 @ 0x653a1b38f034) user 11 builtin 12 -1711462090.589811 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:a03 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rr/topological_navigation_core/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589818 [0] python3: => EVERYONE -1711462090.589824 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:a03) scanning all prds of topic rr/topological_navigation_core/get_parametersReply -1711462090.589828 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:a03) scanning all rds of topic rr/topological_navigation_core/get_parametersReply -1711462090.589827 [0] gc: gc 0x77041c074ce0: deleting -1711462090.589847 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rr/topological_navigation_core/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589852 [0] python3: non-timed queue now has 1 items -1711462090.589858 [0] tev: xpack_addmsg 0x770414001390 0x653a1b389680 0(data(110aba1:7b19badd:ce0adb73:3c2:#5/1)): niov 0 sz 0 => now niov 3 sz 328 -1711462090.589863 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:b04, (default).rq/topological_navigation_core/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_) -1711462090.589865 [0] tev: nn_xpack_send 328: 0x77041400139c:20 0x653a1b389778:36 0x653a1aea0434:272 [ udp/239.255.0.1:7400@2 ] -1711462090.589868 [0] tev: traffic-xmit (1) 328 -1711462090.589869 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:b04 @ 0x653a1b38fd64) user 12 builtin 12 -1711462090.589871 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 328 from udp/10.101.12.71:52025 -1711462090.589875 [0] recv: INFOTS(1711462090.589837664) -1711462090.589881 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #5 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.589884 [0] python3: READER 110aba1:7b19badd:ce0adb73:b04 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rq/topological_navigation_core/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589890 [0] python3: => EVERYONE -1711462090.589895 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:b04) scanning all pwrs of topic rq/topological_navigation_core/get_parametersRequest -1711462090.589899 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:b04) scanning all wrs of topic rq/topological_navigation_core/get_parametersRequest -1711462090.589914 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rq/topological_navigation_core/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:b04},adlink_entity_factory=1} -1711462090.589919 [0] python3: non-timed queue now has 1 items -1711462090.589928 [0] tev: xpack_addmsg 0x770414001390 0x653a1b389500 0(data(110aba1:7b19badd:ce0adb73:4c2:#3/1)): niov 0 sz 0 => now niov 3 sz 324 -1711462090.589936 [0] tev: nn_xpack_send 324: 0x77041400139c:20 0x653a1b3895f8:36 0x653a1ad658a4:268 [ udp/239.255.0.1:7400@2 ] -1711462090.589937 [0] tev: traffic-xmit (1) 324 -1711462090.589938 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 324 from udp/10.101.12.71:52025 -1711462090.589944 [0] recv: INFOTS(1711462090.589906240) -1711462090.589947 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #3 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.589963 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #5: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}}}}} -1711462090.589970 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b3909f0 0(data(110aba1:7b19badd:ce0adb73:403:#5/1)): niov 0 sz 0 => now niov 3 sz 280 -1711462090.589979 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 5 maxseq 3) -1711462090.589984 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.000161673 s (min-ack 2!, avail-seq 5, xmit 4) -1711462090.589987 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b390b70 0(control): niov 3 sz 280 => now niov 4 sz 312 -1711462090.590013 [0] python3: nn_xpack_send 312: 0x653a1add92cc:20 0x653a1b390ae8:36 0x653a1b390860:224 0x653a1b390c68:32 [ udp/239.255.0.1:7401@2 ] -1711462090.590019 [0] python3: traffic-xmit (1) 312 -1711462090.590015 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 312 from udp/10.101.12.71:52025 -1711462090.590023 [0] python3: => EVERYONE -1711462090.590028 [0] recvMC: INFOTS(1711462090.589932268) -1711462090.590039 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #5 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590043 [0] recvMC: HEARTBEAT(F#8:5..5 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590152 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) suppressed, resched in 0.1 s (min-ack 2!, avail-seq 5, xmit 5) -1711462090.590159 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b391a50 reg?no refc 1 rr/topological_navigation_core/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_) -1711462090.590164 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.590168 [0] python3: create_and_lock_ktopic: ktp 0x653a1b391530 -1711462090.590171 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b391a50 -1711462090.590176 [0] python3: dds_create_topic_generic: new topic 2012040662 -1711462090.590184 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b392510 reg?no refc 1 rq/topological_navigation_core/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_) -1711462090.590186 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.590189 [0] python3: create_and_lock_ktopic: ktp 0x653a1b392d80 -1711462090.590192 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b392510 -1711462090.590196 [0] python3: dds_create_topic_generic: new topic 1928216990 -1711462090.590209 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:c03, (default).rr/topological_navigation_core/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_) -1711462090.590214 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:c03 @ 0x653a1b3939c4) user 13 builtin 12 -1711462090.590228 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:c03 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rr/topological_navigation_core/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.590235 [0] python3: => EVERYONE -1711462090.590241 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:c03) scanning all prds of topic rr/topological_navigation_core/get_parameter_typesReply -1711462090.590245 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:c03) scanning all rds of topic rr/topological_navigation_core/get_parameter_typesReply -1711462090.590262 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rr/topological_navigation_core/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.590267 [0] python3: non-timed queue now has 1 items -1711462090.590272 [0] tev: xpack_addmsg 0x770414001390 0x653a1b390b70 0(data(110aba1:7b19badd:ce0adb73:3c2:#6/1)): niov 0 sz 0 => now niov 3 sz 336 -1711462090.590277 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:d04, (default).rq/topological_navigation_core/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_) -1711462090.590279 [0] tev: nn_xpack_send 336: 0x77041400139c:20 0x653a1b390c68:36 0x653a1b38a754:280 [ udp/239.255.0.1:7400@2 ] -1711462090.590283 [0] tev: traffic-xmit (1) 336 -1711462090.590283 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 336 from udp/10.101.12.71:52025 -1711462090.590289 [0] recv: INFOTS(1711462090.590253167) -1711462090.590284 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:d04 @ 0x653a1b3948d4) user 14 builtin 12 -1711462090.590295 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #6 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.590312 [0] python3: READER 110aba1:7b19badd:ce0adb73:d04 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rq/topological_navigation_core/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.590322 [0] python3: => EVERYONE -1711462090.590327 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:d04) scanning all pwrs of topic rq/topological_navigation_core/get_parameter_typesRequest -1711462090.590331 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:d04) scanning all wrs of topic rq/topological_navigation_core/get_parameter_typesRequest -1711462090.590346 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rq/topological_navigation_core/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:d04},adlink_entity_factory=1} -1711462090.590352 [0] python3: non-timed queue now has 1 items -1711462090.590357 [0] tev: xpack_addmsg 0x770414001390 0x653a1b3909f0 0(data(110aba1:7b19badd:ce0adb73:4c2:#4/1)): niov 0 sz 0 => now niov 3 sz 332 -1711462090.590363 [0] tev: nn_xpack_send 332: 0x77041400139c:20 0x653a1b390ae8:36 0x653a1ac1cf74:276 [ udp/239.255.0.1:7400@2 ] -1711462090.590364 [0] tev: traffic-xmit (1) 332 -1711462090.590367 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 332 from udp/10.101.12.71:52025 -1711462090.590370 [0] recv: INFOTS(1711462090.590339038) -1711462090.590375 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #4 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.590397 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #6: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}}}}} -1711462090.590406 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b395790 0(data(110aba1:7b19badd:ce0adb73:403:#6/1)): niov 0 sz 0 => now niov 3 sz 328 -1711462090.590410 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 6 maxseq 3) -1711462090.590415 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0997847 s (min-ack 2!, avail-seq 6, xmit 5) -1711462090.590419 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b395910 0(control): niov 3 sz 328 => now niov 4 sz 360 -1711462090.590443 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 360 from udp/10.101.12.71:52025 -1711462090.590442 [0] python3: nn_xpack_send 360: 0x653a1add92cc:20 0x653a1b395888:36 0x653a1b3955d0:272 0x653a1b395a08:32 [ udp/239.255.0.1:7401@2 ] -1711462090.590449 [0] recvMC: INFOTS(1711462090.590361377) -1711462090.590455 [0] python3: traffic-xmit (1) 360 -1711462090.590455 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #6 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590460 [0] python3: => EVERYONE -1711462090.590466 [0] recvMC: HEARTBEAT(F#9:6..6 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590623 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b396ac0 reg?no refc 1 rr/topological_navigation_core/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_) -1711462090.590629 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.590633 [0] python3: create_and_lock_ktopic: ktp 0x653a1b3963a0 -1711462090.590637 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b396ac0 -1711462090.590645 [0] python3: dds_create_topic_generic: new topic 1330459960 -1711462090.590655 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b397940 reg?no refc 1 rq/topological_navigation_core/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_) -1711462090.590658 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.590661 [0] python3: create_and_lock_ktopic: ktp 0x653a1b398260 -1711462090.590663 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b397940 -1711462090.590668 [0] python3: dds_create_topic_generic: new topic 2110528528 -1711462090.590681 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:e03, (default).rr/topological_navigation_core/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_) -1711462090.590687 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:e03 @ 0x653a1b398e84) user 15 builtin 12 -1711462090.590702 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:e03 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rr/topological_navigation_core/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.590714 [0] python3: => EVERYONE -1711462090.590720 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:e03) scanning all prds of topic rr/topological_navigation_core/list_parametersReply -1711462090.590725 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:e03) scanning all rds of topic rr/topological_navigation_core/list_parametersReply -1711462090.590742 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rr/topological_navigation_core/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.590748 [0] python3: non-timed queue now has 1 items -1711462090.590753 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395910 0(data(110aba1:7b19badd:ce0adb73:3c2:#7/1)): niov 0 sz 0 => now niov 3 sz 328 -1711462090.590759 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:f04, (default).rq/topological_navigation_core/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_) -1711462090.590759 [0] tev: nn_xpack_send 328: 0x77041400139c:20 0x653a1b395a08:36 0x653a1b399994:272 [ udp/239.255.0.1:7400@2 ] -1711462090.590764 [0] tev: traffic-xmit (1) 328 -1711462090.590765 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:f04 @ 0x653a1b39a0c4) user 16 builtin 12 -1711462090.590765 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 328 from udp/10.101.12.71:52025 -1711462090.590777 [0] recv: INFOTS(1711462090.590733363) -1711462090.590780 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #7 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.590782 [0] python3: READER 110aba1:7b19badd:ce0adb73:f04 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rq/topological_navigation_core/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.590792 [0] python3: => EVERYONE -1711462090.590798 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:f04) scanning all pwrs of topic rq/topological_navigation_core/list_parametersRequest -1711462090.590802 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:f04) scanning all wrs of topic rq/topological_navigation_core/list_parametersRequest -1711462090.590817 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rq/topological_navigation_core/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:f04},adlink_entity_factory=1} -1711462090.590822 [0] python3: non-timed queue now has 1 items -1711462090.590828 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395790 0(data(110aba1:7b19badd:ce0adb73:4c2:#5/1)): niov 0 sz 0 => now niov 3 sz 324 -1711462090.590835 [0] tev: nn_xpack_send 324: 0x77041400139c:20 0x653a1b395888:36 0x653a1b39ac34:268 [ udp/239.255.0.1:7400@2 ] -1711462090.590836 [0] tev: traffic-xmit (1) 324 -1711462090.590837 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 324 from udp/10.101.12.71:52025 -1711462090.590840 [0] recv: INFOTS(1711462090.590809109) -1711462090.590843 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #5 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.590876 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #7: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,14,3,0,0,0,0,0,0,0,0}}}}}}} -1711462090.590884 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b444c20 0(data(110aba1:7b19badd:ce0adb73:403:#7/1)): niov 0 sz 0 => now niov 3 sz 376 -1711462090.590888 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 7 maxseq 3) -1711462090.590893 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0993134 s (min-ack 2!, avail-seq 7, xmit 6) -1711462090.590897 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b444da0 0(control): niov 3 sz 376 => now niov 4 sz 408 -1711462090.590920 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 408 from udp/10.101.12.71:52025 -1711462090.590927 [0] recvMC: INFOTS(1711462090.590832188) -1711462090.590922 [0] python3: nn_xpack_send 408: 0x653a1add92cc:20 0x653a1b444d18:36 0x653a1b444a30:320 0x653a1b444e98:32 [ udp/239.255.0.1:7401@2 ] -1711462090.590933 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #7 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590937 [0] python3: traffic-xmit (1) 408 -1711462090.590941 [0] recvMC: HEARTBEAT(F#10:7..7 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590945 [0] python3: => EVERYONE -1711462090.591100 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b4464b0 reg?no refc 1 rr/topological_navigation_core/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_) -1711462090.591107 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.591110 [0] python3: create_and_lock_ktopic: ktp 0x653a1b445e90 -1711462090.591113 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b4464b0 -1711462090.591118 [0] python3: dds_create_topic_generic: new topic 364593783 -1711462090.591149 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b447270 reg?no refc 1 rq/topological_navigation_core/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_) -1711462090.591152 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.591155 [0] python3: create_and_lock_ktopic: ktp 0x653a1b449ae0 -1711462090.591158 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b447270 -1711462090.591162 [0] python3: dds_create_topic_generic: new topic 1707060952 -1711462090.591175 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:1003, (default).rr/topological_navigation_core/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_) -1711462090.591181 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1003 @ 0x653a1b44a5e4) user 17 builtin 12 -1711462090.591196 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:1003 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rr/topological_navigation_core/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591203 [0] python3: => EVERYONE -1711462090.591209 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:1003) scanning all prds of topic rr/topological_navigation_core/set_parametersReply -1711462090.591213 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:1003) scanning all rds of topic rr/topological_navigation_core/set_parametersReply -1711462090.591230 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rr/topological_navigation_core/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591236 [0] python3: non-timed queue now has 1 items -1711462090.591241 [0] tev: xpack_addmsg 0x770414001390 0x653a1b444da0 0(data(110aba1:7b19badd:ce0adb73:3c2:#8/1)): niov 0 sz 0 => now niov 3 sz 328 -1711462090.591246 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:1104, (default).rq/topological_navigation_core/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_) -1711462090.591247 [0] tev: nn_xpack_send 328: 0x77041400139c:20 0x653a1b444e98:36 0x653a1b44ad24:272 [ udp/239.255.0.1:7400@2 ] -1711462090.591252 [0] tev: traffic-xmit (1) 328 -1711462090.591252 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1104 @ 0x653a1b44b6f4) user 18 builtin 12 -1711462090.591279 [0] python3: READER 110aba1:7b19badd:ce0adb73:1104 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rq/topological_navigation_core/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.591285 [0] python3: => EVERYONE -1711462090.591253 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 328 from udp/10.101.12.71:52025 -1711462090.591291 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:1104) scanning all pwrs of topic rq/topological_navigation_core/set_parametersRequest -1711462090.591297 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:1104) scanning all wrs of topic rq/topological_navigation_core/set_parametersRequest -1711462090.591315 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rq/topological_navigation_core/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1104},adlink_entity_factory=1} -1711462090.591317 [0] recv: INFOTS(1711462090.591221553) -1711462090.591322 [0] python3: non-timed queue now has 1 items -1711462090.591325 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #8 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.591327 [0] tev: xpack_addmsg 0x770414001390 0x653a1b444c20 0(data(110aba1:7b19badd:ce0adb73:4c2:#6/1)): niov 0 sz 0 => now niov 3 sz 324 -1711462090.591335 [0] tev: nn_xpack_send 324: 0x77041400139c:20 0x653a1b444d18:36 0x653a1b44c264:268 [ udp/239.255.0.1:7400@2 ] -1711462090.591336 [0] tev: traffic-xmit (1) 324 -1711462090.591341 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 324 from udp/10.101.12.71:52025 -1711462090.591345 [0] recv: INFOTS(1711462090.591307020) -1711462090.591350 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #6 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.591386 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #8: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,16,3,0,0,0,0,0,0,0,0}}}}}}} -1711462090.591392 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b44ca20 0(data(110aba1:7b19badd:ce0adb73:403:#8/1)): niov 0 sz 0 => now niov 3 sz 424 -1711462090.591394 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 8 maxseq 3) -1711462090.591398 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0988136 s (min-ack 2!, avail-seq 8, xmit 7) -1711462090.591400 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b44cba0 0(control): niov 3 sz 424 => now niov 4 sz 456 -1711462090.591417 [0] python3: nn_xpack_send 456: 0x653a1add92cc:20 0x653a1b44cb18:36 0x653a1b44c800:368 0x653a1b44cc98:32 [ udp/239.255.0.1:7401@2 ] -1711462090.591424 [0] python3: traffic-xmit (1) 456 -1711462090.591420 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 456 from udp/10.101.12.71:52025 -1711462090.591428 [0] python3: => EVERYONE -1711462090.591437 [0] recvMC: INFOTS(1711462090.591332052) -1711462090.591444 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #8 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591448 [0] recvMC: HEARTBEAT(F#11:8..8 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591547 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b44e180 reg?no refc 1 rr/topological_navigation_core/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_) -1711462090.591550 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.591551 [0] python3: create_and_lock_ktopic: ktp 0x653a1b44dba0 -1711462090.591553 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b44e180 -1711462090.591557 [0] python3: dds_create_topic_generic: new topic 1573531873 -1711462090.591572 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b44ee20 reg?no refc 1 rq/topological_navigation_core/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_) -1711462090.591573 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.591575 [0] python3: create_and_lock_ktopic: ktp 0x653a1b44eed0 -1711462090.591576 [0] python3: dds_create_topic_generic: register new sertype 0x653a1b44ee20 -1711462090.591578 [0] python3: dds_create_topic_generic: new topic 705050328 -1711462090.591586 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:1203, (default).rr/topological_navigation_core/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_) -1711462090.591590 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1203 @ 0x653a1b4523c4) user 19 builtin 12 -1711462090.591600 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:1203 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rr/topological_navigation_core/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591605 [0] python3: => EVERYONE -1711462090.591609 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:1203) scanning all prds of topic rr/topological_navigation_core/set_parameters_atomicallyReply -1711462090.591611 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:1203) scanning all rds of topic rr/topological_navigation_core/set_parameters_atomicallyReply -1711462090.591623 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rr/topological_navigation_core/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591626 [0] python3: non-timed queue now has 1 items -1711462090.591630 [0] tev: xpack_addmsg 0x770414001390 0x653a1b444c20 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.591631 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:1304, (default).rq/topological_navigation_core/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_) -1711462090.591634 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1304 @ 0x653a1b444a34) user 20 builtin 12 -1711462090.591636 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b444d08:44 [ udp/10.101.12.71:7413@2 ] -1711462090.591638 [0] tev: traffic-xmit (1) 64 -1711462090.591640 [0] tev: xpack_addmsg 0x770414001390 0x653a1b44cba0 0(data(110aba1:7b19badd:ce0adb73:3c2:#9/1)): niov 0 sz 0 => now niov 3 sz 352 -1711462090.591643 [0] python3: READER 110aba1:7b19badd:ce0adb73:1304 QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rq/topological_navigation_core/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.591647 [0] python3: => EVERYONE -1711462090.591647 [0] tev: nn_xpack_send 352: 0x77041400139c:20 0x653a1b44cc98:36 0x653a1b452ea4:296 [ udp/239.255.0.1:7400@2 ] -1711462090.591653 [0] tev: traffic-xmit (1) 352 -1711462090.591651 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:1304) scanning all pwrs of topic rq/topological_navigation_core/set_parameters_atomicallyRequest -1711462090.591651 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 352 from udp/10.101.12.71:52025 -1711462090.591667 [0] recv: INFOTS(1711462090.591617144) -1711462090.591665 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:1304) scanning all wrs of topic rq/topological_navigation_core/set_parameters_atomicallyRequest -1711462090.591670 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #9 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.591682 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rq/topological_navigation_core/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1304},adlink_entity_factory=1} -1711462090.591685 [0] python3: non-timed queue now has 1 items -1711462090.591690 [0] tev: xpack_addmsg 0x770414001390 0x653a1b44ca20 0(data(110aba1:7b19badd:ce0adb73:4c2:#7/1)): niov 0 sz 0 => now niov 3 sz 340 -1711462090.591697 [0] tev: nn_xpack_send 340: 0x77041400139c:20 0x653a1b44cb18:36 0x653a1b44deb4:284 [ udp/239.255.0.1:7400@2 ] -1711462090.591699 [0] tev: traffic-xmit (1) 340 -1711462090.591700 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 340 from udp/10.101.12.71:52025 -1711462090.591706 [0] recv: INFOTS(1711462090.591678024) -1711462090.591710 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #7 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.591710 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 68 from udp/10.101.12.71:58212 -1711462090.591723 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.591733 [0] recvUC: HEARTBEAT(#16:11..11 L(:1c1 16938.251226)110e21c:d0762c48:a9f0fb28:403 -> 110aba1:7b19badd:ce0adb73:504: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:504 #11) 110aba1:7b19badd:ce0adb73:504@10) -1711462090.591736 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110e21c:d0762c48:a9f0fb28:403: F#1:11/1:1 -1711462090.591739 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #9: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,19,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, -1711462090.591741 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462090.591745 [0] tev: xpack_addmsg 0x770414001390 0x653a1b44ca20 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.591746 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b454360 0(data(110aba1:7b19badd:ce0adb73:403:#9/1)): niov 0 sz 0 => now niov 3 sz 472 -1711462090.591750 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x653a1b44cb08:48 [ udp/10.101.12.71:7413@2 ] -1711462090.591752 [0] tev: traffic-xmit (1) 68 -1711462090.591751 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 9 maxseq 3) -1711462090.591758 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0984537 s (min-ack 2!, avail-seq 9, xmit 8) -1711462090.591760 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b4544e0 0(control): niov 3 sz 472 => now niov 4 sz 504 -1711462090.591778 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 504 from udp/10.101.12.71:52025 -1711462090.591787 [0] recvMC: INFOTS(1711462090.591692230) -1711462090.591791 [0] python3: nn_xpack_send 504: 0x653a1add92cc:20 0x653a1b454458:36 0x653a1ada15f0:416 0x653a1b4545d8:32 [ udp/239.255.0.1:7401@2 ] -1711462090.591793 [0] python3: traffic-xmit (1) 504 -1711462090.591797 [0] python3: => EVERYONE -1711462090.591791 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #9 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591801 [0] recvMC: HEARTBEAT(F#12:9..9 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591834 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 540 from udp/10.101.12.71:58212 -1711462090.591839 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.591843 [0] recvUC: INFOTS(1711462090.543222303) -1711462090.591847 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b455d30 reg?no refc 1 rq/controller_server/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_) -1711462090.591849 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.591851 [0] python3: create_and_lock_ktopic: ktp 0x653a1b458620 -1711462090.591852 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b447270 -1711462090.591857 [0] python3: dds_create_topic_generic: new topic 357629096 -1711462090.591862 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b455d30 reg?no refc 1 rr/controller_server/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_) -1711462090.591864 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.591865 [0] python3: create_and_lock_ktopic: ktp 0x653a1b453bb0 -1711462090.591866 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b4464b0 -1711462090.591869 [0] python3: dds_create_topic_generic: new topic 994292308 -1711462090.591875 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:1403, (default).rq/controller_server/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_) -1711462090.591878 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1403 @ 0x653a1b458d64) user 21 builtin 12 -1711462090.591887 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:1403 QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rq/controller_server/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591891 [0] python3: => EVERYONE -1711462090.591894 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:1403) scanning all prds of topic rq/controller_server/set_parametersRequest -1711462090.591896 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:1403) scanning all rds of topic rq/controller_server/set_parametersRequest -1711462090.591901 [0] recvUC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 110aba1:7b19badd:ce0adb73:504 #11 L(:1c1 16938.251342) msr_in_sync(110aba1:7b19badd:ce0adb73:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #11: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,14,3,0,0, =>110aba1:7b19badd:ce0adb73:504 -1711462090.591905 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #10: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rq/controller_server/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591908 [0] python3: non-timed queue now has 1 items -1711462090.591911 [0] tev: xpack_addmsg 0x770414001390 0x653a1b4544e0 0(data(110aba1:7b19badd:ce0adb73:3c2:#10/1)): niov 0 sz 0 => now niov 3 sz 316 -1711462090.591911 [0] recvUC: ) -1711462090.591913 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:1504, (default).rr/controller_server/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_) -1711462090.591918 [0] tev: nn_xpack_send 316: 0x77041400139c:20 0x653a1b4545d8:36 0x653a1aec35d4:260 [ udp/239.255.0.1:7400@2 ] -1711462090.591920 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1504 @ 0x653a1b459964) user 22 builtin 12 -1711462090.591919 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 68 from udp/10.101.12.71:58212 -1711462090.591918 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 316 from udp/10.101.12.71:52025 -1711462090.591920 [0] tev: traffic-xmit (1) 316 -1711462090.591932 [0] recv: INFOTS(1711462090.591900460) -1711462090.591935 [0] python3: READER 110aba1:7b19badd:ce0adb73:1504 QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rr/controller_server/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.591938 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #10 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.591940 [0] python3: => EVERYONE -1711462090.591931 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.591944 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:1504) scanning all pwrs of topic rr/controller_server/set_parametersReply -1711462090.591950 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:1504) scanning all wrs of topic rr/controller_server/set_parametersReply -1711462090.591954 [0] recvUC: HEARTBEAT(#17:11..11 L(:1c1 16938.251434)110e21c:d0762c48:a9f0fb28:403 -> 110aba1:7b19badd:ce0adb73:504: 110aba1:7b19badd:ce0adb73:504@11(sync)) -1711462090.591956 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110e21c:d0762c48:a9f0fb28:403: F#2:12/0: -1711462090.591959 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rr/controller_server/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1504},adlink_entity_factory=1} -1711462090.591963 [0] python3: non-timed queue now has 1 items -1711462090.591961 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462090.591967 [0] tev: xpack_addmsg 0x770414001390 0x653a1b4544e0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.591972 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b4545c8:44 [ udp/10.101.12.71:7413@2 ] -1711462090.591973 [0] tev: traffic-xmit (1) 64 -1711462090.591975 [0] tev: xpack_addmsg 0x770414001390 0x653a1b454360 0(data(110aba1:7b19badd:ce0adb73:4c2:#8/1)): niov 0 sz 0 => now niov 3 sz 308 -1711462090.591984 [0] tev: nn_xpack_send 308: 0x77041400139c:20 0x653a1b454458:36 0x653a1adcc124:252 [ udp/239.255.0.1:7400@2 ] -1711462090.591990 [0] tev: traffic-xmit (1) 308 -1711462090.591985 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 308 from udp/10.101.12.71:52025 -1711462090.591994 [0] recv: INFOTS(1711462090.591954823) -1711462090.591998 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #8 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.592019 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #10: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, -1711462090.592024 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b395470 0(data(110aba1:7b19badd:ce0adb73:403:#10/1)): niov 0 sz 0 => now niov 3 sz 520 -1711462090.592027 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 10 maxseq 3) -1711462090.592030 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0981777 s (min-ack 2!, avail-seq 10, xmit 9) -1711462090.592032 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1ada16a0 0(control): niov 3 sz 520 => now niov 4 sz 552 -1711462090.592047 [0] python3: nn_xpack_send 552: 0x653a1add92cc:20 0x653a1ada1618:36 0x653a1aed0ae0:464 0x653a1b45a558:32 [ udp/239.255.0.1:7401@2 ] -1711462090.592050 [0] python3: traffic-xmit (1) 552 -1711462090.592055 [0] python3: => EVERYONE -1711462090.592048 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 552 from udp/10.101.12.71:52025 -1711462090.592066 [0] recvMC: INFOTS(1711462090.591969903) -1711462090.592071 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #10 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592075 [0] recvMC: HEARTBEAT(F#13:10..10 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592098 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b45b260 reg?no refc 1 rq/controller_server/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_) -1711462090.592100 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.592102 [0] python3: create_and_lock_ktopic: ktp 0x653a1b452ac0 -1711462090.592103 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b397940 -1711462090.592106 [0] python3: dds_create_topic_generic: new topic 972247460 -1711462090.592114 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b45b260 reg?no refc 1 rr/controller_server/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_) -1711462090.592115 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.592116 [0] python3: create_and_lock_ktopic: ktp 0x653a1b455ce0 -1711462090.592118 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b396ac0 -1711462090.592121 [0] python3: dds_create_topic_generic: new topic 1835298763 -1711462090.592128 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:1603, (default).rq/controller_server/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_) -1711462090.592131 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1603 @ 0x653a1b39b554) user 23 builtin 12 -1711462090.592141 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:1603 QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rq/controller_server/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.592145 [0] python3: => EVERYONE -1711462090.592147 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:1603) scanning all prds of topic rq/controller_server/list_parametersRequest -1711462090.592150 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:1603) scanning all rds of topic rq/controller_server/list_parametersRequest -1711462090.592160 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #11: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rq/controller_server/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.592162 [0] python3: non-timed queue now has 1 items -1711462090.592165 [0] tev: xpack_addmsg 0x770414001390 0x653a1ada16a0 0(data(110aba1:7b19badd:ce0adb73:3c2:#11/1)): niov 0 sz 0 => now niov 3 sz 316 -1711462090.592168 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:1704, (default).rr/controller_server/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_) -1711462090.592170 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1704 @ 0x653a1b39c054) user 24 builtin 12 -1711462090.592174 [0] tev: nn_xpack_send 316: 0x77041400139c:20 0x653a1b45a558:36 0x653a1ad3b8b4:260 [ udp/239.255.0.1:7400@2 ] -1711462090.592177 [0] tev: traffic-xmit (1) 316 -1711462090.592175 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 316 from udp/10.101.12.71:52025 -1711462090.592184 [0] recv: INFOTS(1711462090.592154676) -1711462090.592187 [0] python3: READER 110aba1:7b19badd:ce0adb73:1704 QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rr/controller_server/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.592187 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #11 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.592192 [0] python3: => EVERYONE -1711462090.592196 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:1704) scanning all pwrs of topic rr/controller_server/list_parametersReply -1711462090.592199 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:1704) scanning all wrs of topic rr/controller_server/list_parametersReply -1711462090.592209 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #9: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rr/controller_server/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1704},adlink_entity_factory=1} -1711462090.592211 [0] python3: non-timed queue now has 1 items -1711462090.592216 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(data(110aba1:7b19badd:ce0adb73:4c2:#9/1)): niov 0 sz 0 => now niov 3 sz 308 -1711462090.592225 [0] tev: nn_xpack_send 308: 0x77041400139c:20 0x653a1ada1618:36 0x653a1aa5ff64:252 [ udp/239.255.0.1:7400@2 ] -1711462090.592227 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 308 from udp/10.101.12.71:52025 -1711462090.592232 [0] recv: INFOTS(1711462090.592204340) -1711462090.592236 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #9 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.592228 [0] tev: traffic-xmit (1) 308 -1711462090.592264 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #11: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, -1711462090.592269 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b39d0a0 0(data(110aba1:7b19badd:ce0adb73:403:#11/1)): niov 0 sz 0 => now niov 3 sz 568 -1711462090.592272 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 11 maxseq 3) -1711462090.592274 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.097931 s (min-ack 2!, avail-seq 11, xmit 10) -1711462090.592277 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b39d170 0(control): niov 3 sz 568 => now niov 4 sz 600 -1711462090.592290 [0] python3: nn_xpack_send 600: 0x653a1add92cc:20 0x653a1b45b288:36 0x653a1b38a960:512 0x653a1b39d268:32 [ udp/239.255.0.1:7401@2 ] -1711462090.592295 [0] python3: traffic-xmit (1) 600 -1711462090.592300 [0] python3: => EVERYONE -1711462090.592291 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 600 from udp/10.101.12.71:52025 -1711462090.592307 [0] recvMC: INFOTS(1711462090.592217036) -1711462090.592312 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #11 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592315 [0] recvMC: HEARTBEAT(F#14:11..11 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592336 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b39e620 reg?no refc 1 rq/controller_server/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_) -1711462090.592338 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.592339 [0] python3: create_and_lock_ktopic: ktp 0x653a1b39e9c0 -1711462090.592341 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b38dc70 -1711462090.592344 [0] python3: dds_create_topic_generic: new topic 1361955521 -1711462090.592361 [0] python3: dds_create_topic_generic (pp 0x653a1b4855a0 110aba1:7b19badd:ce0adb73:1c1 sertype 0x653a1b39e620 reg?no refc 1 rr/controller_server/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_) -1711462090.592362 [0] python3: lookup_and_check_ktopic_may_unlock_pp: no such ktopic -1711462090.592364 [0] python3: create_and_lock_ktopic: ktp 0x653a1b454e10 -1711462090.592366 [0] python3: dds_create_topic_generic: reuse sertype 0x653a1b38ac30 -1711462090.592371 [0] python3: dds_create_topic_generic: new topic 1248342734 -1711462090.592377 [0] python3: new_writer(guid 110aba1:7b19badd:ce0adb73:1803, (default).rq/controller_server/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_) -1711462090.592380 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1803 @ 0x653a1b39f924) user 25 builtin 12 -1711462090.592389 [0] python3: WRITER 110aba1:7b19badd:ce0adb73:1803 QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rq/controller_server/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.592393 [0] python3: => EVERYONE -1711462090.592396 [0] python3: match_writer_with_proxy_readers(wr 110aba1:7b19badd:ce0adb73:1803) scanning all prds of topic rq/controller_server/get_parametersRequest -1711462090.592398 [0] python3: match_writer_with_readers(wr 110aba1:7b19badd:ce0adb73:1803) scanning all rds of topic rq/controller_server/get_parametersRequest -1711462090.592408 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:3c2 #12: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rq/controller_server/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.592411 [0] python3: non-timed queue now has 1 items -1711462090.592413 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d170 0(data(110aba1:7b19badd:ce0adb73:3c2:#12/1)): niov 0 sz 0 => now niov 3 sz 316 -1711462090.592415 [0] python3: new_reader(guid 110aba1:7b19badd:ce0adb73:1904, (default).rr/controller_server/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_) -1711462090.592418 [0] python3: ddsi_ref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1904 @ 0x653a1b3a0714) user 26 builtin 12 -1711462090.592422 [0] tev: nn_xpack_send 316: 0x77041400139c:20 0x653a1b39d268:36 0x653a1b3a00f4:260 [ udp/239.255.0.1:7400@2 ] -1711462090.592426 [0] tev: traffic-xmit (1) 316 -1711462090.592423 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 316 from udp/10.101.12.71:52025 -1711462090.592433 [0] recv: INFOTS(1711462090.592403130) -1711462090.592435 [0] python3: READER 110aba1:7b19badd:ce0adb73:1904 QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rr/controller_server/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.592439 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #12 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.592445 [0] python3: => EVERYONE -1711462090.592448 [0] python3: match_reader_with_proxy_writers(rd 110aba1:7b19badd:ce0adb73:1904) scanning all pwrs of topic rr/controller_server/get_parametersReply -1711462090.592451 [0] python3: match_reader_with_writers(rd 110aba1:7b19badd:ce0adb73:1904) scanning all wrs of topic rr/controller_server/get_parametersReply -1711462090.592459 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:4c2 #10: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rr/controller_server/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1904},adlink_entity_factory=1} -1711462090.592462 [0] python3: non-timed queue now has 1 items -1711462090.592465 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(data(110aba1:7b19badd:ce0adb73:4c2:#10/1)): niov 0 sz 0 => now niov 3 sz 308 -1711462090.592473 [0] tev: nn_xpack_send 308: 0x77041400139c:20 0x653a1b45b288:36 0x653a1b3a0ff4:252 [ udp/239.255.0.1:7400@2 ] -1711462090.592476 [0] tev: traffic-xmit (1) 308 -1711462090.592474 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 308 from udp/10.101.12.71:52025 -1711462090.592484 [0] recv: INFOTS(1711462090.592455025) -1711462090.592488 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #10 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.592526 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, -1711462090.592531 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b44c6d0 0(data(110aba1:7b19badd:ce0adb73:403:#12/1)): niov 0 sz 0 => now niov 3 sz 616 -1711462090.592535 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 12 maxseq 3) -1711462090.592539 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0976805 s (min-ack 2!, avail-seq 12, xmit 11) -1711462090.592542 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1b3a0d30 0(control): niov 3 sz 616 => now niov 4 sz 648 -1711462090.592560 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 648 from udp/10.101.12.71:52025 -1711462090.592560 [0] python3: nn_xpack_send 648: 0x653a1add92cc:20 0x653a1b39e648:36 0x653a1b3a15d0:560 0x653a1b3a18d8:32 [ udp/239.255.0.1:7401@2 ] -1711462090.592582 [0] recvMC: INFOTS(1711462090.592467231) -1711462090.592583 [0] python3: traffic-xmit (1) 648 -1711462090.592591 [0] python3: => EVERYONE -1711462090.592587 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #12 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592599 [0] recvMC: HEARTBEAT(F#15:12..12 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592743 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.592752 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b45b278:44 [ udp/10.101.12.71:7417@2 ] -1711462090.592755 [0] tev: traffic-xmit (1) 64 -1711462090.592802 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 68 from udp/10.101.12.71:44991 -1711462090.592808 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.592819 [0] recvUC: HEARTBEAT(#16:10..10 L(:1c1 16938.252311)110443d:bb7f10a5:18901533:403 -> 110aba1:7b19badd:ce0adb73:504: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:504 #10) 110aba1:7b19badd:ce0adb73:504@9) -1711462090.592823 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110443d:bb7f10a5:18901533:403: F#1:10/1:1 -1711462090.592826 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.592830 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.592837 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x653a1b45b278:48 [ udp/10.101.12.71:7417@2 ] -1711462090.592839 [0] tev: traffic-xmit (1) 68 -1711462090.592871 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 504 from udp/10.101.12.71:44991 -1711462090.592877 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.592883 [0] recvUC: INFOTS(1711462090.571985769) -1711462090.592949 [0] recvUC: DATA(110443d:bb7f10a5:18901533:403 -> 110aba1:7b19badd:ce0adb73:504 #10 L(:1c1 16938.252380) msr_in_sync(110aba1:7b19badd:ce0adb73:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #10: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,19,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,16,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,16 =>110aba1:7b19badd:ce0adb73:504 -1711462090.592956 [0] recvUC: ) -1711462090.592962 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 68 from udp/10.101.12.71:44991 -1711462090.592971 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.592978 [0] recvUC: HEARTBEAT(#17:10..10 L(:1c1 16938.252474)110443d:bb7f10a5:18901533:403 -> 110aba1:7b19badd:ce0adb73:504: 110aba1:7b19badd:ce0adb73:504@10(sync)) -1711462090.592983 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110443d:bb7f10a5:18901533:403: F#2:11/0: -1711462090.592986 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.592989 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.592997 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b45b278:44 [ udp/10.101.12.71:7417@2 ] -1711462090.592999 [0] tev: traffic-xmit (1) 64 -1711462090.597279 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.597333 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b45b278:44 [ udp/10.101.12.71:7411@2 ] -1711462090.597338 [0] tev: traffic-xmit (1) 64 -1711462090.598358 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.598381 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b45b278:44 [ udp/10.101.12.71:7411@2 ] -1711462090.598385 [0] tev: traffic-xmit (1) 64 -1711462090.598918 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.598936 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b45b278:44 [ udp/10.101.12.71:7411@2 ] -1711462090.598941 [0] tev: traffic-xmit (1) 64 -1711462090.619799 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462090.619836 [0] recv: INFOTS(1711462090.519442682) -1711462090.619855 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462090.619871 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462090.619893 [0] recv: INFOTS(1711462090.519442682) -1711462090.619904 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462090.619913 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.619930 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16938.279431) -1711462090.619955 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.619968 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16938.279471) -1711462090.642157 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462090.642192 [0] recv: INFOTS(1711462090.541682763) -1711462090.642201 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462090.642210 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462090.642214 [0] recv: INFOTS(1711462090.541682763) -1711462090.642221 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462090.642232 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.642243 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16938.301746) -1711462090.642263 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.642269 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16938.301772) -1711462090.649536 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462090.649544 [0] recv: INFOTS(1711462090.549308855) -1711462090.649554 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462090.649591 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462090.649597 [0] recv: INFOTS(1711462090.549308855) -1711462090.649604 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462090.649705 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.649740 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16938.309240) -1711462090.649777 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.649787 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16938.309289) -1711462090.670514 [0] tev: non-timed queue now has 1 items -1711462090.670551 [0] tev: xmit spdp 110aba1:7b19badd:ce0adb73:1c1 to 0:0:0:100c7 (resched 8s) -1711462090.670563 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462090.670687 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462090.670702 [0] recv: INFOTS(1711462090.570393375) -1711462090.670711 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462090.670727 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462090.670731 [0] recv: INFOTS(1711462090.570393375) -1711462090.670735 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462090.670781 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.670813 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (local) -1711462090.670847 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.670855 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (local) -1711462090.671539 [0] tev: nn_xpack_send 444: 0x77041400139c:20 0x653a1b45b288:36 0x653a1b2b4514:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462090.671554 [0] tev: traffic-xmit (101) 444 -1711462090.671881 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 84 from udp/10.101.12.71:58212 -1711462090.671901 [0] recv: HEARTBEAT(#9:1..10 L(:1c1 16938.331390)110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@10(sync)) -1711462090.671908 [0] recv: HEARTBEAT(#9:1..8 110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:4c7@8(sync)) -1711462090.671923 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#3:11/0: -1711462090.671925 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.671937 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462090.671948 [0] recv: HEARTBEAT(#7:1..10 L(:1c1 16938.331447)110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@10(sync)) -1711462090.671958 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.671967 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#3:9/0: -1711462090.671985 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462090.671999 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d170 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.672006 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#3:11/0: -1711462090.672012 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462090.672037 [0] tev: nn_xpack_send 92: 0x77041400139c:20 0x653a1b45b278:44 0x653a1b39d268:28 [ udp/10.101.12.71:7412@2 ] -1711462090.672042 [0] tev: traffic-xmit (1) 92 -1711462090.672062 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672077 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1ada1608:44 [ udp/10.101.12.71:7416@2 ] -1711462090.672089 [0] tev: traffic-xmit (1) 64 -1711462090.672107 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462090.672134 [0] recv: HEARTBEAT(#5:1..0 L(:1c1 16938.331629)110e21c:d0762c48:a9f0fb28:300c3 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:300c4@0(sync)) -1711462090.672144 [0] tev: acknack 110aba1:7b19badd:ce0adb73:300c4 -> 110e21c:d0762c48:a9f0fb28:300c3: F#2:1/0: -1711462090.672150 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672153 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:300c4 -> pwr 110e21c:d0762c48:a9f0fb28:300c3) -1711462090.672159 [0] recv: HEARTBEAT(#7:1..7 L(:1c1 16938.331658)110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:4c7@7(sync)) -1711462090.672160 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672177 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#3:8/0: -1711462090.672183 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462090.672197 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1ada1608:44 [ udp/10.101.12.71:7412@2 ] -1711462090.672204 [0] tev: traffic-xmit (1) 64 -1711462090.672220 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672197 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462090.672237 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b45b278:44 [ udp/10.101.12.71:7416@2 ] -1711462090.672252 [0] tev: traffic-xmit (1) 64 -1711462090.672245 [0] recv: HEARTBEAT(#8:1..42 L(:1c1 16938.331742)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@42(sync)) -1711462090.672262 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#3:43/0: -1711462090.672274 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462090.672277 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462090.672293 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672305 [0] tev: acknack 110aba1:7b19badd:ce0adb73:301c4 -> 110e21c:d0762c48:a9f0fb28:301c3: F#2:1/0: -1711462090.672313 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:301c4 -> pwr 110e21c:d0762c48:a9f0fb28:301c3) -1711462090.672333 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b45b278:44 [ udp/10.101.12.71:7414@2 ] -1711462090.672342 [0] tev: traffic-xmit (1) 64 -1711462090.672349 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672364 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1ada1608:44 [ udp/10.101.12.71:7412@2 ] -1711462090.672369 [0] tev: traffic-xmit (1) 64 -1711462090.672286 [0] recv: HEARTBEAT(#5:1..0 L(:1c1 16938.331784)110e21c:d0762c48:a9f0fb28:301c3 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:301c4@0(sync)) -1711462090.672405 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672438 [0] recv: HEARTBEAT(#7:1..1 L(:1c1 16938.331930)110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@1(sync)) -1711462090.672445 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#3:2/0: -1711462090.672451 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462090.672457 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672474 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1ada1608:44 [ udp/10.101.12.71:7416@2 ] -1711462090.672480 [0] tev: traffic-xmit (1) 64 -1711462090.672542 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462090.672559 [0] recv: HEARTBEAT(#8:1..40 L(:1c1 16938.332052)110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:4c7@40(sync)) -1711462090.672569 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672582 [0] recv: HEARTBEAT(#3:1..0 L(:1c1 16938.332078)110443d:bb7f10a5:18901533:300c3 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:300c4@0(sync)) -1711462090.672586 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#3:41/0: -1711462090.672604 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462090.672613 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672620 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672622 [0] tev: acknack 110aba1:7b19badd:ce0adb73:300c4 -> 110443d:bb7f10a5:18901533:300c3: F#2:1/0: -1711462090.672644 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:300c4 -> pwr 110443d:bb7f10a5:18901533:300c3) -1711462090.672659 [0] recv: HEARTBEAT(#3:1..0 L(:1c1 16938.332135)110443d:bb7f10a5:18901533:301c3 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:301c4@0(sync)) -1711462090.672674 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1ada1608:44 [ udp/10.101.12.71:7414@2 ] -1711462090.672783 [0] tev: traffic-xmit (1) 64 -1711462090.672794 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672807 [0] tev: acknack 110aba1:7b19badd:ce0adb73:301c4 -> 110443d:bb7f10a5:18901533:301c3: F#2:1/0: -1711462090.672811 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:301c4 -> pwr 110443d:bb7f10a5:18901533:301c3) -1711462090.672818 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.672837 [0] tev: nn_xpack_send 92: 0x77041400139c:20 0x653a1b45b278:44 0x653a1ada1618:28 [ udp/10.101.12.71:7416@2 ] -1711462090.672849 [0] tev: traffic-xmit (1) 92 -1711462090.674277 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:3c2 multicasting (rel-prd 6 seq-eq-max 3 seq 12 maxseq 2) -1711462090.674293 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:3c2) sent, resched in 0.1 s (min-ack 2!, avail-seq 12, xmit 12) -1711462090.674299 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462090.674303 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:4c2 multicasting (rel-prd 6 seq-eq-max 3 seq 10 maxseq 1) -1711462090.674308 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:4c2) sent, resched in 0.1 s (min-ack 1!, avail-seq 10, xmit 10) -1711462090.674312 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 2 sz 52 => now niov 3 sz 84 -1711462090.674317 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:200c2 multicasting (rel-prd 6 seq-eq-max 3 seq 1 maxseq 1) -1711462090.674322 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:200c2) sent, resched in 0.1 s (min-ack 1!, avail-seq 1, xmit 1) -1711462090.674326 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d170 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462090.674360 [0] tev: nn_xpack_send 116: 0x77041400139c:20 0x653a1b45b288:32 0x653a1ada1618:32 0x653a1b39d268:32 [ udp/239.255.0.1:7400@2 ] -1711462090.674368 [0] tev: traffic-xmit (1) 116 -1711462090.674368 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 116 from udp/10.101.12.71:52025 -1711462090.674378 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:300c3 multicasting (rel-prd 6 seq-eq-max 0 seq 0 maxseq 0) -1711462090.674405 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:300c3) sent, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.674413 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462090.674413 [0] recv: HEARTBEAT(#5:1..12 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462090.674430 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:301c3 multicasting (rel-prd 6 seq-eq-max 0 seq 0 maxseq 0) -1711462090.674444 [0] recv: HEARTBEAT(#5:1..10 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462090.674445 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:301c3) sent, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.674477 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 2 sz 52 => now niov 3 sz 84 -1711462090.674468 [0] recv: HEARTBEAT(#5:1..1 110aba1:7b19badd:ce0adb73:200c2? -> 0:0:0:0) -1711462090.674496 [0] tev: nn_xpack_send 84: 0x77041400139c:20 0x653a1b45b288:32 0x653a1ada1618:32 [ udp/239.255.0.1:7400@2 ] -1711462090.674501 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462090.674503 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 84 from udp/10.101.12.71:52025 -1711462090.674542 [0] recvMC: HEARTBEAT(#18:11..11 L(:1c1 16938.334035)110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@11(sync)) -1711462090.674547 [0] recv: HEARTBEAT(#2:1..0 110aba1:7b19badd:ce0adb73:300c3? -> 0:0:0:0) -1711462090.674555 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.674504 [0] tev: traffic-xmit (1) 84 -1711462090.674560 [0] recv: HEARTBEAT(#2:1..0 110aba1:7b19badd:ce0adb73:301c3? -> 0:0:0:0) -1711462090.674574 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110e21c:d0762c48:a9f0fb28:403: F#3:12/0: -1711462090.674584 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462090.674593 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 92 from udp/10.101.12.71:58212 -1711462090.674565 [0] recvMC: HEARTBEAT(#18:10..10 L(:1c1 16938.334065)110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@10(sync)) -1711462090.674596 [0] tev: xpack_addmsg 0x770414001390 0x653a1b39d0a0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.674607 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.674621 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110443d:bb7f10a5:18901533:403: F#3:11/0: -1711462090.674637 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.674638 [0] recv: ACKNACK(F#3:13/0: L(:1c1 16938.334109) 110e21c:d0762c48:a9f0fb28:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK10 RM0) -1711462090.674652 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1b45b278:44 [ udp/10.101.12.71:7413@2 ] -1711462090.674670 [0] tev: traffic-xmit (1) 64 -1711462090.674664 [0] recv: ACKNACK(F#3:11/0: 110e21c:d0762c48:a9f0fb28:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0) -1711462090.674675 [0] tev: xpack_addmsg 0x770414001390 0x653a1b395470 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.674697 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 92 from udp/10.101.12.71:44991 -1711462090.674700 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x653a1ada1608:44 [ udp/10.101.12.71:7417@2 ] -1711462090.674712 [0] tev: traffic-xmit (1) 64 -1711462090.674706 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.674741 [0] recv: ACKNACK(F#3:13/0: L(:1c1 16938.334208) 110443d:bb7f10a5:18901533:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK10 RM0) -1711462090.674755 [0] recv: ACKNACK(F#3:11/0: 110443d:bb7f10a5:18901533:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0) -1711462090.674788 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 120 from udp/10.101.12.71:40473 -1711462090.674801 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.674814 [0] recv: ACKNACK(F#3:13/0: L(:1c1 16938.334303) 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK10 RM0) -1711462090.674824 [0] recv: ACKNACK(F#3:11/0: 110d7b4:17c5b8c5:94bd0ff4:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0) -1711462090.674836 [0] recv: ACKNACK(F#3:2/0: 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110aba1:7b19badd:ce0adb73:200c2) -1711462090.674851 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 120 from udp/10.101.12.71:44991 -1711462090.674858 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.674881 [0] recv: ACKNACK(F#3:2/0: L(:1c1 16938.334361) 110443d:bb7f10a5:18901533:200c7 -> 110aba1:7b19badd:ce0adb73:200c2) -1711462090.674890 [0] recv: ACKNACK(F#2:1/0: 110443d:bb7f10a5:18901533:300c4 -> 110aba1:7b19badd:ce0adb73:300c3) -1711462090.674898 [0] recv: ACKNACK(F#2:1/0: 110443d:bb7f10a5:18901533:301c4 -> 110aba1:7b19badd:ce0adb73:301c3) -1711462090.674911 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 92 from udp/10.101.12.71:40473 -1711462090.674917 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.674927 [0] recv: ACKNACK(F#2:1/0: L(:1c1 16938.334420) 110d7b4:17c5b8c5:94bd0ff4:300c4 -> 110aba1:7b19badd:ce0adb73:300c3) -1711462090.674935 [0] recv: ACKNACK(F#2:1/0: 110d7b4:17c5b8c5:94bd0ff4:301c4 -> 110aba1:7b19badd:ce0adb73:301c3) -1711462090.674947 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462090.674953 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.674963 [0] recv: ACKNACK(F#3:2/0: L(:1c1 16938.334456) 110e21c:d0762c48:a9f0fb28:200c7 -> 110aba1:7b19badd:ce0adb73:200c2) -1711462090.674975 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 332 from udp/10.101.12.71:50807 -1711462090.674983 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.675005 [0] recv: ACKNACK(F#2:1/12:110000000000 L(:1c1 16938.334485) 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 complying RX1non-timed queue now has 1 items -1711462090.675020 [0] tev: xpack_addmsg 0x770414001390 0x653a1b44c6d0 0(rexmit(110aba1:7b19badd:ce0adb73:3c2:#1/1)): niov 0 sz 0 => now niov 3 sz 244 -1711462090.675025 [0] recv: RX2non-timed queue now has 1 items -1711462090.675039 [0] recv: rexmit#2 maxseq:2<12<=12defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:3c2 -> 110cd0d:56a27910:57318171:3c7 - queue for transmit -1711462090.675048 [0] recv: ) -1711462090.675054 [0] tev: xpack_addmsg 0x770414001390 0x653a1b3a0d30 0(rexmit(110aba1:7b19badd:ce0adb73:3c2:#2/1)): niov 3 sz 244 => now niov 5 sz 480 -1711462090.675059 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.675075 [0] tev: nn_xpack_send 480: 0x77041400139c:20 0x653a1b39e638:52 0x653a1b4036c4:172 0x653a1b3a18d8:36 0x653a1af8b024:200 [ udp/10.101.12.71:7410@2 ] -1711462090.675078 [0] recv: ACKNACK(F#13:13/0: L(:1c1 16938.334485) 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK10 RM0 setting-has-replied-to-hb happy-now) -1711462090.675079 [0] tev: traffic-xmit (1) 480 -1711462090.675089 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.675101 [0] recv: ACKNACK(F#7:13/0: L(:1c1 16938.334485) 110cd0d:79d6eeac:ea4a8907:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK10 RM0 setting-has-replied-to-hb happy-now) -1711462090.675107 [0] recv: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462090.675121 [0] recv: ACKNACK(F#2:1/10:1000000000 110cd0d:56a27910:57318171:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 complying RX1non-timed queue now has 1 items -1711462090.675130 [0] recv: rexmit#1 maxseq:1<10<=10defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:4c2 -> 110cd0d:56a27910:57318171:4c7 - queue for transmit -1711462090.675132 [0] tev: xpack_addmsg 0x770414001390 0x77040c005dc0 0(rexmit(110aba1:7b19badd:ce0adb73:4c2:#1/1)): niov 0 sz 0 => now niov 3 sz 248 -1711462090.675153 [0] recv: non-timed queue now has 1 items -1711462090.675168 [0] tev: xpack_addmsg 0x770414001390 0x77040c005c40 0(control): niov 3 sz 248 => now niov 4 sz 280 -1711462090.675173 [0] recv: ) -1711462090.675181 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.675181 [0] tev: nn_xpack_send 280: 0x77041400139c:20 0x77040c005ea8:52 0x653a1b2ca014:176 0x77040c005d38:32 [ udp/10.101.12.71:7410@2 ] -1711462090.675209 [0] recv: ACKNACK(F#11:11/0: 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0 setting-has-replied-to-hb happy-now) -1711462090.675222 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.675228 [0] recv: ACKNACK(F#6:11/0: 110cd0d:79d6eeac:ea4a8907:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0 setting-has-replied-to-hb happy-now) -1711462090.675236 [0] recv: send_deferred_heartbeat: 34e8ac789b0d206c -> 130111643236e53f - queue for transmit -1711462090.675240 [0] recv: non-timed queue now has 1 items -1711462090.675255 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 92 from udp/10.101.12.71:58212 -1711462090.675259 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.675268 [0] recv: ACKNACK(F#2:1/0: L(:1c1 16938.334762) 110e21c:d0762c48:a9f0fb28:300c4 -> 110aba1:7b19badd:ce0adb73:300c3) -1711462090.675275 [0] recv: ACKNACK(F#2:1/0: 110e21c:d0762c48:a9f0fb28:301c4 -> 110aba1:7b19badd:ce0adb73:301c3) -1711462090.675281 [0] tev: traffic-xmit (1) 280 -1711462090.675290 [0] tev: xpack_addmsg 0x770414001390 0x77040c005f40 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.675299 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x77040c006028:48 [ udp/10.101.12.71:7410@2 ] -1711462090.675304 [0] tev: traffic-xmit (1) 68 -1711462090.675641 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 480 from udp/10.101.12.71:50807 -1711462090.675655 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.675671 [0] recv: ACKNACK(F#2:2/0: L(:1c1 16938.335156) 110cd0d:56a27910:57318171:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 setting-has-replied-to-hb happy-now) -1711462090.675681 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.675692 [0] recv: ACKNACK(F#8:2/0: L(:1c1 16938.335156) 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 setting-has-replied-to-hb happy-now) -1711462090.675698 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.675708 [0] recv: ACKNACK(F#6:2/0: L(:1c1 16938.335156) 110cd0d:79d6eeac:ea4a8907:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 setting-has-replied-to-hb happy-now) -1711462090.675715 [0] recv: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462090.675725 [0] recv: ACKNACK(F#2:1/0: 110cd0d:56a27910:57318171:300c4 -> 110aba1:7b19badd:ce0adb73:300c3 setting-has-replied-to-hb happy-now) -1711462090.675731 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.675739 [0] recv: ACKNACK(F#6:1/0: 110cd0d:e3b81b8d:1ccb65b1:300c4 -> 110aba1:7b19badd:ce0adb73:300c3 setting-has-replied-to-hb happy-now) -1711462090.675748 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.675761 [0] recv: ACKNACK(F#4:1/0: 110cd0d:79d6eeac:ea4a8907:300c4 -> 110aba1:7b19badd:ce0adb73:300c3 setting-has-replied-to-hb happy-now) -1711462090.675767 [0] recv: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462090.675776 [0] recv: ACKNACK(F#2:1/0: 110cd0d:56a27910:57318171:301c4 -> 110aba1:7b19badd:ce0adb73:301c3 setting-has-replied-to-hb happy-now) -1711462090.675783 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.675790 [0] recv: ACKNACK(F#6:1/0: 110cd0d:e3b81b8d:1ccb65b1:301c4 -> 110aba1:7b19badd:ce0adb73:301c3 setting-has-replied-to-hb happy-now) -1711462090.675796 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.675803 [0] recv: ACKNACK(F#4:1/0: 110cd0d:79d6eeac:ea4a8907:301c4 -> 110aba1:7b19badd:ce0adb73:301c3 setting-has-replied-to-hb happy-now) -1711462090.675943 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 92 from udp/10.101.12.71:50807 -1711462090.675950 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.675965 [0] recv: ACKNACK(F#3:13/0: L(:1c1 16938.335453) 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK10 RM0 setting-has-replied-to-hb happy-now) -1711462090.675979 [0] recv: ACKNACK(F#3:11/0: 110cd0d:56a27910:57318171:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0 setting-has-replied-to-hb happy-now) -1711462090.677479 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 124 from udp/10.101.12.71:50807 -1711462090.677491 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.677514 [0] recvUC: HEARTBEAT(#7:12..12 L(:1c1 16938.336993)110cd0d:56a27910:57318171:403 -> 110aba1:7b19badd:ce0adb73:0: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:504 #12) 110aba1:7b19badd:ce0adb73:504@11) -1711462090.677520 [0] recvUC: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.677550 [0] recvUC: HEARTBEAT(#26:13..13 L(:1c1 16938.336993)110cd0d:e3b81b8d:1ccb65b1:403 -> 110aba1:7b19badd:ce0adb73:0: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:504 #13) 110aba1:7b19badd:ce0adb73:504@12) -1711462090.677557 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:56a27910:57318171:403: F#1:12/1:1 -1711462090.677563 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110cd0d:56a27910:57318171:403) -1711462090.677568 [0] tev: xpack_addmsg 0x770414001390 0x77040c005f40 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.677574 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:e3b81b8d:1ccb65b1:403: F#1:13/1:1 -1711462090.677577 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110cd0d:e3b81b8d:1ccb65b1:403) -1711462090.677582 [0] tev: xpack_addmsg 0x770414001390 0x77040c005dc0 0(control): niov 2 sz 68 => now niov 3 sz 116 -1711462090.677603 [0] tev: nn_xpack_send 116: 0x77041400139c:20 0x77040c006028:48 0x77040c005ea8:48 [ udp/10.101.12.71:7411@2 ] -1711462090.677606 [0] tev: traffic-xmit (1) 116 -1711462090.677622 [0] recvUC: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.677630 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.677649 [0] recvUC: HEARTBEAT(#18:13..13 L(:1c1 16938.337132)110cd0d:79d6eeac:ea4a8907:403 -> 110aba1:7b19badd:ce0adb73:0: end-of-tl-seq(rd 110aba1:7b19badd:ce0adb73:504 #13) 110aba1:7b19badd:ce0adb73:504@12) -1711462090.677656 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:79d6eeac:ea4a8907:403: F#1:13/1:1 -1711462090.677660 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110cd0d:79d6eeac:ea4a8907:403) -1711462090.677665 [0] tev: xpack_addmsg 0x770414001390 0x77040c005f40 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.677674 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x77040c006028:48 [ udp/10.101.12.71:7411@2 ] -1711462090.677678 [0] tev: traffic-xmit (1) 68 -1711462090.677715 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 544 from udp/10.101.12.71:50807 -1711462090.677727 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.677734 [0] recvUC: INFOTS(1711462090.149525610) -1711462090.677859 [0] recvUC: DATA(110cd0d:56a27910:57318171:403 -> 110aba1:7b19badd:ce0adb73:504 #12 L(:1c1 16938.337229) msr_in_sync(110aba1:7b19badd:ce0adb73:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110cd0d:56a27910:57318171:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162, =>110aba1:7b19badd:ce0adb73:504 -1711462090.677877 [0] recvUC: ) -1711462090.677889 [0] recvUC: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 632 from udp/10.101.12.71:50807 -1711462090.677898 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.677903 [0] recvUC: INFOTS(1711462069.292105261) -1711462090.678009 [0] recvUC: DATA(110cd0d:e3b81b8d:1ccb65b1:403 -> 110aba1:7b19badd:ce0adb73:504 #13 L(:1c1 16938.337400) msr_in_sync(110aba1:7b19badd:ce0adb73:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,12,3,0,0,0,0,0,0 =>110aba1:7b19badd:ce0adb73:504 -1711462090.678021 [0] recvUC: ) -1711462090.678026 [0] recvUC: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462090.678041 [0] recvUC: HEARTBEAT(#8:12..12 L(:1c1 16938.337400)110cd0d:56a27910:57318171:403 -> 110aba1:7b19badd:ce0adb73:504: 110aba1:7b19badd:ce0adb73:504@12(sync)) -1711462090.678046 [0] recvUC: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.678055 [0] recvUC: HEARTBEAT(#27:13..13 110cd0d:e3b81b8d:1ccb65b1:403 -> 110aba1:7b19badd:ce0adb73:504: 110aba1:7b19badd:ce0adb73:504@13(sync)) -1711462090.678066 [0] recvUC: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 520 from udp/10.101.12.71:50807 -1711462090.678066 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:56a27910:57318171:403: F#2:13/0: -1711462090.678085 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110cd0d:56a27910:57318171:403) -1711462090.678074 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.678090 [0] tev: xpack_addmsg 0x770414001390 0x77040c005f40 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.678106 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:e3b81b8d:1ccb65b1:403: F#2:14/0: -1711462090.678098 [0] recvUC: INFOTS(1711462089.657917473) -1711462090.678109 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110cd0d:e3b81b8d:1ccb65b1:403) -1711462090.678122 [0] tev: xpack_addmsg 0x770414001390 0x77040c005dc0 0(control): niov 2 sz 64 => now niov 3 sz 108 -1711462090.678133 [0] tev: nn_xpack_send 108: 0x77041400139c:20 0x77040c006028:44 0x77040c005ea8:44 [ udp/10.101.12.71:7411@2 ] -1711462090.678136 [0] tev: traffic-xmit (1) 108 -1711462090.678222 [0] recvUC: DATA(110cd0d:79d6eeac:ea4a8907:403 -> 110aba1:7b19badd:ce0adb73:504 #13 L(:1c1 16938.337576) msr_in_sync(110aba1:7b19badd:ce0adb73:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16 =>110aba1:7b19badd:ce0adb73:504 -1711462090.678233 [0] recvUC: ) -1711462090.678244 [0] recvUC: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.678249 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.678261 [0] recvUC: HEARTBEAT(#19:13..13 L(:1c1 16938.337752)110cd0d:79d6eeac:ea4a8907:403 -> 110aba1:7b19badd:ce0adb73:504: 110aba1:7b19badd:ce0adb73:504@13(sync)) -1711462090.678267 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:79d6eeac:ea4a8907:403: F#2:14/0: -1711462090.678271 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110cd0d:79d6eeac:ea4a8907:403) -1711462090.678275 [0] tev: xpack_addmsg 0x770414001390 0x77040c005f40 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.678284 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x77040c006028:44 [ udp/10.101.12.71:7411@2 ] -1711462090.678287 [0] tev: traffic-xmit (1) 64 -1711462090.685477 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462090.685489 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.685501 [0] recvUC: ACKNACK(#0:1/0: L(:1c1 16938.344991) 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403 preemptive-nack rebase defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110cd0d:56a27910:57318171:504 - queue for transmit -1711462090.685511 [0] recvUC: non-timed queue now has 1 items -1711462090.685516 [0] recvUC: ) -1711462090.685522 [0] recvUC: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.685531 [0] recvUC: ACKNACK(#0:1/0: L(:1c1 16938.344991) 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403 preemptive-nack rebase defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110cd0d:79d6eeac:ea4a8907:504 - queue for transmit -1711462090.685533 [0] tev: xpack_addmsg 0x770414001390 0x770404002e30 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.685545 [0] recvUC: non-timed queue now has 1 items -1711462090.685555 [0] recvUC: ) -1711462090.685560 [0] tev: xpack_addmsg 0x770414001390 0x770404002fb0 0(control): niov 2 sz 68 => now niov 3 sz 116 -1711462090.685560 [0] recvUC: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.685572 [0] tev: nn_xpack_send 116: 0x77041400139c:20 0x770404002f18:48 0x770404003098:48 [ udp/10.101.12.71:7411@2 ] -1711462090.685582 [0] tev: traffic-xmit (1) 116 -1711462090.685583 [0] recvUC: ACKNACK(#0:1/0: L(:1c1 16938.344991) 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403 preemptive-nack rebase defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110cd0d:e3b81b8d:1ccb65b1:504 - queue for transmit -1711462090.685603 [0] recvUC: non-timed queue now has 1 items -1711462090.685610 [0] recvUC: ) -1711462090.685614 [0] tev: xpack_addmsg 0x770414001390 0x770404003130 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.685624 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x770404003218:48 [ udp/10.101.12.71:7411@2 ] -1711462090.685628 [0] tev: traffic-xmit (1) 68 -1711462090.685680 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.685692 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.685712 [0] recvUC: ACKNACK(F#1:12/1:1 L(:1c1 16938.345194) 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403 ACK9 RM0 complying RX12non-timed queue now has 1 items -1711462090.685723 [0] recvUC: rexmit#1 maxseq:12<12<=12defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110cd0d:56a27910:57318171:504 - queue for transmit -1711462090.685727 [0] tev: xpack_addmsg 0x770414001390 0x7704040032b0 0(rexmit(110aba1:7b19badd:ce0adb73:403:#12/1)): niov 0 sz 0 => now niov 3 sz 632 -1711462090.685731 [0] recvUC: ) -1711462090.685742 [0] tev: nn_xpack_send 632: 0x77041400139c:20 0x770404003398:52 0x653a1b3a15d0:560 [ udp/10.101.12.71:7411@2 ] -1711462090.685749 [0] recvUC: send_deferred_heartbeat: 3aa8b43931dec43 -> 52c2ca619c723b37 - queue for transmit -1711462090.685759 [0] recvUC: non-timed queue now has 1 items -1711462090.685762 [0] tev: traffic-xmit (1) 632 -1711462090.685772 [0] recvUC: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.685774 [0] tev: xpack_addmsg 0x770414001390 0x770404003430 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.685783 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.685800 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x770404003518:48 [ udp/10.101.12.71:7411@2 ] -1711462090.685805 [0] tev: traffic-xmit (1) 68 -1711462090.685812 [0] recvUC: ACKNACK(F#8:12/1:1 L(:1c1 16938.345285) 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403 ACK8 RM0 complying RX12non-timed queue now has 1 items -1711462090.685822 [0] recvUC: rexmit#1 maxseq:12<12<=12defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110cd0d:79d6eeac:ea4a8907:504 - queue for transmit -1711462090.685825 [0] tev: xpack_addmsg 0x770414001390 0x7704040035b0 0(rexmit(110aba1:7b19badd:ce0adb73:403:#12/1)): niov 0 sz 0 => now niov 3 sz 632 -1711462090.685831 [0] recvUC: ) -1711462090.685839 [0] recvUC: send_deferred_heartbeat: 3aa8b43931dec43 -> e46f26c62d5068b - queue for transmit -1711462090.685846 [0] recvUC: non-timed queue now has 1 items -1711462090.685859 [0] tev: xpack_addmsg 0x770414001390 0x770404003730 0(control): niov 3 sz 632 => now niov 4 sz 664 -1711462090.685859 [0] recvUC: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 68 from udp/10.101.12.71:50807 -1711462090.685872 [0] tev: nn_xpack_send 664: 0x77041400139c:20 0x770404003698:52 0x653a1b3a15d0:560 0x770404003828:32 [ udp/10.101.12.71:7411@2 ] -1711462090.685878 [0] tev: traffic-xmit (1) 664 -1711462090.685886 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.685902 [0] recvUC: ACKNACK(F#17:12/1:1 L(:1c1 16938.345388) 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403 ACK8 RM0 complying RX12non-timed queue now has 1 items -1711462090.685911 [0] recvUC: rexmit#1 maxseq:12<12<=12defer_heartbeat_to_peer: 110aba1:7b19badd:ce0adb73:403 -> 110cd0d:e3b81b8d:1ccb65b1:504 - queue for transmit -1711462090.685915 [0] tev: xpack_addmsg 0x770414001390 0x7704040038b0 0(rexmit(110aba1:7b19badd:ce0adb73:403:#12/1)): niov 0 sz 0 => now niov 3 sz 632 -1711462090.685917 [0] recvUC: ) -1711462090.685924 [0] recvUC: send_deferred_heartbeat: 3aa8b43931dec43 -> a5c74bd281b51900 - queue for transmit -1711462090.685926 [0] tev: nn_xpack_send 632: 0x77041400139c:20 0x770404003998:52 0x653a1b3a15d0:560 [ udp/10.101.12.71:7411@2 ] -1711462090.685930 [0] recvUC: non-timed queue now has 1 items -1711462090.685931 [0] tev: traffic-xmit (1) 632 -1711462090.685944 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.685953 [0] tev: nn_xpack_send 68: 0x77041400139c:20 0x770404003b18:48 [ udp/10.101.12.71:7411@2 ] -1711462090.685963 [0] tev: traffic-xmit (1) 68 -1711462090.686269 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 64 from udp/10.101.12.71:50807 -1711462090.686279 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.686292 [0] recvUC: ACKNACK(F#2:13/0: L(:1c1 16938.345781) 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0 setting-has-replied-to-hb happy-now) -1711462090.686335 [0] recvUC: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 64 from udp/10.101.12.71:50807 -1711462090.686343 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.686353 [0] recvUC: ACKNACK(F#9:13/0: L(:1c1 16938.345846) 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0 setting-has-replied-to-hb happy-now) -1711462090.686441 [0] recvUC: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 64 from udp/10.101.12.71:50807 -1711462090.686449 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.686459 [0] recvUC: ACKNACK(F#18:13/0: L(:1c1 16938.345951) 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0 setting-has-replied-to-hb happy-now) -1711462090.690247 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) suppressed, resched in 0.1 s (min-ack 3, avail-seq 12, xmit 12) -1711462090.774454 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:3c2) suppressed, resched in inf s (min-ack 12, avail-seq 12, xmit 12) -1711462090.774475 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:4c2) suppressed, resched in inf s (min-ack 10, avail-seq 10, xmit 10) -1711462090.774480 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:200c2) suppressed, resched in inf s (min-ack 1, avail-seq 1, xmit 1) -1711462090.774484 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:300c3) suppressed, resched in inf s (min-ack 0, avail-seq 0, xmit 0) -1711462090.774488 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:301c3) suppressed, resched in inf s (min-ack 0, avail-seq 0, xmit 0) -1711462090.774544 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462090.774574 [0] recvMC: HEARTBEAT(#50:42..42 L(:1c1 16938.434064)110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@42(sync)) -1711462090.774635 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#3:43/0: -1711462090.774652 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462090.774657 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.774685 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7415@2 ] -1711462090.774689 [0] tev: traffic-xmit (1) 64 -1711462090.790396 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 3 seq 12 maxseq 12) -1711462090.790422 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) sent, resched in 0.1 s (min-ack 3, avail-seq 12, xmit 12) -1711462090.790429 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462090.790472 [0] tev: nn_xpack_send 52: 0x77041400139c:20 0x770404003b28:32 [ udp/239.255.0.1:7401@2 ] -1711462090.790475 [0] tev: traffic-xmit (1) 52 -1711462090.790638 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462090.790664 [0] recvMC: HEARTBEAT(#22:12..12 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.790868 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462090.790890 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.790906 [0] recvUC: ACKNACK(F#3:13/0: L(:1c1 16938.450391) 110d7b4:17c5b8c5:94bd0ff4:504 -> 110aba1:7b19badd:ce0adb73:403 ACK9 RM0) -1711462090.790918 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.790922 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.790932 [0] recvUC: ACKNACK(F#3:13/0: L(:1c1 16938.450425) 110443d:bb7f10a5:18901533:504 -> 110aba1:7b19badd:ce0adb73:403 ACK9 RM0) -1711462090.790941 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462090.790945 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.790953 [0] recvUC: ACKNACK(F#3:13/0: L(:1c1 16938.450448) 110e21c:d0762c48:a9f0fb28:504 -> 110aba1:7b19badd:ce0adb73:403 ACK9 RM0) -1711462090.790961 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462090.790965 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462090.790973 [0] recvUC: ACKNACK(F#3:13/0: L(:1c1 16938.450468) 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403) -1711462090.790977 [0] recvUC: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462090.790984 [0] recvUC: ACKNACK(F#19:13/0: L(:1c1 16938.450468) 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403) -1711462090.790988 [0] recvUC: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462090.790995 [0] recvUC: ACKNACK(F#10:13/0: L(:1c1 16938.450468) 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403) -1711462090.890548 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) suppressed, resched in inf s (min-ack 12, avail-seq 12, xmit 12) -1711462091.469301 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 256 from udp/10.101.12.71:40473 -1711462091.469337 [0] recv: INFOTS(1711462091.469007481) -1711462091.469354 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #43 L(:1c1 16939.128838)) -1711462091.469420 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #43: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",durability=1,durability_service=0:0:10:-1:-1:-1,reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.469317 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2096 from udp/10.101.12.71:40473 -1711462091.469473 [0] recvMC: INFOTS(1711462091.469060530) -1711462091.469464 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5603 reliable transient-local writer unnamed: (default).rt/topological_map_2/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:10:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.469484 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #43 L(:1c1 16939.128974) => EVERYONE -1711462091.469540 [0] dq.builtin: => EVERYONE -1711462091.469581 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #43: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462091.469592 [0] recvMC: HEARTBEAT(#51:43..43 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@43(sync)) -1711462091.469593 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5603) scanning all rds of topic rt/topological_map_2 -1711462091.469632 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#4:44/0: -1711462091.469875 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462091.469883 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462091.469885 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.469891 [0] recvMC: HEARTBEAT(#1:1..0 L(:1c1 16939.129391)110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0:110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 no-reliable-readers) -1711462091.469924 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7415@2 ] -1711462091.469932 [0] tev: traffic-xmit (1) 64 -1711462091.481806 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 200 from udp/10.101.12.71:40473 -1711462091.481821 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2120 from udp/10.101.12.71:40473 -1711462091.481851 [0] recvMC: INFOTS(1711462091.481540997) -1711462091.481834 [0] recv: INFOTS(1711462091.481496692) -1711462091.481869 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #44 L(:1c1 16939.141352) => EVERYONE -1711462091.481881 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #44) -1711462091.481976 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #44: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/tf",type_name="tf2_msgs::msg::dds_::TFMessage_",reliability=1:9223372036854775807,history=0:100,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.482006 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #44: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462091.482019 [0] recvMC: HEARTBEAT(F#52:44..44 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@44(sync)) -1711462091.482031 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5703 reliable volatile writer unnamed: (default).rt/tf/tf2_msgs::msg::dds_::TFMessage_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="rt/tf",type_name="tf2_msgs::msg::dds_::TFMessage_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:100,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.482293 [0] dq.builtin: => EVERYONE -1711462091.482314 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5703) scanning all rds of topic rt/tf -1711462091.482986 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 276 from udp/10.101.12.71:40473 -1711462091.483001 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2144 from udp/10.101.12.71:40473 -1711462091.483027 [0] recvMC: INFOTS(1711462091.482763810) -1711462091.483009 [0] recv: INFOTS(1711462091.482728775) -1711462091.483042 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #45 L(:1c1 16939.142528) => EVERYONE -1711462091.483050 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #45) -1711462091.483094 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #45: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/topological_map",type_name="topological_navigation_msgs::msg::dds_::TopologicalMap_",durability=1,durability_service=0:0:10:-1:-1:-1,reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.483145 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5803 reliable transient-local writer unnamed: (default).rt/topological_map/topological_navigation_msgs::msg::dds_::TopologicalMap_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="rt/topological_map",type_name="topological_navigation_msgs::msg::dds_::TopologicalMap_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:10:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.483169 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #45: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462091.483181 [0] recvMC: HEARTBEAT(F#53:45..45 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@45(sync)) -1711462091.483190 [0] dq.builtin: => EVERYONE -1711462091.483204 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5803) scanning all rds of topic rt/topological_map -1711462091.569367 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462091.569407 [0] recv: HEARTBEAT(#9:1..45 L(:1c1 16939.228892)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@45(sync)) -1711462091.569574 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#4:46/0: -1711462091.569608 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462091.569618 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.569653 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7414@2 ] -1711462091.569662 [0] tev: traffic-xmit (1) 64 -1711462091.571599 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 460 from udp/10.101.12.71:58212 -1711462091.571624 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462091.571632 [0] recv: INFOTS(1711462090.519442682) -1711462091.571648 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462091.571657 [0] recv: thread_cputime 0.007394000 -1711462091.571686 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 460 from udp/10.101.12.71:44991 -1711462091.571694 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462091.571700 [0] recv: INFOTS(1711462090.549308855) -1711462091.571708 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462091.571751 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462091.571777 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16939.231277) -1711462091.571785 [0] dq.builtin: thread_cputime 0.005238000 -1711462091.571818 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462091.571829 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 460 from udp/10.101.12.71:40473 -1711462091.571849 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462091.571832 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16939.231333) -1711462091.571860 [0] recv: INFOTS(1711462090.541682763) -1711462091.571884 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462091.571977 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462091.571999 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16939.231500) -1711462091.573055 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 1356 from udp/10.101.12.71:50807 -1711462091.573073 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462091.573081 [0] recv: INFOTS(1711462090.143481252) -1711462091.573095 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462091.573101 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462091.573106 [0] recv: INFOTS(1711462067.905323275) -1711462091.573113 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462091.573118 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462091.573123 [0] recv: INFOTS(1711462069.653526636) -1711462091.573130 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462091.573155 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462091.573180 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (known) L(:1c1 16939.232680) -1711462091.573213 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462091.573226 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16939.232728) -1711462091.573256 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462091.573266 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16939.232768) -1711462091.625953 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #1: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462091,625880474},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462091.625991 [0] python3: => EVERYONE -1711462091.669877 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462091.669988 [0] recvMC: HEARTBEAT(#54:45..45 L(:1c1 16939.329448)110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@45(sync)) -1711462091.670004 [0] recvMC: thread_cputime 0.001222000 -1711462091.670056 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462091.670059 [0] tev: thread_cputime 0.000000000 -1711462091.670154 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#5:46/0: -1711462091.670194 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462091.670079 [0] recvMC: HEARTBEAT(#3:1..1 L(:1c1 16939.329571)110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0:110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 no-reliable-readers) -1711462091.670274 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.670393 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7415@2 ] -1711462091.670449 [0] tev: traffic-xmit (1) 64 -1711462091.671717 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20568 from udp/10.101.12.71:40473 -1711462091.671766 [0] recvMC: INFOTS(1711462091.478307355) -1711462091.671793 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[1..1]/[0..20500) of 340680 L(:1c1 16939.331265) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.673579 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.673640 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[5..5]/[82000..102500) of 340680 L(:1c1 16939.333125) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.673679 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.673693 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[6..6]/[102500..123000) of 340680 L(:1c1 16939.333190) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.673951 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674000 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[7..7]/[123000..143500) of 340680 L(:1c1 16939.333487) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.674027 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674043 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[8..8]/[143500..164000) of 340680 L(:1c1 16939.333540) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.674099 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674112 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[9..9]/[164000..184500) of 340680 L(:1c1 16939.333610) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.674365 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674416 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[10..10]/[184500..205000) of 340680 L(:1c1 16939.333903) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.674444 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674459 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[11..11]/[205000..225500) of 340680 L(:1c1 16939.333956) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.674536 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674561 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[12..12]/[225500..246000) of 340680 L(:1c1 16939.334057) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.676611 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.676656 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[13..13]/[246000..266500) of 340680 L(:1c1 16939.336147) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.676681 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.676696 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[14..14]/[266500..287000) of 340680 L(:1c1 16939.336194) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.676714 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.676723 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[15..15]/[287000..307500) of 340680 L(:1c1 16939.336222) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.676743 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.676780 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[16..16]/[307500..328000) of 340680 L(:1c1 16939.336279) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.680215 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 12736 from udp/10.101.12.71:40473 -1711462091.680276 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[17..17]/[328000..340680) of 340680 L(:1c1 16939.339763) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.680326 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.680343 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[2..2]/[20500..41000) of 340680 L(:1c1 16939.339840) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.680389 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.680400 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[3..3]/[41000..61500) of 340680 L(:1c1 16939.339898) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.680626 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.680677 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[4..4]/[61500..82000) of 340680 L(:1c1 16939.340164) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.680709 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.680732 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[5..5]/[82000..102500) of 340680 L(:1c1 16939.340229) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.680752 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.680766 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[6..6]/[102500..123000) of 340680 L(:1c1 16939.340265) 110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: no readers) -1711462091.906149 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462091.906213 [0] recv: INFOTS(1711462091.905809560) -1711462091.906241 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0 #4 L(:1c1 16939.565712)) -1711462091.906272 [0] recv: HEARTBEAT(#29:4..4 110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@4(sync)) -1711462091.906326 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2: F#3:5/0: -1711462091.906361 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462091.906377 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.906383 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:200c2 #4: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:e3b81b8d:1ccb65b1:1}:1<0> -1711462091.906415 [0] dq.builtin: PMD ST0 pp 110cd0d:e3b81b8d:1ccb65b1 kind 1 data 1 -1711462091.906527 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7410@2 ] -1711462091.906560 [0] tev: traffic-xmit (1) 64 -1711462092.006445 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462092.006547 [0] recv: INFOTS(1711462067.905323275) -1711462092.006631 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462092.006678 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462092.006695 [0] recv: INFOTS(1711462067.905323275) -1711462092.006714 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462092.006806 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.006865 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16939.666361) -1711462092.006927 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.006948 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16939.666447) -1711462092.571680 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 460 from udp/10.101.12.71:58212 -1711462092.571759 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462092.571779 [0] recv: INFOTS(1711462090.519442682) -1711462092.571813 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462092.571833 [0] recv: thread_cputime 0.008399000 -1711462092.571866 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 460 from udp/10.101.12.71:40473 -1711462092.571882 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462092.571893 [0] recv: INFOTS(1711462090.541682763) -1711462092.571909 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462092.571988 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 460 from udp/10.101.12.71:44991 -1711462092.572005 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462092.572016 [0] recv: INFOTS(1711462090.549308855) -1711462092.572032 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462092.572104 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.572191 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16940.231685) -1711462092.572266 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.572292 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16940.231791) -1711462092.572309 [0] dq.builtin: thread_cputime 0.005635000 -1711462092.572373 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.572396 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16940.231895) -1711462092.573601 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 1356 from udp/10.101.12.71:50807 -1711462092.573654 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462092.573670 [0] recv: INFOTS(1711462090.143481252) -1711462092.573703 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462092.573716 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462092.573725 [0] recv: INFOTS(1711462067.905323275) -1711462092.573738 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462092.573747 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462092.573757 [0] recv: INFOTS(1711462069.653526636) -1711462092.573770 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462092.573971 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.574038 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (known) L(:1c1 16940.233533) -1711462092.574103 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.574126 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16940.233625) -1711462092.574182 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.574204 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16940.233703) -1711462092.628483 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #2: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462092,628416427},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462092.628536 [0] python3: => EVERYONE -1711462093.571895 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 460 from udp/10.101.12.71:58212 -1711462093.571959 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462093.571977 [0] recv: INFOTS(1711462090.519442682) -1711462093.572011 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462093.572030 [0] recv: thread_cputime 0.009172000 -1711462093.572061 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 460 from udp/10.101.12.71:40473 -1711462093.572076 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462093.572090 [0] recv: INFOTS(1711462090.541682763) -1711462093.572107 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462093.572136 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 460 from udp/10.101.12.71:44991 -1711462093.572148 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462093.572159 [0] recv: INFOTS(1711462090.549308855) -1711462093.572174 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462093.572301 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.572393 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16941.231886) -1711462093.572485 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.572549 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16941.232047) -1711462093.572585 [0] dq.builtin: thread_cputime 0.005635000 -1711462093.572652 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.572675 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16941.232173) -1711462093.573546 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 1356 from udp/10.101.12.71:50807 -1711462093.573605 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462093.573624 [0] recv: INFOTS(1711462090.143481252) -1711462093.573655 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462093.573668 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462093.573679 [0] recv: INFOTS(1711462067.905323275) -1711462093.573692 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462093.573703 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462093.573713 [0] recv: INFOTS(1711462069.653526636) -1711462093.573727 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 110aba1:7b19badd:ce0adb73:100c7 #1) -1711462093.573897 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.573974 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (known) L(:1c1 16941.233468) -1711462093.574039 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.574062 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16941.233560) -1711462093.574118 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.574138 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16941.233637) -1711462093.631085 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #3: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462093,631011940},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462093.631123 [0] python3: => EVERYONE -1711462093.654843 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462093.654912 [0] recv: INFOTS(1711462093.654299867) -1711462093.654952 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0 #4 L(:1c1 16941.314410)) -1711462093.655004 [0] recv: HEARTBEAT(#21:4..4 110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@4(sync)) -1711462093.655111 [0] tev: thread_cputime 0.000000000 -1711462093.655460 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:200c2 #4: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:79d6eeac:ea4a8907:1}:1<0> -1711462093.655515 [0] dq.builtin: PMD ST0 pp 110cd0d:79d6eeac:ea4a8907 kind 1 data 1 -1711462093.655491 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2: F#3:5/0: -1711462093.655578 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:79d6eeac:ea4a8907:200c2) -1711462093.655599 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462093.655690 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7410@2 ] -1711462093.655706 [0] tev: traffic-xmit (1) 64 -1711462093.754387 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462093.754436 [0] recv: INFOTS(1711462069.653526636) -1711462093.754458 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462093.754489 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462093.754496 [0] recv: INFOTS(1711462069.653526636) -1711462093.754515 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462093.754581 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.754616 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16941.414115) -1711462093.754672 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.754688 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16941.414189) -1711462094.633442 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #4: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462094,633356879},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462094.633487 [0] python3: => EVERYONE -1711462095.636690 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #5: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462095,636452133},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462095.636758 [0] python3: => EVERYONE -1711462096.641501 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #6: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462096,641316097},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462096.641580 [0] python3: => EVERYONE -1711462097.645189 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #7: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462097,645025498},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462097.645276 [0] python3: => EVERYONE -1711462098.144633 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462098.144750 [0] recv: INFOTS(1711462098.144063519) -1711462098.144804 [0] recv: DATA(110cd0d:56a27910:57318171:200c2 -> 0:0:0:0 #2 L(:1c1 16945.804248)) -1711462098.144835 [0] recv: HEARTBEAT(#9:2..2 110cd0d:56a27910:57318171:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@2(sync)) -1711462098.144853 [0] recv: thread_cputime 0.010577000 -1711462098.145024 [0] tev: thread_cputime 0.000000000 -1711462098.145041 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:56a27910:57318171:1}:1<0> -1711462098.145156 [0] dq.builtin: PMD ST0 pp 110cd0d:56a27910:57318171 kind 1 data 1 -1711462098.145181 [0] dq.builtin: thread_cputime 0.005635000 -1711462098.145118 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2: F#3:3/0: -1711462098.145465 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:56a27910:57318171:200c2) -1711462098.145486 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462098.145582 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7410@2 ] -1711462098.145600 [0] tev: traffic-xmit (1) 64 -1711462098.244408 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462098.244481 [0] recv: INFOTS(1711462090.143481252) -1711462098.244582 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 0:0:0:0 #1) -1711462098.244621 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462098.244631 [0] recv: INFOTS(1711462090.143481252) -1711462098.244645 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 0:0:0:0 #1) -1711462098.244786 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.244818 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (known) L(:1c1 16945.904316) -1711462098.244894 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.244930 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (known) L(:1c1 16945.904430) -1711462098.520053 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 116 from udp/10.101.12.71:58212 -1711462098.520104 [0] recv: INFOTS(1711462098.519615324) -1711462098.520134 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:200c2 -> 0:0:0:0 #2 L(:1c1 16946.179602)) -1711462098.520183 [0] recv: HEARTBEAT(#10:2..2 110e21c:d0762c48:a9f0fb28:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@2(sync)) -1711462098.520393 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110e21c:d0762c48:a9f0fb28:1}:1<0> -1711462098.520463 [0] dq.builtin: PMD ST0 pp 110e21c:d0762c48:a9f0fb28 kind 1 data 1 -1711462098.520394 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#3:3/0: -1711462098.520635 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462098.520662 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462098.520774 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7412@2 ] -1711462098.520793 [0] tev: traffic-xmit (1) 64 -1711462098.542263 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 116 from udp/10.101.12.71:40473 -1711462098.542280 [0] recv: INFOTS(1711462098.541939305) -1711462098.542298 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:200c2 -> 0:0:0:0 #2 L(:1c1 16946.201782)) -1711462098.542316 [0] recv: HEARTBEAT(#8:2..2 110d7b4:17c5b8c5:94bd0ff4:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@2(sync)) -1711462098.542395 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110d7b4:17c5b8c5:94bd0ff4:1}:1<0> -1711462098.542430 [0] dq.builtin: PMD ST0 pp 110d7b4:17c5b8c5:94bd0ff4 kind 1 data 1 -1711462098.542515 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#3:3/0: -1711462098.542571 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462098.542594 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462098.542707 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7414@2 ] -1711462098.542749 [0] tev: traffic-xmit (1) 64 -1711462098.549632 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 116 from udp/10.101.12.71:44991 -1711462098.549651 [0] recv: INFOTS(1711462098.549450527) -1711462098.549676 [0] recv: DATA(110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0 #2 L(:1c1 16946.209150)) -1711462098.549694 [0] recv: HEARTBEAT(#8:2..2 110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@2(sync)) -1711462098.549868 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110443d:bb7f10a5:18901533:1}:1<0> -1711462098.549904 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#4:3/0: -1711462098.549965 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462098.549984 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462098.549917 [0] dq.builtin: PMD ST0 pp 110443d:bb7f10a5:18901533 kind 1 data 1 -1711462098.550082 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7416@2 ] -1711462098.550245 [0] tev: traffic-xmit (1) 64 -1711462098.570629 [0] tev: write_sample 110aba1:7b19badd:ce0adb73:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110aba1:7b19badd:ce0adb73:1}:1<0> -1711462098.570680 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(data(110aba1:7b19badd:ce0adb73:200c2:#2/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462098.570691 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:200c2 multicasting (rel-prd 6 seq-eq-max 6 seq 2 maxseq 1) -1711462098.570704 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:200c2) piggybacked, resched in 0.1 s (min-ack 1, avail-seq 2, xmit 1) -1711462098.570712 [0] tev: xpack_addmsg 0x770414001390 0x7704040038b0 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462098.570784 [0] tev: nn_xpack_send 116: 0x77041400139c:20 0x770404003b28:36 0x77041400279c:28 0x7704040039a8:32 [ udp/239.255.0.1:7400@2 ] -1711462098.570793 [0] tev: traffic-xmit (1) 116 -1711462098.570802 [0] tev: resched pmd(110aba1:7b19badd:ce0adb73:1c1): 8s -1711462098.570830 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 116 from udp/10.101.12.71:52025 -1711462098.570851 [0] recv: INFOTS(1711462098.570608394) -1711462098.570872 [0] recv: DATA(110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0 #2 110aba1:7b19badd:ce0adb73:200c2? -> 0:0:0:0) -1711462098.570879 [0] recv: HEARTBEAT(#6:2..2 110aba1:7b19badd:ce0adb73:200c2? -> 0:0:0:0) -1711462098.571015 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462098.571023 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462098.571041 [0] recv: ACKNACK(F#4:3/0: L(:1c1 16946.230525) 110443d:bb7f10a5:18901533:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462098.571087 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462098.571093 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462098.571102 [0] recv: ACKNACK(F#4:3/0: L(:1c1 16946.230597) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462098.571140 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462098.571148 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462098.571155 [0] recv: ACKNACK(F#4:3/0: L(:1c1 16946.230651) 110e21c:d0762c48:a9f0fb28:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462098.571163 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462098.571167 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462098.571174 [0] recv: ACKNACK(F#3:3/0: L(:1c1 16946.230670) 110cd0d:56a27910:57318171:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462098.571178 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462098.571185 [0] recv: ACKNACK(F#9:3/0: L(:1c1 16946.230670) 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462098.571189 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462098.571196 [0] recv: ACKNACK(F#7:3/0: L(:1c1 16946.230670) 110cd0d:79d6eeac:ea4a8907:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462098.620168 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462098.620193 [0] recv: INFOTS(1711462090.519442682) -1711462098.620226 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462098.620331 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462098.620364 [0] recv: INFOTS(1711462090.519442682) -1711462098.620382 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462098.620344 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.620434 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16946.279931) -1711462098.620487 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.620539 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16946.280038) -1711462098.642437 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462098.642485 [0] recv: INFOTS(1711462090.541682763) -1711462098.642546 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462098.642570 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462098.642581 [0] recv: INFOTS(1711462090.541682763) -1711462098.642591 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462098.642631 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.642681 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16946.302181) -1711462098.642723 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.642755 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16946.302256) -1711462098.648195 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #8: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462098,648083000},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462098.648249 [0] python3: => EVERYONE -1711462098.649872 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462098.649901 [0] recv: INFOTS(1711462090.549308855) -1711462098.649927 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462098.649949 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462098.649960 [0] recv: INFOTS(1711462090.549308855) -1711462098.649974 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462098.650023 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.650075 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16946.309575) -1711462098.650128 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.650140 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16946.309642) -1711462098.670621 [0] tev: non-timed queue now has 1 items -1711462098.670666 [0] tev: xmit spdp 110aba1:7b19badd:ce0adb73:1c1 to 0:0:0:100c7 (resched 8s) -1711462098.670680 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:200c2) suppressed, resched in inf s (min-ack 2, avail-seq 2, xmit 2) -1711462098.670690 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462098.670820 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462098.670844 [0] recv: INFOTS(1711462090.570393375) -1711462098.670857 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462098.670868 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462098.670873 [0] recv: INFOTS(1711462090.570393375) -1711462098.670877 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462098.670944 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.670968 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (local) -1711462098.670992 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.671001 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (local) -1711462098.671924 [0] tev: nn_xpack_send 444: 0x77041400139c:20 0x770404003b28:36 0x653a1b2b4514:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462098.671937 [0] tev: traffic-xmit (101) 444 -1711462099.651577 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #9: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462099,651428765},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462099.651663 [0] python3: => EVERYONE -1711462099.906728 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462099.906796 [0] recv: INFOTS(1711462099.906152938) -1711462099.906823 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0 #5 L(:1c1 16947.566296)) -1711462099.906845 [0] recv: HEARTBEAT(#30:5..5 110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@5(sync)) -1711462099.906862 [0] recv: thread_cputime 0.012825000 -1711462099.907028 [0] tev: thread_cputime 0.000000000 -1711462099.907048 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:200c2 #5: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:e3b81b8d:1ccb65b1:1}:1<0> -1711462099.907153 [0] dq.builtin: PMD ST0 pp 110cd0d:e3b81b8d:1ccb65b1 kind 1 data 1 -1711462099.907168 [0] dq.builtin: thread_cputime 0.005635000 -1711462099.907106 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2: F#4:6/0: -1711462099.907252 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462099.907271 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462099.907348 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7410@2 ] -1711462099.907360 [0] tev: traffic-xmit (1) 64 -1711462100.006679 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462100.006728 [0] recv: INFOTS(1711462067.905323275) -1711462100.006756 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462100.006916 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462100.006931 [0] recv: INFOTS(1711462067.905323275) -1711462100.007000 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462100.006914 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462100.007163 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16947.666654) -1711462100.007223 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462100.007236 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16947.666737) -1711462100.571482 [0] gc: thread_cputime 0.000000000 -1711462100.654475 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #10: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462100,654389597},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462100.654521 [0] python3: => EVERYONE -1711462101.626198 [0] tev: thread_cputime 0.000000000 -1711462101.655202 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462101.655304 [0] recv: INFOTS(1711462101.654695551) -1711462101.655335 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0 #5 L(:1c1 16949.314801)) -1711462101.655369 [0] recv: HEARTBEAT(#22:5..5 110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@5(sync)) -1711462101.655392 [0] recv: thread_cputime 0.013366000 -1711462101.655587 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:200c2 #5: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:79d6eeac:ea4a8907:1}:1<0> -1711462101.655671 [0] dq.builtin: PMD ST0 pp 110cd0d:79d6eeac:ea4a8907 kind 1 data 1 -1711462101.655701 [0] dq.builtin: thread_cputime 0.005635000 -1711462101.655589 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2: F#4:6/0: -1711462101.655773 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:79d6eeac:ea4a8907:200c2) -1711462101.655797 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462101.655908 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7410@2 ] -1711462101.655927 [0] tev: traffic-xmit (1) 64 -1711462101.657900 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #11: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462101,657789967},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462101.657943 [0] python3: => EVERYONE -1711462101.754736 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462101.754788 [0] recv: INFOTS(1711462069.653526636) -1711462101.754823 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462101.754863 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462101.754878 [0] recv: INFOTS(1711462069.653526636) -1711462101.754893 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462101.755054 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462101.755145 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16949.414642) -1711462101.755193 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462101.755229 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16949.414730) -1711462102.628783 [0] tev: thread_cputime 0.000000000 -1711462102.661508 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #12: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462102,661351033},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462102.661601 [0] python3: => EVERYONE -1711462103.631282 [0] tev: thread_cputime 0.000000000 -1711462103.664954 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #13: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462103,664794328},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462103.665041 [0] python3: => EVERYONE -1711462104.633681 [0] tev: thread_cputime 0.000000000 -1711462104.668242 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #14: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462104,668103899},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462104.668313 [0] python3: => EVERYONE -1711462105.636913 [0] tev: thread_cputime 0.000000000 -1711462105.671620 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #15: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462105,671463026},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462105.671708 [0] python3: => EVERYONE -1711462106.144559 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462106.144635 [0] recv: INFOTS(1711462106.144172669) -1711462106.144658 [0] recv: DATA(110cd0d:56a27910:57318171:200c2 -> 0:0:0:0 #3 L(:1c1 16953.804135)) -1711462106.144682 [0] recv: HEARTBEAT(#10:3..3 110cd0d:56a27910:57318171:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@3(sync)) -1711462106.144698 [0] recv: thread_cputime 0.013366000 -1711462106.144815 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2: F#4:4/0: -1711462106.144863 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:56a27910:57318171:200c2) -1711462106.144878 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462106.144943 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7410@2 ] -1711462106.144831 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:56a27910:57318171:1}:1<0> -1711462106.144956 [0] tev: traffic-xmit (1) 64 -1711462106.144998 [0] dq.builtin: PMD ST0 pp 110cd0d:56a27910:57318171 kind 1 data 1 -1711462106.145017 [0] dq.builtin: thread_cputime 0.005635000 -1711462106.244546 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462106.244579 [0] recv: INFOTS(1711462090.143481252) -1711462106.244601 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 0:0:0:0 #1) -1711462106.244613 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462106.244618 [0] recv: INFOTS(1711462090.143481252) -1711462106.244624 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 0:0:0:0 #1) -1711462106.244669 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.244706 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (known) L(:1c1 16953.904206) -1711462106.244751 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.244760 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (known) L(:1c1 16953.904262) -1711462106.520048 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 116 from udp/10.101.12.71:58212 -1711462106.520091 [0] recv: INFOTS(1711462106.519773993) -1711462106.520112 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:200c2 -> 0:0:0:0 #3 L(:1c1 16954.179592)) -1711462106.520127 [0] recv: HEARTBEAT(#11:3..3 110e21c:d0762c48:a9f0fb28:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@3(sync)) -1711462106.520168 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110e21c:d0762c48:a9f0fb28:1}:1<0> -1711462106.520169 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#4:4/0: -1711462106.520217 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462106.520186 [0] dq.builtin: PMD ST0 pp 110e21c:d0762c48:a9f0fb28 kind 1 data 1 -1711462106.520229 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462106.520298 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7412@2 ] -1711462106.520307 [0] tev: traffic-xmit (1) 64 -1711462106.542264 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 116 from udp/10.101.12.71:40473 -1711462106.542292 [0] recv: INFOTS(1711462106.542026862) -1711462106.542314 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:200c2 -> 0:0:0:0 #3 L(:1c1 16954.201793)) -1711462106.542327 [0] recv: HEARTBEAT(#9:3..3 110d7b4:17c5b8c5:94bd0ff4:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@3(sync)) -1711462106.542419 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110d7b4:17c5b8c5:94bd0ff4:1}:1<0> -1711462106.542446 [0] dq.builtin: PMD ST0 pp 110d7b4:17c5b8c5:94bd0ff4 kind 1 data 1 -1711462106.542495 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#4:4/0: -1711462106.542524 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462106.542547 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462106.542596 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7414@2 ] -1711462106.542606 [0] tev: traffic-xmit (1) 64 -1711462106.549745 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 116 from udp/10.101.12.71:44991 -1711462106.549764 [0] recv: INFOTS(1711462106.549523877) -1711462106.549780 [0] recv: DATA(110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0 #3 L(:1c1 16954.209263)) -1711462106.549804 [0] recv: HEARTBEAT(#9:3..3 110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@3(sync)) -1711462106.549842 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110443d:bb7f10a5:18901533:1}:1<0> -1711462106.549872 [0] dq.builtin: PMD ST0 pp 110443d:bb7f10a5:18901533 kind 1 data 1 -1711462106.550007 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#5:4/0: -1711462106.550058 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462106.550077 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462106.550183 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7416@2 ] -1711462106.550206 [0] tev: traffic-xmit (1) 64 -1711462106.570871 [0] tev: write_sample 110aba1:7b19badd:ce0adb73:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110aba1:7b19badd:ce0adb73:1}:1<0> -1711462106.570967 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(data(110aba1:7b19badd:ce0adb73:200c2:#3/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462106.570988 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:200c2 multicasting (rel-prd 6 seq-eq-max 6 seq 3 maxseq 2) -1711462106.571013 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:200c2) piggybacked, resched in 0.1 s (min-ack 2, avail-seq 3, xmit 2) -1711462106.571029 [0] tev: xpack_addmsg 0x770414001390 0x7704040038b0 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462106.571180 [0] tev: nn_xpack_send 116: 0x77041400139c:20 0x770404003b28:36 0x7704140024ec:28 0x7704040039a8:32 [ udp/239.255.0.1:7400@2 ] -1711462106.571198 [0] tev: traffic-xmit (1) 116 -1711462106.571218 [0] tev: resched pmd(110aba1:7b19badd:ce0adb73:1c1): 8s -1711462106.571326 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 116 from udp/10.101.12.71:52025 -1711462106.571381 [0] recv: INFOTS(1711462106.570832443) -1711462106.571402 [0] recv: DATA(110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0 #3 110aba1:7b19badd:ce0adb73:200c2? -> 0:0:0:0) -1711462106.571437 [0] recv: HEARTBEAT(#7:3..3 110aba1:7b19badd:ce0adb73:200c2? -> 0:0:0:0) -1711462106.571911 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462106.571966 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462106.572003 [0] recv: ACKNACK(F#5:4/0: L(:1c1 16954.231463) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462106.572033 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462106.572043 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462106.572059 [0] recv: ACKNACK(F#5:4/0: L(:1c1 16954.231544) 110443d:bb7f10a5:18901533:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462106.572076 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462106.572084 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462106.572106 [0] recv: ACKNACK(F#5:4/0: L(:1c1 16954.231587) 110e21c:d0762c48:a9f0fb28:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462106.572210 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462106.572228 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462106.572245 [0] recv: ACKNACK(F#4:4/0: L(:1c1 16954.231729) 110cd0d:56a27910:57318171:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462106.572254 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462106.572267 [0] recv: ACKNACK(F#10:4/0: L(:1c1 16954.231729) 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462106.572274 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462106.572289 [0] recv: ACKNACK(F#8:4/0: L(:1c1 16954.231729) 110cd0d:79d6eeac:ea4a8907:200c7 -> 110aba1:7b19badd:ce0adb73:200c2 ACK1 RM0) -1711462106.620320 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462106.620344 [0] recv: INFOTS(1711462090.519442682) -1711462106.620367 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462106.620376 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462106.620382 [0] recv: INFOTS(1711462090.519442682) -1711462106.620387 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462106.620557 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.620595 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16954.280095) -1711462106.620620 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.620629 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16954.280132) -1711462106.641679 [0] tev: thread_cputime 0.000000000 -1711462106.642550 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462106.642587 [0] recv: INFOTS(1711462090.541682763) -1711462106.642623 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462106.642643 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462106.642652 [0] recv: INFOTS(1711462090.541682763) -1711462106.642659 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462106.642797 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.642841 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16954.302341) -1711462106.642875 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.642883 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16954.302386) -1711462106.650183 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462106.650231 [0] recv: INFOTS(1711462090.549308855) -1711462106.650348 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462106.650366 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462106.650380 [0] recv: INFOTS(1711462090.549308855) -1711462106.650402 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462106.650454 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.650485 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16954.309983) -1711462106.650582 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.650604 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16954.310105) -1711462106.670860 [0] tev: non-timed queue now has 1 items -1711462106.670947 [0] tev: xmit spdp 110aba1:7b19badd:ce0adb73:1c1 to 0:0:0:100c7 (resched 8s) -1711462106.670973 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:200c2) suppressed, resched in inf s (min-ack 3, avail-seq 3, xmit 3) -1711462106.670997 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462106.671317 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462106.671367 [0] recv: INFOTS(1711462090.570393375) -1711462106.671433 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462106.671471 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462106.671489 [0] recv: INFOTS(1711462090.570393375) -1711462106.671501 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462106.671653 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.671751 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (local) -1711462106.671811 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.671824 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (local) -1711462106.673476 [0] tev: nn_xpack_send 444: 0x77041400139c:20 0x770404003b28:36 0x653a1b2b4514:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462106.673505 [0] tev: traffic-xmit (101) 444 -1711462106.675023 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #16: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462106,674889026},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462106.675069 [0] python3: => EVERYONE -1711462107.645244 [0] tev: thread_cputime 0.002890000 -1711462107.678436 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #17: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462107,678259247},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462107.678537 [0] python3: => EVERYONE -1711462107.906917 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462107.906974 [0] recv: INFOTS(1711462107.906432793) -1711462107.907028 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0 #6 L(:1c1 16955.566471)) -1711462107.907071 [0] recv: HEARTBEAT(#31:6..6 110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@6(sync)) -1711462107.907091 [0] recv: thread_cputime 0.013366000 -1711462107.907203 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2: F#5:7/0: -1711462107.907244 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462107.907297 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462107.907259 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:e3b81b8d:1ccb65b1:200c2 #6: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:e3b81b8d:1ccb65b1:1}:1<0> -1711462107.907364 [0] dq.builtin: PMD ST0 pp 110cd0d:e3b81b8d:1ccb65b1 kind 1 data 1 -1711462107.907401 [0] dq.builtin: thread_cputime 0.005635000 -1711462107.907378 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7410@2 ] -1711462107.907444 [0] tev: traffic-xmit (1) 64 -1711462108.007025 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462108.007108 [0] recv: INFOTS(1711462067.905323275) -1711462108.007181 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462108.007232 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462108.007241 [0] recv: INFOTS(1711462067.905323275) -1711462108.007261 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462108.007426 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462108.007559 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16955.667056) -1711462108.007620 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462108.007636 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (known) L(:1c1 16955.667137) -1711462108.571428 [0] gc: thread_cputime 0.000000000 -1711462108.648381 [0] tev: thread_cputime 0.003345000 -1711462108.681684 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #18: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462108,681534805},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462108.681781 [0] python3: => EVERYONE -1711462109.651755 [0] tev: thread_cputime 0.003477000 -1711462109.655014 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462109.655051 [0] recv: INFOTS(1711462109.654791672) -1711462109.655078 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0 #6 L(:1c1 16957.314552)) -1711462109.655092 [0] recv: HEARTBEAT(#23:6..6 110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:200c7@6(sync)) -1711462109.655103 [0] recv: thread_cputime 0.013701000 -1711462109.655137 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:79d6eeac:ea4a8907:200c2 #6: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:79d6eeac:ea4a8907:1}:1<0> -1711462109.655152 [0] dq.builtin: PMD ST0 pp 110cd0d:79d6eeac:ea4a8907 kind 1 data 1 -1711462109.655163 [0] dq.builtin: thread_cputime 0.005635000 -1711462109.655220 [0] tev: acknack 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2: F#5:7/0: -1711462109.655238 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:200c7 -> pwr 110cd0d:79d6eeac:ea4a8907:200c2) -1711462109.655248 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462109.655292 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7410@2 ] -1711462109.655304 [0] tev: traffic-xmit (1) 64 -1711462109.684395 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:603 #19: ST0 rt/rosout/rcl_interfaces::msg::dds_::Log_:{{1711462109,684296957},30,"topological_navigation_core","service not available, waiting again... /controller_server/set_parameters","/home/developer/aoc_strawberry_scenario_ws/install/topological_navigation/lib/python3.10/site-packages/topological_navigation/scripts/param_processing.py","__init__",18} -1711462109.684453 [0] python3: => EVERYONE -1711462109.754734 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462109.754771 [0] recv: INFOTS(1711462069.653526636) -1711462109.754810 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462109.754834 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462109.754841 [0] recv: INFOTS(1711462069.653526636) -1711462109.754853 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462109.755028 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462109.755092 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16957.414591) -1711462109.755138 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462109.755155 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (known) L(:1c1 16957.414657) -1711462110.007371 [0] gc: thread_cputime 0.000000000 -1711462110.151030 [0] recvMC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 536 from udp/10.101.12.71:50807 -1711462110.151095 [0] recvMC: INFOTS(1711462110.150718264) -1711462110.151121 [0] recvMC: DATA(110cd0d:56a27910:57318171:403 -> 0:0:0:0 #13 L(:1c1 16957.810589) => EVERYONE -1711462110.151322 [0] recvMC: data(application, vendor 1.16): 110cd0d:56a27910:57318171:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162) -1711462110.151365 [0] recvMC: HEARTBEAT(#9:13..13 110cd0d:56a27910:57318171:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@13(sync)) -1711462110.151377 [0] recvMC: thread_cputime 0.002372000 -1711462110.151389 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:56a27910:57318171:403: F#3:14/0: -1711462110.151411 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110cd0d:56a27910:57318171:403) -1711462110.151418 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.151450 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7411@2 ] -1711462110.151454 [0] tev: traffic-xmit (1) 64 -1711462110.151686 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 96 from udp/10.101.12.71:50807 -1711462110.151709 [0] recv: INFOTS(1711462110.151439386) -1711462110.151731 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 0:0:0:0 #10 L(:1c1 16957.811210)) -1711462110.151779 [0] dq.builtin: data(builtin, vendor 1.16): 110cd0d:56a27910:57318171:3c2 #10: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:603}} -1711462110.151804 [0] dq.builtin: SEDP ST3 110cd0d:56a27910:57318171:603 ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:603) - deleting -1711462110.151818 [0] dq.builtin: => EVERYONE -1711462110.151855 [0] dq.builtin: delete -1711462110.151921 [0] gc: gc 0x77041c0814a0: deleting -1711462110.151940 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c0814a0, 110cd0d:56a27910:57318171:603) -1711462110.151969 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c0814a0, 110cd0d:56a27910:57318171:603) -1711462110.151985 [0] dq.user: thread_cputime 0.000000000 -1711462110.152051 [0] gc: gc 0x77041c0814a0: deleting -1711462110.152069 [0] gc: gc_delete_proxy_writer (0x77041c0814a0, 110cd0d:56a27910:57318171:603) -1711462110.152080 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=30 -1711462110.152100 [0] gc: gc 0x77041c014c20: deleting -1711462110.176653 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 472 from udp/10.101.12.71:44991 -1711462110.176673 [0] recvMC: INFOTS(1711462110.176055959) -1711462110.176692 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #11 L(:1c1 16957.836173) => EVERYONE -1711462110.176695 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, -1711462110.176737 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#13/1)): niov 0 sz 0 => now niov 3 sz 568 -1711462110.176754 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 13 maxseq 12) -1711462110.176769 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) piggybacked, resched in 0.1 s (min-ack 12, avail-seq 13, xmit 12) -1711462110.176778 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 568 => now niov 4 sz 600 -1711462110.176815 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #11: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,17,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,16,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.176847 [0] recvMC: HEARTBEAT(#19:11..11 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@11(sync)) -1711462110.176854 [0] python3: nn_xpack_send 600: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1aae5e60:512 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.176887 [0] python3: traffic-xmit (1) 600 -1711462110.176897 [0] python3: => EVERYONE -1711462110.176903 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110443d:bb7f10a5:18901533:403: F#4:12/0: -1711462110.176944 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:1304) ... -1711462110.176965 [0] python3: => EVERYONE -1711462110.176951 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.176878 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 600 from udp/10.101.12.71:52025 -1711462110.177006 [0] recvMC: INFOTS(1711462110.176561088) -1711462110.177023 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #13 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.177035 [0] recvMC: HEARTBEAT(#23:13..13 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.177097 [0] tev: xpack_addmsg 0x770414001390 0x770404003a30 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.177150 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.177161 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770404003b18:44 [ udp/10.101.12.71:7417@2 ] -1711462110.177180 [0] tev: traffic-xmit (1) 64 -1711462110.177171 [0] recv: INFOTS(1711462110.176811721) -1711462110.177243 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #8 L(:1c1 16957.836671)) -1711462110.177019 [0] gc: gc 0x653a1aae64b0: not yet, shortsleep -1711462110.177381 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #8: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1304}} -1711462110.177425 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1304 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:1304) => EVERYONE -1711462110.177454 [0] dq.builtin: - deleting -1711462110.177467 [0] dq.builtin: delete -1711462110.177724 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.177749 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.177815 [0] recvUC: ACKNACK(F#4:14/0: L(:1c1 16957.837248) 110e21c:d0762c48:a9f0fb28:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0) -1711462110.177866 [0] recvUC: thread_cputime 0.000000000 -1711462110.177885 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.177896 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.177913 [0] recvUC: ACKNACK(F#4:14/0: L(:1c1 16957.837397) 110d7b4:17c5b8c5:94bd0ff4:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0) -1711462110.178113 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.178135 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.178158 [0] recvUC: ACKNACK(F#4:14/0: L(:1c1 16957.837635) 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0) -1711462110.178167 [0] recvUC: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.178183 [0] recvUC: ACKNACK(F#20:14/0: L(:1c1 16957.837635) 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0) -1711462110.178191 [0] recvUC: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.178204 [0] recvUC: ACKNACK(F#11:14/0: L(:1c1 16957.837635) 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0) -1711462110.178220 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.178229 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.178243 [0] recvUC: ACKNACK(F#4:14/0: L(:1c1 16957.837731) 110443d:bb7f10a5:18901533:504 -> 110aba1:7b19badd:ce0adb73:403 ACK1 RM0) -1711462110.178358 [0] gc: gc 0x653a1aae64b0: deleting -1711462110.178373 [0] gc: gc_delete_reader(0x653a1aae64b0, 110aba1:7b19badd:ce0adb73:1304) -1711462110.178420 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1304}} -1711462110.178442 [0] gc: non-timed queue now has 1 items -1711462110.178465 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1304 @ 0x653a1b444a34) user 25 builtin 12 -1711462110.178464 [0] tev: xpack_addmsg 0x770414001390 0x7704240013d0 0(data(110aba1:7b19badd:ce0adb73:4c2:#11/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.178483 [0] gc: gc 0x77041c0814a0: deleting -1711462110.178495 [0] gc: gc_delete_proxy_reader (0x77041c0814a0, 110443d:bb7f10a5:18901533:1304) -1711462110.178506 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=28 -1711462110.178519 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.178553 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:1203) ... -1711462110.178566 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1203) state transition 0 -> 1 -1711462110.178573 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1203) ... -1711462110.178578 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1203) - no unack'ed samples -1711462110.178583 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:1203) ... -1711462110.178599 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.178600 [0] python3: => EVERYONE -1711462110.178581 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x7704240014c8:48 0x7704240015b4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.178630 [0] recv: INFOTS(1711462110.178392915) -1711462110.178651 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1203) state transition 1 -> 3 -1711462110.178675 [0] tev: traffic-xmit (1) 96 -1711462110.178658 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #11 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.179464 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.179494 [0] recv: INFOTS(1711462110.179299892) -1711462110.179515 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #11 L(:1c1 16957.838995)) -1711462110.179583 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #11: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1203}} -1711462110.179614 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1203 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:1203) - deleting -1711462110.179614 [0] gc: gc 0x77041c014c20: deleting -1711462110.179655 [0] gc: gc 0x77041c0814a0: deleting -1711462110.179666 [0] gc: gc 0x653a1b4507a0: deleting -1711462110.179630 [0] dq.builtin: => EVERYONE -1711462110.179674 [0] gc: gc_delete_writer(0x653a1b4507a0, 110aba1:7b19badd:ce0adb73:1203) -1711462110.179707 [0] dq.builtin: delete -1711462110.179735 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1203}} -1711462110.179735 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 424 from udp/10.101.12.71:44991 -1711462110.179775 [0] gc: non-timed queue now has 1 items -1711462110.179793 [0] recvMC: INFOTS(1711462110.179423769) -1711462110.179837 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1203 @ 0x653a1b4523c4) user 24 builtin 12 -1711462110.179845 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #12 L(:1c1 16957.839293) => EVERYONE -1711462110.179848 [0] gc: gc 0x77041c0814a0: deleting -1711462110.179879 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c0814a0, 110443d:bb7f10a5:18901533:1203) -1711462110.179836 [0] tev: xpack_addmsg 0x770414001390 0x770424001680 0(data(110aba1:7b19badd:ce0adb73:3c2:#13/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.179887 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.179909 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c0814a0, 110443d:bb7f10a5:18901533:1203) -1711462110.179962 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.179980 [0] recvMC: HEARTBEAT(F#20:12..12 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@12(sync)) -1711462110.180007 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.180040 [0] recv: INFOTS(1711462110.179720416) -1711462110.180009 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424001778:48 0x770424001864:28 [ udp/239.255.0.1:7400@2 ] -1711462110.180072 [0] tev: traffic-xmit (1) 96 -1711462110.180056 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, -1711462110.180115 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#14/1)): niov 0 sz 0 => now niov 3 sz 520 -1711462110.180131 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 14 maxseq 13) -1711462110.180057 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #13 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.180151 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0966763 s (min-ack 13, avail-seq 14, xmit 13) -1711462110.180189 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 520 => now niov 4 sz 552 -1711462110.180251 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 552 from udp/10.101.12.71:52025 -1711462110.180259 [0] python3: nn_xpack_send 552: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1ae73c30:464 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.180259 [0] recvMC: INFOTS(1711462110.179892567) -1711462110.180277 [0] python3: traffic-xmit (1) 552 -1711462110.180294 [0] python3: => EVERYONE -1711462110.180307 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:1104) ... -1711462110.180317 [0] python3: => EVERYONE -1711462110.180310 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #14 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.180337 [0] recvMC: HEARTBEAT(F#24:14..14 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.180811 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.180833 [0] recv: INFOTS(1711462110.180647407) -1711462110.180863 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #9 L(:1c1 16957.840333)) -1711462110.180905 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #9: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1104}} -1711462110.180922 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1104 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:1104) => EVERYONE -1711462110.180935 [0] dq.builtin: - deleting -1711462110.180945 [0] dq.builtin: delete -1711462110.180971 [0] gc: gc 0x77041c014c20: deleting -1711462110.180979 [0] gc: gc 0x77041c0814a0: deleting -1711462110.180987 [0] gc: gc_delete_proxy_writer (0x77041c0814a0, 110443d:bb7f10a5:18901533:1203) -1711462110.180994 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=27 -1711462110.181004 [0] gc: gc 0x653a1b4507a0: deleting -1711462110.181009 [0] gc: gc_delete_reader(0x653a1b4507a0, 110aba1:7b19badd:ce0adb73:1104) -1711462110.181026 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1104}} -1711462110.181036 [0] gc: non-timed queue now has 1 items -1711462110.181049 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1104 @ 0x653a1b44b6f4) user 23 builtin 12 -1711462110.181055 [0] gc: gc 0x77041c0816d0: deleting -1711462110.181068 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:1003) ... -1711462110.181070 [0] gc: gc_delete_proxy_reader (0x77041c0816d0, 110443d:bb7f10a5:18901533:1104) -1711462110.181083 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1003) state transition 0 -> 1 -1711462110.181061 [0] tev: xpack_addmsg 0x770414001390 0x770424001890 0(data(110aba1:7b19badd:ce0adb73:4c2:#12/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.181114 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=26 -1711462110.181096 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1003) ... -1711462110.181124 [0] gc: gc 0x77041c014c20: deleting -1711462110.181142 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1003) - no unack'ed samples -1711462110.181148 [0] gc: gc 0x653a1b4507a0: not yet, shortsleep -1711462110.181149 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:1003) ... -1711462110.181168 [0] python3: => EVERYONE -1711462110.181176 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424001988:48 0x770424001a74:28 [ udp/239.255.0.1:7400@2 ] -1711462110.181185 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.181212 [0] recv: INFOTS(1711462110.181018038) -1711462110.181181 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1003) state transition 1 -> 3 -1711462110.181193 [0] tev: traffic-xmit (1) 96 -1711462110.181227 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #12 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.182015 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.182030 [0] recv: INFOTS(1711462110.181868318) -1711462110.182045 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #12 L(:1c1 16957.841531)) -1711462110.182101 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #12: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1003}} -1711462110.182124 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1003 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:1003) - deleting -1711462110.182133 [0] dq.builtin: => EVERYONE -1711462110.182147 [0] dq.builtin: delete -1711462110.182199 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 376 from udp/10.101.12.71:44991 -1711462110.182215 [0] gc: gc 0x653a1b4507a0: deleting -1711462110.182236 [0] gc: gc 0x77041c0816d0: deleting -1711462110.182264 [0] gc: gc 0x653a1b4509d0: deleting -1711462110.182280 [0] gc: gc_delete_writer(0x653a1b4509d0, 110aba1:7b19badd:ce0adb73:1003) -1711462110.182218 [0] recvMC: INFOTS(1711462110.181963719) -1711462110.182306 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1003}} -1711462110.182316 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #13 L(:1c1 16957.841719) => EVERYONE -1711462110.182323 [0] gc: non-timed queue now has 1 items -1711462110.182355 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1003 @ 0x653a1b44a5e4) user 22 builtin 12 -1711462110.182369 [0] gc: gc 0x77041c0814a0: deleting -1711462110.182375 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c0814a0, 110443d:bb7f10a5:18901533:1003) -1711462110.182360 [0] tev: xpack_addmsg 0x770414001390 0x770424001aa0 0(data(110aba1:7b19badd:ce0adb73:3c2:#14/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.182391 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.182409 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c0814a0, 110443d:bb7f10a5:18901533:1003) -1711462110.182445 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.182458 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424001b98:48 0x770424001c84:28 [ udp/239.255.0.1:7400@2 ] -1711462110.182473 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.182488 [0] recv: INFOTS(1711462110.182296975) -1711462110.182462 [0] recvMC: HEARTBEAT(F#21:13..13 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@13(sync)) -1711462110.182507 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #14 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.182478 [0] tev: traffic-xmit (1) 96 -1711462110.182524 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, -1711462110.182547 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#15/1)): niov 0 sz 0 => now niov 3 sz 472 -1711462110.182555 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 15 maxseq 13) -1711462110.182583 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0941893 s (min-ack 13, avail-seq 15, xmit 14) -1711462110.182590 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 472 => now niov 4 sz 504 -1711462110.182644 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 504 from udp/10.101.12.71:52025 -1711462110.182650 [0] recvMC: INFOTS(1711462110.182388702) -1711462110.182668 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #15 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.182685 [0] recvMC: HEARTBEAT(F#25:15..15 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.182651 [0] python3: nn_xpack_send 504: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b4591d0:416 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.182705 [0] python3: traffic-xmit (1) 504 -1711462110.182710 [0] python3: => EVERYONE -1711462110.182718 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:f04) ... -1711462110.182723 [0] python3: => EVERYONE -1711462110.183201 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.183211 [0] recv: INFOTS(1711462110.183032571) -1711462110.183228 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #10 L(:1c1 16957.842714)) -1711462110.183280 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #10: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:f04}} -1711462110.183303 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:f04 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:f04) => EVERYONE -1711462110.183317 [0] dq.builtin: - deleting -1711462110.183329 [0] dq.builtin: delete -1711462110.183496 [0] gc: gc 0x77041c014c20: deleting -1711462110.183516 [0] gc: gc 0x77041c0814a0: deleting -1711462110.183528 [0] gc: gc_delete_proxy_writer (0x77041c0814a0, 110443d:bb7f10a5:18901533:1003) -1711462110.183542 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=25 -1711462110.183560 [0] gc: gc 0x653a1b4507a0: deleting -1711462110.183569 [0] gc: gc_delete_reader(0x653a1b4507a0, 110aba1:7b19badd:ce0adb73:f04) -1711462110.183611 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:f04}} -1711462110.183661 [0] gc: non-timed queue now has 1 items -1711462110.183687 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:f04 @ 0x653a1b39a0c4) user 21 builtin 12 -1711462110.183693 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:e03) ... -1711462110.183699 [0] gc: gc 0x77041c031bc0: deleting -1711462110.183712 [0] gc: gc_delete_proxy_reader (0x77041c031bc0, 110443d:bb7f10a5:18901533:f04) -1711462110.183700 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:e03) state transition 0 -> 1 -1711462110.183699 [0] tev: xpack_addmsg 0x770414001390 0x770424001cb0 0(data(110aba1:7b19badd:ce0adb73:4c2:#13/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.183724 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:e03) ... -1711462110.183750 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:e03) - no unack'ed samples -1711462110.183755 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:e03) ... -1711462110.183755 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=24 -1711462110.183766 [0] python3: => EVERYONE -1711462110.183780 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:e03) state transition 1 -> 3 -1711462110.183782 [0] gc: gc 0x77041c014c20: deleting -1711462110.183791 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424001da8:48 0x770424001e94:28 [ udp/239.255.0.1:7400@2 ] -1711462110.183809 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.183836 [0] recv: INFOTS(1711462110.183595883) -1711462110.183851 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #13 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.183869 [0] tev: traffic-xmit (1) 96 -1711462110.183816 [0] gc: gc 0x653a1b4507a0: not yet, shortsleep -1711462110.184426 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.184449 [0] recv: INFOTS(1711462110.184270366) -1711462110.184475 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #13 L(:1c1 16957.843949)) -1711462110.184546 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:e03}} -1711462110.184560 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:e03 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:e03) - deleting -1711462110.184565 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 328 from udp/10.101.12.71:44991 -1711462110.184570 [0] dq.builtin: => EVERYONE -1711462110.184669 [0] dq.builtin: delete -1711462110.184653 [0] recvMC: INFOTS(1711462110.184354578) -1711462110.184755 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #14 L(:1c1 16957.844147) => EVERYONE -1711462110.184918 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.184956 [0] recvMC: HEARTBEAT(F#22:14..14 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@14(sync)) -1711462110.184970 [0] gc: gc 0x653a1b4507a0: deleting -1711462110.185010 [0] gc: gc 0x77041c031bc0: deleting -1711462110.185025 [0] gc: gc 0x653a1b4509d0: deleting -1711462110.185033 [0] gc: gc_delete_writer(0x653a1b4509d0, 110aba1:7b19badd:ce0adb73:e03) -1711462110.185080 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:e03}} -1711462110.185099 [0] gc: non-timed queue now has 1 items -1711462110.185124 [0] tev: xpack_addmsg 0x770414001390 0x770424001ec0 0(data(110aba1:7b19badd:ce0adb73:3c2:#15/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.185127 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:e03 @ 0x653a1b398e84) user 20 builtin 12 -1711462110.185166 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.185174 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02e1f0, 110443d:bb7f10a5:18901533:e03) -1711462110.185185 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.185202 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424001fb8:48 0x7704240020a4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.185225 [0] tev: traffic-xmit (1) 96 -1711462110.185204 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.185247 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.185263 [0] recv: INFOTS(1711462110.185066146) -1711462110.185281 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #15 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.185203 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02e1f0, 110443d:bb7f10a5:18901533:e03) -1711462110.185290 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#16/1)): niov 0 sz 0 => now niov 3 sz 424 -1711462110.185346 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 16 maxseq 13) -1711462110.185361 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0914344 s (min-ack 13, avail-seq 16, xmit 15) -1711462110.185374 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 424 => now niov 4 sz 456 -1711462110.185425 [0] python3: nn_xpack_send 456: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b3b0da0:368 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.185436 [0] python3: traffic-xmit (1) 456 -1711462110.185452 [0] python3: => EVERYONE -1711462110.185429 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 456 from udp/10.101.12.71:52025 -1711462110.185471 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:d04) ... -1711462110.185484 [0] python3: => EVERYONE -1711462110.185502 [0] recvMC: INFOTS(1711462110.185144558) -1711462110.185517 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #16 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.185529 [0] recvMC: HEARTBEAT(F#26:16..16 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.185651 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.185678 [0] recv: INFOTS(1711462110.185500957) -1711462110.185720 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #11 L(:1c1 16957.845179)) -1711462110.185745 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:d04}} -1711462110.185756 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:d04 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:d04) => EVERYONE -1711462110.185774 [0] dq.builtin: - deleting -1711462110.185788 [0] dq.builtin: delete -1711462110.186261 [0] gc: gc 0x77041c014c20: deleting -1711462110.186280 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.186289 [0] gc: gc_delete_proxy_writer (0x77041c02e1f0, 110443d:bb7f10a5:18901533:e03) -1711462110.186298 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=23 -1711462110.186315 [0] gc: gc 0x653a1b4507a0: deleting -1711462110.186322 [0] gc: gc_delete_reader(0x653a1b4507a0, 110aba1:7b19badd:ce0adb73:d04) -1711462110.186347 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:d04}} -1711462110.186365 [0] gc: non-timed queue now has 1 items -1711462110.186383 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:d04 @ 0x653a1b3948d4) user 19 builtin 12 -1711462110.186394 [0] gc: gc 0x77041c0313c0: deleting -1711462110.186401 [0] gc: gc_delete_proxy_reader (0x77041c0313c0, 110443d:bb7f10a5:18901533:d04) -1711462110.186409 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=22 -1711462110.186413 [0] tev: xpack_addmsg 0x770414001390 0x7704240020d0 0(data(110aba1:7b19badd:ce0adb73:4c2:#14/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.186422 [0] gc: gc 0x77041c014c20: deleting -1711462110.186438 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:c03) ... -1711462110.186462 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:c03) state transition 0 -> 1 -1711462110.186469 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:c03) ... -1711462110.186476 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:c03) - no unack'ed samples -1711462110.186453 [0] gc: gc 0x653a1b4507a0: deleting -1711462110.186492 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x7704240021c8:48 0x7704240022b4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.186482 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:c03) ... -1711462110.186518 [0] tev: traffic-xmit (1) 96 -1711462110.186496 [0] gc: gc 0x77041c0313c0: not yet, shortsleep -1711462110.186496 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.186575 [0] recv: INFOTS(1711462110.186335683) -1711462110.186578 [0] python3: => EVERYONE -1711462110.186592 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #14 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.186614 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:c03) state transition 1 -> 3 -1711462110.186991 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.187015 [0] recv: INFOTS(1711462110.186833962) -1711462110.187032 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #14 L(:1c1 16957.846517)) -1711462110.187085 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:c03}} -1711462110.187108 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:c03 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:c03) - deleting -1711462110.187116 [0] dq.builtin: => EVERYONE -1711462110.187132 [0] dq.builtin: delete -1711462110.187224 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 280 from udp/10.101.12.71:44991 -1711462110.187252 [0] recvMC: INFOTS(1711462110.186930064) -1711462110.187276 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #15 L(:1c1 16957.846751) => EVERYONE -1711462110.187418 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.187444 [0] recvMC: HEARTBEAT(F#23:15..15 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@15(sync)) -1711462110.187623 [0] gc: gc 0x77041c0313c0: deleting -1711462110.187642 [0] gc: gc 0x653a1b4507a0: deleting -1711462110.187650 [0] gc: gc_delete_writer(0x653a1b4507a0, 110aba1:7b19badd:ce0adb73:c03) -1711462110.187703 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:c03}} -1711462110.187723 [0] gc: non-timed queue now has 1 items -1711462110.187750 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:c03 @ 0x653a1b3939c4) user 18 builtin 12 -1711462110.187754 [0] tev: xpack_addmsg 0x770414001390 0x7704240022e0 0(data(110aba1:7b19badd:ce0adb73:3c2:#16/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.187765 [0] gc: gc 0x77041c031bc0: deleting -1711462110.187796 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c031bc0, 110443d:bb7f10a5:18901533:c03) -1711462110.187809 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.187826 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c031bc0, 110443d:bb7f10a5:18901533:c03) -1711462110.187837 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.187834 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x7704240023d8:48 0x7704240024c4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.187859 [0] recv: INFOTS(1711462110.187668713) -1711462110.187880 [0] tev: traffic-xmit (1) 96 -1711462110.187907 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #16 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.187924 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.187947 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#17/1)): niov 0 sz 0 => now niov 3 sz 376 -1711462110.187967 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 17 maxseq 13) -1711462110.187993 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0887647 s (min-ack 13, avail-seq 17, xmit 16) -1711462110.188002 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 376 => now niov 4 sz 408 -1711462110.188064 [0] python3: nn_xpack_send 408: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b44b100:320 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.188079 [0] python3: traffic-xmit (1) 408 -1711462110.188106 [0] python3: => EVERYONE -1711462110.188067 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 408 from udp/10.101.12.71:52025 -1711462110.188120 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:b04) ... -1711462110.188136 [0] recvMC: INFOTS(1711462110.187809186) -1711462110.188155 [0] python3: => EVERYONE -1711462110.188155 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #17 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.188183 [0] recvMC: HEARTBEAT(F#27:17..17 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.188354 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.188365 [0] recv: INFOTS(1711462110.188204641) -1711462110.188390 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #12 L(:1c1 16957.847868)) -1711462110.188456 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:b04}} -1711462110.188483 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:b04 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:b04) => EVERYONE -1711462110.188529 [0] dq.builtin: - deleting -1711462110.188546 [0] dq.builtin: delete -1711462110.188886 [0] gc: gc 0x77041c014c20: deleting -1711462110.188903 [0] gc: gc 0x77041c031bc0: deleting -1711462110.188911 [0] gc: gc_delete_proxy_writer (0x77041c031bc0, 110443d:bb7f10a5:18901533:c03) -1711462110.188920 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=21 -1711462110.188934 [0] gc: gc 0x653a1b392270: deleting -1711462110.188941 [0] gc: gc_delete_reader(0x653a1b392270, 110aba1:7b19badd:ce0adb73:b04) -1711462110.188965 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #15: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:b04}} -1711462110.188983 [0] gc: non-timed queue now has 1 items -1711462110.189004 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:b04 @ 0x653a1b38fd64) user 17 builtin 12 -1711462110.189017 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.189027 [0] gc: gc_delete_proxy_reader (0x77041c02e1f0, 110443d:bb7f10a5:18901533:b04) -1711462110.189038 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=20 -1711462110.189005 [0] tev: xpack_addmsg 0x770414001390 0x7704240024f0 0(data(110aba1:7b19badd:ce0adb73:4c2:#15/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.189056 [0] gc: gc 0x77041c014c20: deleting -1711462110.189065 [0] gc: gc 0x653a1b392270: not yet, shortsleep -1711462110.189106 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:a03) ... -1711462110.189138 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.189160 [0] recv: INFOTS(1711462110.188954699) -1711462110.189147 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:a03) state transition 0 -> 1 -1711462110.189171 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #15 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.189184 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:a03) ... -1711462110.189201 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:a03) - no unack'ed samples -1711462110.189211 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:a03) ... -1711462110.189230 [0] python3: => EVERYONE -1711462110.189118 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x7704240025e8:48 0x7704240026d4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.189289 [0] tev: traffic-xmit (1) 96 -1711462110.189270 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:a03) state transition 1 -> 3 -1711462110.189895 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.189914 [0] recv: INFOTS(1711462110.189703464) -1711462110.189932 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #15 L(:1c1 16957.849417)) -1711462110.189971 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:a03}} -1711462110.189989 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:a03 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:a03) - deleting -1711462110.190006 [0] dq.builtin: => EVERYONE -1711462110.190035 [0] dq.builtin: delete -1711462110.190100 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 232 from udp/10.101.12.71:44991 -1711462110.190123 [0] recvMC: INFOTS(1711462110.189872726) -1711462110.190138 [0] gc: gc 0x653a1b392270: deleting -1711462110.190140 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #16 L(:1c1 16957.849625) => EVERYONE -1711462110.190166 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.190204 [0] gc: gc 0x653a1b38e4e0: deleting -1711462110.190212 [0] gc: gc_delete_writer(0x653a1b38e4e0, 110aba1:7b19badd:ce0adb73:a03) -1711462110.190272 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:a03}} -1711462110.190292 [0] gc: non-timed queue now has 1 items -1711462110.190331 [0] tev: xpack_addmsg 0x770414001390 0x770424002700 0(data(110aba1:7b19badd:ce0adb73:3c2:#17/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.190333 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.190349 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:a03 @ 0x653a1b38f034) user 16 builtin 12 -1711462110.190377 [0] recvMC: HEARTBEAT(F#24:16..16 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@16(sync)) -1711462110.190387 [0] gc: gc 0x77041c02d8b0: deleting -1711462110.190397 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02d8b0, 110443d:bb7f10a5:18901533:a03) -1711462110.190422 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.190437 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02d8b0, 110443d:bb7f10a5:18901533:a03) -1711462110.190472 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.190522 [0] recv: INFOTS(1711462110.190259026) -1711462110.190536 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #17 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.190560 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.190491 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x7704240027f8:48 0x7704240028e4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.190616 [0] tev: traffic-xmit (1) 96 -1711462110.190599 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#18/1)): niov 0 sz 0 => now niov 3 sz 328 -1711462110.190792 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 18 maxseq 13) -1711462110.190814 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0861065 s (min-ack 13, avail-seq 18, xmit 17) -1711462110.190828 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 328 => now niov 4 sz 360 -1711462110.190910 [0] python3: nn_xpack_send 360: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b44bd40:272 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.190933 [0] python3: traffic-xmit (1) 360 -1711462110.190954 [0] python3: => EVERYONE -1711462110.190922 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 360 from udp/10.101.12.71:52025 -1711462110.190988 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:904) ... -1711462110.190996 [0] recvMC: INFOTS(1711462110.190464105) -1711462110.191003 [0] python3: => EVERYONE -1711462110.191014 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #18 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.191028 [0] recvMC: HEARTBEAT(F#28:18..18 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.191137 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.191161 [0] recv: INFOTS(1711462110.190989734) -1711462110.191186 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #13 L(:1c1 16957.850661)) -1711462110.191227 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:904}} -1711462110.191255 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:904 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:904) => EVERYONE -1711462110.191276 [0] dq.builtin: - deleting -1711462110.191288 [0] dq.builtin: delete -1711462110.191497 [0] gc: gc 0x77041c014c20: deleting -1711462110.191505 [0] gc: gc 0x77041c02d8b0: deleting -1711462110.191511 [0] gc: gc_delete_proxy_writer (0x77041c02d8b0, 110443d:bb7f10a5:18901533:a03) -1711462110.191518 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=19 -1711462110.191528 [0] gc: gc 0x653a1b314040: deleting -1711462110.191534 [0] gc: gc_delete_reader(0x653a1b314040, 110aba1:7b19badd:ce0adb73:904) -1711462110.191560 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #16: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:904}} -1711462110.191575 [0] gc: non-timed queue now has 1 items -1711462110.191588 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:904 @ 0x653a1b3ded14) user 15 builtin 12 -1711462110.191594 [0] gc: gc 0x77041c0313c0: deleting -1711462110.191598 [0] tev: xpack_addmsg 0x770414001390 0x770424002910 0(data(110aba1:7b19badd:ce0adb73:4c2:#16/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.191601 [0] gc: gc_delete_proxy_reader (0x77041c0313c0, 110443d:bb7f10a5:18901533:904) -1711462110.191630 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=18 -1711462110.191630 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:803) ... -1711462110.191646 [0] gc: gc 0x77041c014c20: deleting -1711462110.191663 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:803) state transition 0 -> 1 -1711462110.191687 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:803) ... -1711462110.191646 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 508 from udp/10.101.12.71:58212 -1711462110.191706 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424002a08:48 0x770424002af4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.191744 [0] tev: traffic-xmit (1) 96 -1711462110.191702 [0] gc: gc 0x653a1b314040: not yet, shortsleep -1711462110.191745 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.191818 [0] recv: INFOTS(1711462110.191549194) -1711462110.191827 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:803) - no unack'ed samples -1711462110.191843 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #16 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.191717 [0] recvMC: INFOTS(1711462110.191353654) -1711462110.191876 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:803) ... -1711462110.191883 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #12 L(:1c1 16957.851217) => EVERYONE -1711462110.191991 [0] python3: => EVERYONE -1711462110.192070 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:803) state transition 1 -> 3 -1711462110.192148 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,16,3,0,0,) -1711462110.192186 [0] recvMC: HEARTBEAT(#19:12..12 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@12(sync)) -1711462110.192214 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110e21c:d0762c48:a9f0fb28:403: F#4:13/0: -1711462110.192233 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462110.192248 [0] tev: xpack_addmsg 0x770414001390 0x770424002910 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.192297 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x7704240029f8:44 [ udp/10.101.12.71:7413@2 ] -1711462110.192310 [0] tev: traffic-xmit (1) 64 -1711462110.192471 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.192482 [0] recv: INFOTS(1711462110.192364136) -1711462110.192513 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #16 L(:1c1 16957.851984)) -1711462110.192553 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:803}} -1711462110.192566 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:803 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:803) - deleting -1711462110.192577 [0] dq.builtin: => EVERYONE -1711462110.192601 [0] dq.builtin: delete -1711462110.192755 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 208 from udp/10.101.12.71:44991 -1711462110.192791 [0] recvMC: INFOTS(1711462110.192525975) -1711462110.192810 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #17 L(:1c1 16957.852291) => EVERYONE -1711462110.192854 [0] gc: gc 0x653a1b314040: deleting -1711462110.192865 [0] gc: gc 0x77041c0313c0: deleting -1711462110.192875 [0] gc: gc 0x653a1b392270: deleting -1711462110.192881 [0] gc: gc_delete_writer(0x653a1b392270, 110aba1:7b19badd:ce0adb73:803) -1711462110.192881 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.192908 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:803}} -1711462110.192918 [0] recvMC: HEARTBEAT(F#25:17..17 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@17(sync)) -1711462110.192920 [0] gc: non-timed queue now has 1 items -1711462110.192951 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:803 @ 0x653a1b2ebe84) user 14 builtin 12 -1711462110.192958 [0] gc: gc 0x77041c031bc0: deleting -1711462110.192966 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c031bc0, 110443d:bb7f10a5:18901533:803) -1711462110.192958 [0] tev: xpack_addmsg 0x770414001390 0x770424002b20 0(data(110aba1:7b19badd:ce0adb73:3c2:#18/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.192974 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.192999 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c031bc0, 110443d:bb7f10a5:18901533:803) -1711462110.193053 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424002c18:48 0x770424002d04:28 [ udp/239.255.0.1:7400@2 ] -1711462110.193075 [0] tev: traffic-xmit (1) 96 -1711462110.193158 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.193181 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#19/1)): niov 0 sz 0 => now niov 3 sz 304 -1711462110.193198 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 19 maxseq 13) -1711462110.193063 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.193212 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0835409 s (min-ack 13, avail-seq 19, xmit 18) -1711462110.193232 [0] recv: INFOTS(1711462110.192899351) -1711462110.193238 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 304 => now niov 4 sz 336 -1711462110.193251 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #18 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.193300 [0] python3: nn_xpack_send 336: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b39ce30:248 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.193310 [0] python3: traffic-xmit (1) 336 -1711462110.193318 [0] python3: => EVERYONE -1711462110.193338 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:703) ... -1711462110.193344 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:703) state transition 0 -> 1 -1711462110.193353 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:703) ... -1711462110.193359 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:703) - no unack'ed samples -1711462110.193371 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:703) ... -1711462110.193379 [0] python3: => EVERYONE -1711462110.193394 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:703) state transition 1 -> 3 -1711462110.193398 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 336 from udp/10.101.12.71:52025 -1711462110.193434 [0] recvMC: INFOTS(1711462110.193029153) -1711462110.193456 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #19 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.193487 [0] recvMC: HEARTBEAT(F#29:19..19 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.193823 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.193840 [0] recv: INFOTS(1711462110.193730576) -1711462110.193877 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #17 L(:1c1 16957.853340)) -1711462110.193905 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:703}} -1711462110.193916 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:703 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:703) - deleting -1711462110.193938 [0] dq.builtin: => EVERYONE -1711462110.193955 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.193973 [0] recv: INFOTS(1711462110.193391175) -1711462110.193959 [0] dq.builtin: delete -1711462110.193999 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #9 L(:1c1 16957.853474)) -1711462110.194060 [0] gc: gc 0x77041c014c20: deleting -1711462110.194118 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #9: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1304}} -1711462110.194122 [0] gc: gc 0x77041c031bc0: deleting -1711462110.194202 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 184 from udp/10.101.12.71:44991 -1711462110.194211 [0] gc: gc_delete_proxy_writer (0x77041c031bc0, 110443d:bb7f10a5:18901533:803) -1711462110.194220 [0] recvMC: INFOTS(1711462110.193879178) -1711462110.194145 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1304 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:1304) => EVERYONE -1711462110.194227 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=17 -1711462110.194276 [0] dq.builtin: - deleting -1711462110.194296 [0] gc: gc 0x653a1b314040: deleting -1711462110.194246 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #18 L(:1c1 16957.853719) => EVERYONE -1711462110.194309 [0] gc: gc_delete_writer(0x653a1b314040, 110aba1:7b19badd:ce0adb73:703) -1711462110.194373 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #19: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:703}} -1711462110.194383 [0] dq.builtin: delete -1711462110.194375 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.194404 [0] gc: non-timed queue now has 1 items -1711462110.194469 [0] recvMC: HEARTBEAT(F#26:18..18 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@18(sync)) -1711462110.194490 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:703 @ 0x653a1b3ea6c4) user 13 builtin 12 -1711462110.194489 [0] tev: xpack_addmsg 0x770414001390 0x770424002d30 0(data(110aba1:7b19badd:ce0adb73:3c2:#19/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.194509 [0] gc: gc 0x77041c02cf40: deleting -1711462110.194542 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02cf40, 110443d:bb7f10a5:18901533:703) -1711462110.194557 [0] gc: gc 0x77041c014c20: deleting -1711462110.194574 [0] gc: gc 0x77041c030430: deleting -1711462110.194579 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02cf40, 110443d:bb7f10a5:18901533:703) -1711462110.194582 [0] gc: gc_delete_proxy_reader (0x77041c030430, 110e21c:d0762c48:a9f0fb28:1304) -1711462110.194616 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.194639 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=29 -1711462110.194657 [0] gc: gc 0x653a1b314040: not yet, shortsleep -1711462110.194620 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424002e28:48 0x770424002f14:28 [ udp/239.255.0.1:7400@2 ] -1711462110.194697 [0] tev: traffic-xmit (1) 96 -1711462110.194704 [0] recv: INFOTS(1711462110.194357465) -1711462110.194776 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #19 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.194726 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.194836 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#20/1)): niov 0 sz 0 => now niov 3 sz 256 -1711462110.194864 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 20 maxseq 13) -1711462110.194886 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.081939 s (min-ack 13, avail-seq 20, xmit 19) -1711462110.194900 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 256 => now niov 4 sz 288 -1711462110.194965 [0] python3: nn_xpack_send 288: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b383f20:200 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.194999 [0] python3: traffic-xmit (1) 288 -1711462110.195017 [0] python3: => EVERYONE -1711462110.194968 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 288 from udp/10.101.12.71:52025 -1711462110.195042 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:1504) ... -1711462110.195082 [0] recvMC: INFOTS(1711462110.194629401) -1711462110.195105 [0] python3: => EVERYONE -1711462110.195139 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #20 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.195171 [0] recvMC: HEARTBEAT(F#30:20..20 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.195419 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.195441 [0] recv: INFOTS(1711462110.195259141) -1711462110.195466 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #11 L(:1c1 16957.854941)) -1711462110.195506 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #11: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1203}} -1711462110.195520 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1203 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:1203) - deleting -1711462110.195540 [0] dq.builtin: => EVERYONE -1711462110.195579 [0] dq.builtin: delete -1711462110.195582 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.195622 [0] recv: INFOTS(1711462110.195373351) -1711462110.195646 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #18 L(:1c1 16957.855120)) -1711462110.195706 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1403}} -1711462110.195724 [0] gc: gc 0x653a1b314040: deleting -1711462110.195736 [0] gc: gc 0x77041c02cf40: deleting -1711462110.195754 [0] gc: gc_delete_proxy_writer (0x77041c02cf40, 110443d:bb7f10a5:18901533:703) -1711462110.195763 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=16 -1711462110.195775 [0] gc: gc 0x77041c014c20: deleting -1711462110.195725 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1403 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:1403) - deleting -1711462110.195782 [0] gc: gc 0x653a1b36a5e0: deleting -1711462110.195794 [0] dq.builtin: => EVERYONE -1711462110.195814 [0] gc: gc_delete_reader(0x653a1b36a5e0, 110aba1:7b19badd:ce0adb73:1504) -1711462110.195825 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 460 from udp/10.101.12.71:58212 -1711462110.195844 [0] recvMC: INFOTS(1711462110.195421660) -1711462110.195858 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #17: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1504}} -1711462110.195823 [0] dq.builtin: delete -1711462110.195867 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #13 L(:1c1 16957.855346) => EVERYONE -1711462110.195883 [0] gc: non-timed queue now has 1 items -1711462110.195945 [0] tev: xpack_addmsg 0x770414001390 0x770424002f40 0(data(110aba1:7b19badd:ce0adb73:4c2:#17/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.195949 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1504 @ 0x653a1b459964) user 12 builtin 12 -1711462110.195985 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.195991 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:1403) ... -1711462110.196015 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1403) state transition 0 -> 1 -1711462110.196043 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.196058 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1403) ... -1711462110.196068 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.196081 [0] recvMC: HEARTBEAT(F#20:13..13 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@13(sync)) -1711462110.196051 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424003038:48 0x770424003124:28 [ udp/239.255.0.1:7400@2 ] -1711462110.195995 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02e1f0, 110e21c:d0762c48:a9f0fb28:1203) -1711462110.196142 [0] tev: traffic-xmit (1) 96 -1711462110.196158 [0] gc: gc 0x653a1b314040: deleting -1711462110.196169 [0] gc: gc 0x77041c02d8b0: deleting -1711462110.196109 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 160 from udp/10.101.12.71:44991 -1711462110.196182 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02d8b0, 110443d:bb7f10a5:18901533:1403) -1711462110.196211 [0] recvMC: INFOTS(1711462110.195554831) -1711462110.196237 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #19 L(:1c1 16957.855713) => EVERYONE -1711462110.196075 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1403) - no unack'ed samples -1711462110.196281 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:1403) ... -1711462110.196299 [0] python3: => EVERYONE -1711462110.196307 [0] gc: gc 0x77041c014c20: deleting -1711462110.196323 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1403) state transition 1 -> 3 -1711462110.196169 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02e1f0, 110e21c:d0762c48:a9f0fb28:1203) -1711462110.196284 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{}}}}) -1711462110.196356 [0] gc: gc 0x653a1b38e4e0: not yet, shortsleep -1711462110.196372 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02d8b0, 110443d:bb7f10a5:18901533:1403) -1711462110.196088 [0] recv: INFOTS(1711462110.195845630) -1711462110.196392 [0] recvMC: HEARTBEAT(F#27:19..19 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@19(sync)) -1711462110.196450 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #17 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.197083 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.197100 [0] recv: INFOTS(1711462110.196935375) -1711462110.197124 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #10 L(:1c1 16957.856601)) -1711462110.197156 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #10: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1104}} -1711462110.197168 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1104 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:1104) => EVERYONE -1711462110.197186 [0] dq.builtin: - deleting -1711462110.197197 [0] dq.builtin: delete -1711462110.197230 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.197250 [0] recv: INFOTS(1711462110.197064444) -1711462110.197272 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #19 L(:1c1 16957.856749)) -1711462110.197294 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #19: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:603}} -1711462110.197305 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:603 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:603) - deleting -1711462110.197313 [0] dq.builtin: => EVERYONE -1711462110.197338 [0] dq.builtin: delete -1711462110.197464 [0] gc: gc 0x653a1b38e4e0: deleting -1711462110.197474 [0] gc: gc_delete_writer(0x653a1b38e4e0, 110aba1:7b19badd:ce0adb73:1403) -1711462110.197501 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #20: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1403}} -1711462110.197513 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 120 from udp/10.101.12.71:44991 -1711462110.197532 [0] recvMC: INFOTS(1711462110.197216804) -1711462110.197516 [0] gc: non-timed queue now has 1 items -1711462110.197554 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #20 L(:1c1 16957.857034) => EVERYONE -1711462110.197581 [0] tev: xpack_addmsg 0x770414001390 0x770424003150 0(data(110aba1:7b19badd:ce0adb73:3c2:#20/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.197603 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{}}) -1711462110.197589 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1403 @ 0x653a1b458d64) user 11 builtin 12 -1711462110.197746 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.197757 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.197628 [0] recvMC: HEARTBEAT(F#28:20..20 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@20(sync)) -1711462110.197791 [0] recv: INFOTS(1711462110.197489510) -1711462110.197759 [0] gc: gc_delete_proxy_writer (0x77041c02e1f0, 110e21c:d0762c48:a9f0fb28:1203) -1711462110.197822 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=28 -1711462110.197809 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #20 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.197849 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #21: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.197691 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424003248:48 0x770424003334:28 [ udp/239.255.0.1:7400@2 ] -1711462110.197835 [0] gc: gc 0x77041c02d8b0: deleting -1711462110.197898 [0] tev: traffic-xmit (1) 96 -1711462110.197900 [0] gc: gc_delete_proxy_writer (0x77041c02d8b0, 110443d:bb7f10a5:18901533:1403) -1711462110.197874 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#21/1)): niov 0 sz 0 => now niov 3 sz 208 -1711462110.197910 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=15 -1711462110.197923 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 21 maxseq 13) -1711462110.197938 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0787794 s (min-ack 13, avail-seq 21, xmit 20) -1711462110.197941 [0] gc: gc 0x77041c014c20: deleting -1711462110.197947 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 208 => now niov 4 sz 240 -1711462110.197950 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:1104) -1711462110.197965 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=27 -1711462110.197972 [0] gc: gc 0x77041c02ab50: deleting -1711462110.197977 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02ab50, 110443d:bb7f10a5:18901533:603) -1711462110.197984 [0] gc: gc 0x653a1b314040: deleting -1711462110.197989 [0] gc: gc 0x77041c02e1f0: not yet, shortsleep -1711462110.197993 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02ab50, 110443d:bb7f10a5:18901533:603) -1711462110.198017 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 240 from udp/10.101.12.71:52025 -1711462110.198025 [0] python3: nn_xpack_send 240: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b395ad0:152 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.198043 [0] python3: traffic-xmit (1) 240 -1711462110.198026 [0] recvMC: INFOTS(1711462110.197789884) -1711462110.198057 [0] python3: => EVERYONE -1711462110.198074 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #21 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.198082 [0] recvMC: HEARTBEAT(F#31:21..21 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.198085 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:1704) ... -1711462110.198094 [0] python3: => EVERYONE -1711462110.198536 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.198552 [0] recv: INFOTS(1711462110.198317479) -1711462110.198574 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #12 L(:1c1 16957.858053)) -1711462110.198625 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #12: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1003}} -1711462110.198652 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1003 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:1003) - deleting -1711462110.198666 [0] dq.builtin: => EVERYONE -1711462110.198698 [0] dq.builtin: delete -1711462110.198787 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 412 from udp/10.101.12.71:58212 -1711462110.198797 [0] recvMC: INFOTS(1711462110.198438587) -1711462110.198808 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #14 L(:1c1 16957.858299) => EVERYONE -1711462110.198900 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.198917 [0] recvMC: HEARTBEAT(F#21:14..14 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@14(sync)) -1711462110.199057 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.199064 [0] gc: gc 0x77041c02d8b0: deleting -1711462110.199071 [0] gc: gc 0x77041c014c20: deleting -1711462110.199076 [0] gc: gc 0x77041c02ab50: deleting -1711462110.199081 [0] gc: gc_delete_proxy_writer (0x77041c02ab50, 110443d:bb7f10a5:18901533:603) -1711462110.199086 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=14 -1711462110.199094 [0] gc: gc 0x653a1b39cbc0: deleting -1711462110.199098 [0] gc: gc_delete_reader(0x653a1b39cbc0, 110aba1:7b19badd:ce0adb73:1704) -1711462110.199112 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #18: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1704}} -1711462110.199121 [0] gc: non-timed queue now has 1 items -1711462110.199133 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1704 @ 0x653a1b39c054) user 10 builtin 12 -1711462110.199139 [0] gc: gc 0x77041c0313c0: deleting -1711462110.199144 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c0313c0, 110e21c:d0762c48:a9f0fb28:1003) -1711462110.199148 [0] tev: xpack_addmsg 0x770414001390 0x770424003360 0(data(110aba1:7b19badd:ce0adb73:4c2:#18/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.199150 [0] gc: gc 0x653a1b314040: deleting -1711462110.199174 [0] gc: gc 0x653a1b39cbc0: not yet, shortsleep -1711462110.199160 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c0313c0, 110e21c:d0762c48:a9f0fb28:1003) -1711462110.199157 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:1603) ... -1711462110.199201 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1603) state transition 0 -> 1 -1711462110.199207 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1603) ... -1711462110.199212 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1603) - no unack'ed samples -1711462110.199217 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:1603) ... -1711462110.199224 [0] python3: => EVERYONE -1711462110.199237 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1603) state transition 1 -> 3 -1711462110.199249 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.199257 [0] recv: INFOTS(1711462110.199105790) -1711462110.199282 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #18 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.199265 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424003458:48 0x770424003544:28 [ udp/239.255.0.1:7400@2 ] -1711462110.199310 [0] tev: traffic-xmit (1) 96 -1711462110.199982 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.199997 [0] recv: INFOTS(1711462110.199842623) -1711462110.200010 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #11 L(:1c1 16957.859498)) -1711462110.200043 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:f04}} -1711462110.200072 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:f04 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:f04) => EVERYONE -1711462110.200097 [0] dq.builtin: - deleting -1711462110.200110 [0] dq.builtin: delete -1711462110.200238 [0] gc: gc 0x653a1b39cbc0: deleting -1711462110.200246 [0] gc: gc 0x77041c0313c0: deleting -1711462110.200251 [0] gc: gc_delete_proxy_writer (0x77041c0313c0, 110e21c:d0762c48:a9f0fb28:1003) -1711462110.200256 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=26 -1711462110.200267 [0] gc: gc 0x653a1b38d9d0: deleting -1711462110.200272 [0] gc: gc_delete_writer(0x653a1b38d9d0, 110aba1:7b19badd:ce0adb73:1603) -1711462110.200288 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #21: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1603}} -1711462110.200307 [0] gc: non-timed queue now has 1 items -1711462110.200322 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1603 @ 0x653a1b39b554) user 9 builtin 12 -1711462110.200331 [0] tev: xpack_addmsg 0x770414001390 0x770424003570 0(data(110aba1:7b19badd:ce0adb73:3c2:#21/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.200350 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.200333 [0] gc: gc 0x77041c014c20: deleting -1711462110.200371 [0] recv: INFOTS(1711462110.200208970) -1711462110.200399 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424003668:48 0x770424003754:28 [ udp/239.255.0.1:7400@2 ] -1711462110.200424 [0] tev: traffic-xmit (1) 96 -1711462110.200405 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:f04) -1711462110.200452 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #22: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.200471 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=25 -1711462110.200966 [0] gc: gc 0x653a1b314040: deleting -1711462110.200403 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #13 L(:1c1 16957.859870)) -1711462110.201141 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 364 from udp/10.101.12.71:58212 -1711462110.201152 [0] recvMC: INFOTS(1711462110.200343585) -1711462110.201165 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.201174 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:e03}} -1711462110.201028 [0] gc: gc 0x653a1b38d9d0: deleting -1711462110.201204 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:e03 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:e03) - deleting -1711462110.201224 [0] dq.builtin: => EVERYONE -1711462110.201179 [0] recv: INFOTS(1711462110.200281996) -1711462110.201044 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#22/1)): niov 0 sz 0 => now niov 3 sz 160 -1711462110.201255 [0] dq.builtin: delete -1711462110.201321 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 22 maxseq 13) -1711462110.201177 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #15 L(:1c1 16957.860654) => EVERYONE -1711462110.201341 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0761866 s (min-ack 13, avail-seq 22, xmit 21) -1711462110.201339 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #21 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.201364 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 160 => now niov 4 sz 192 -1711462110.201456 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.201504 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.201511 [0] python3: nn_xpack_send 192: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b399fe0:104 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.201547 [0] recvMC: HEARTBEAT(F#22:15..15 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@15(sync)) -1711462110.201564 [0] python3: traffic-xmit (1) 192 -1711462110.201586 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 192 from udp/10.101.12.71:52025 -1711462110.201587 [0] python3: => EVERYONE -1711462110.201597 [0] recvMC: INFOTS(1711462110.200384071) -1711462110.201628 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #22 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.201645 [0] recvMC: HEARTBEAT(F#32:22..22 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.201633 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:1904) ... -1711462110.201681 [0] python3: => EVERYONE -1711462110.201866 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.201941 [0] recv: INFOTS(1711462110.201520038) -1711462110.201969 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #12 L(:1c1 16957.861437)) -1711462110.202045 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:d04}} -1711462110.202064 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:d04 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:d04) => EVERYONE -1711462110.202082 [0] dq.builtin: - deleting -1711462110.202089 [0] dq.builtin: delete -1711462110.202550 [0] gc: gc 0x77041c014c20: deleting -1711462110.202561 [0] gc: gc 0x77041c031bc0: deleting -1711462110.202567 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c031bc0, 110e21c:d0762c48:a9f0fb28:e03) -1711462110.202575 [0] gc: gc 0x653a1b39cbc0: deleting -1711462110.202582 [0] gc: gc_delete_reader(0x653a1b39cbc0, 110aba1:7b19badd:ce0adb73:1904) -1711462110.202607 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #19: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1904}} -1711462110.202604 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c031bc0, 110e21c:d0762c48:a9f0fb28:e03) -1711462110.202622 [0] gc: non-timed queue now has 1 items -1711462110.202657 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1904 @ 0x653a1b3a0714) user 8 builtin 12 -1711462110.202664 [0] tev: xpack_addmsg 0x770414001390 0x770424003780 0(data(110aba1:7b19badd:ce0adb73:4c2:#19/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.202667 [0] gc: gc 0x77041c02a1c0: deleting -1711462110.202694 [0] gc: gc_delete_proxy_reader (0x77041c02a1c0, 110e21c:d0762c48:a9f0fb28:d04) -1711462110.202701 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:1803) ... -1711462110.202704 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=24 -1711462110.202716 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1803) state transition 0 -> 1 -1711462110.202730 [0] gc: gc 0x77041c031bc0: deleting -1711462110.202740 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1803) ... -1711462110.202753 [0] gc: gc_delete_proxy_writer (0x77041c031bc0, 110e21c:d0762c48:a9f0fb28:e03) -1711462110.202741 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424003878:48 0x770424003964:28 [ udp/239.255.0.1:7400@2 ] -1711462110.202782 [0] tev: traffic-xmit (1) 96 -1711462110.202769 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:1803) - no unack'ed samples -1711462110.202789 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=23 -1711462110.202755 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.202820 [0] recv: INFOTS(1711462110.202594273) -1711462110.202806 [0] gc: gc 0x653a1b314040: deleting -1711462110.202838 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #19 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.202863 [0] gc: gc 0x77041c02a1c0: deleting -1711462110.202868 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:1803) ... -1711462110.202884 [0] gc: gc 0x77041c031bc0: not yet, shortsleep -1711462110.202903 [0] python3: => EVERYONE -1711462110.202922 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:1803) state transition 1 -> 3 -1711462110.203048 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.203066 [0] recv: INFOTS(1711462110.202893761) -1711462110.203100 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #14 L(:1c1 16957.862565)) -1711462110.203127 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:c03}} -1711462110.203144 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:c03 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:c03) - deleting -1711462110.203154 [0] dq.builtin: => EVERYONE -1711462110.203172 [0] dq.builtin: delete -1711462110.203223 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 316 from udp/10.101.12.71:58212 -1711462110.203232 [0] recvMC: INFOTS(1711462110.202997343) -1711462110.203261 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #16 L(:1c1 16957.862734) => EVERYONE -1711462110.203328 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.203339 [0] recvMC: HEARTBEAT(F#23:16..16 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@16(sync)) -1711462110.203979 [0] gc: gc 0x77041c031bc0: deleting -1711462110.203993 [0] gc: gc 0x653a1b3a1360: deleting -1711462110.203998 [0] gc: gc_delete_writer(0x653a1b3a1360, 110aba1:7b19badd:ce0adb73:1803) -1711462110.204022 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #22: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1803}} -1711462110.204040 [0] gc: non-timed queue now has 1 items -1711462110.204058 [0] tev: xpack_addmsg 0x770414001390 0x770424003990 0(data(110aba1:7b19badd:ce0adb73:3c2:#22/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.204059 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:1803 @ 0x653a1b39f924) user 7 builtin 12 -1711462110.204103 [0] gc: gc 0x77041c014c20: deleting -1711462110.204112 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:c03) -1711462110.204128 [0] gc: gc 0x653a1b314040: not yet, shortsleep -1711462110.204141 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #23: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{},{}}}} -1711462110.204131 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424003a88:48 0x770424003b74:28 [ udp/239.255.0.1:7400@2 ] -1711462110.204174 [0] tev: traffic-xmit (1) 96 -1711462110.204163 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#23/1)): niov 0 sz 0 => now niov 3 sz 136 -1711462110.204152 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:c03) -1711462110.204233 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 23 maxseq 13) -1711462110.204278 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.072478 s (min-ack 13, avail-seq 23, xmit 22) -1711462110.204294 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 136 => now niov 4 sz 168 -1711462110.204136 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.204338 [0] recv: INFOTS(1711462110.204013066) -1711462110.204349 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #22 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.204368 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 168 from udp/10.101.12.71:52025 -1711462110.204375 [0] recvMC: INFOTS(1711462110.204092708) -1711462110.204398 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #23 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.204414 [0] python3: nn_xpack_send 168: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b44d700:80 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.204460 [0] recvMC: HEARTBEAT(F#33:23..23 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.204426 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.204507 [0] recv: INFOTS(1711462110.204117111) -1711462110.204462 [0] python3: traffic-xmit (1) 168 -1711462110.204533 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #13 L(:1c1 16957.863997)) -1711462110.204566 [0] python3: => EVERYONE -1711462110.204582 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:603) ... -1711462110.204592 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:603) state transition 0 -> 1 -1711462110.204582 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:b04}} -1711462110.204600 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:603) ... -1711462110.204630 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:b04 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:b04) => EVERYONE -1711462110.204656 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:603) - no unack'ed samples -1711462110.204663 [0] python3: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:603) ... -1711462110.204671 [0] dq.builtin: - deleting -1711462110.204672 [0] python3: => EVERYONE -1711462110.204687 [0] dq.builtin: delete -1711462110.204709 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:603) state transition 1 -> 3 -1711462110.205221 [0] gc: gc 0x653a1b314040: deleting -1711462110.205239 [0] gc: gc 0x77041c014c20: deleting -1711462110.205246 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:c03) -1711462110.205253 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=22 -1711462110.205262 [0] gc: gc 0x77041c030430: deleting -1711462110.205268 [0] gc: gc_delete_proxy_reader (0x77041c030430, 110e21c:d0762c48:a9f0fb28:b04) -1711462110.205273 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=21 -1711462110.205280 [0] gc: gc 0x653a1b39cbc0: deleting -1711462110.205285 [0] gc: gc_delete_writer(0x653a1b39cbc0, 110aba1:7b19badd:ce0adb73:603) -1711462110.205304 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #23: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:603}} -1711462110.205315 [0] gc: non-timed queue now has 1 items -1711462110.205333 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(data(110aba1:7b19badd:ce0adb73:3c2:#23/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.205341 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:603 @ 0x653a1b2cb464) user 6 builtin 12 -1711462110.205365 [0] gc: gc 0x653a1b314040: deleting -1711462110.205372 [0] gc: gc 0x77041c030430: deleting -1711462110.205384 [0] gc: gc 0x653a1b39cbc0: not yet, shortsleep -1711462110.205406 [0] python3: write_sample 110aba1:7b19badd:ce0adb73:403 #24: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{}} -1711462110.205425 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.205408 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424003c98:48 0x770424003d84:28 [ udp/239.255.0.1:7400@2 ] -1711462110.205429 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae6110 0(data(110aba1:7b19badd:ce0adb73:403:#24/1)): niov 0 sz 0 => now niov 3 sz 88 -1711462110.205448 [0] tev: traffic-xmit (1) 96 -1711462110.205474 [0] recv: INFOTS(1711462110.205296205) -1711462110.205496 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #23 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.205502 [0] python3: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 24 maxseq 13) -1711462110.205519 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.205547 [0] recv: INFOTS(1711462110.205333502) -1711462110.205537 [0] python3: heartbeat(wr 110aba1:7b19badd:ce0adb73:403 final) piggybacked, resched in 0.0712054 s (min-ack 13, avail-seq 24, xmit 23) -1711462110.205656 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #15 L(:1c1 16957.865049)) -1711462110.205665 [0] python3: xpack_addmsg 0x653a1add92c0 0x653a1aae61e0 0(control): niov 3 sz 88 => now niov 4 sz 120 -1711462110.205705 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:a03}} -1711462110.205735 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:a03 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:a03) - deleting -1711462110.205747 [0] dq.builtin: => EVERYONE -1711462110.205748 [0] python3: nn_xpack_send 120: 0x653a1add92cc:20 0x653a1b3a4ae8:36 0x653a1b479c30:32 0x653a1b3a47f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.205790 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 120 from udp/10.101.12.71:52025 -1711462110.205802 [0] python3: traffic-xmit (1) 120 -1711462110.205827 [0] recvMC: INFOTS(1711462110.205378348) -1711462110.205851 [0] python3: => EVERYONE -1711462110.205833 [0] dq.builtin: delete -1711462110.205939 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #24 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.205954 [0] recvMC: HEARTBEAT(F#34:24..24 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.206029 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 268 from udp/10.101.12.71:58212 -1711462110.206040 [0] recvMC: INFOTS(1711462110.205461976) -1711462110.206053 [0] python3: ddsi_unblock_throttled_writer(guid 110aba1:7b19badd:ce0adb73:403) ... -1711462110.206059 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #17 L(:1c1 16957.865542) => EVERYONE -1711462110.206077 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:403) state transition 0 -> 1 -1711462110.206095 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:403) ... -1711462110.206108 [0] python3: writer_set_state(110aba1:7b19badd:ce0adb73:403) state transition 1 -> 2 -1711462110.206127 [0] python3: delete_writer(guid 110aba1:7b19badd:ce0adb73:403) - unack'ed samples, will delete when ack'd or at t = 16948.865625 -1711462110.206153 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.206172 [0] recvMC: HEARTBEAT(F#24:17..17 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@17(sync)) -1711462110.206454 [0] gc: gc 0x653a1b39cbc0: deleting -1711462110.206464 [0] gc: gc 0x77041c014c20: deleting -1711462110.206470 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:a03) -1711462110.206508 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:a03) -1711462110.206531 [0] gc: gc 0x77041c014c20: deleting -1711462110.206537 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:a03) -1711462110.206544 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=20 -1711462110.206553 [0] gc: gc 0x653a1b314040: deleting -1711462110.206789 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.206801 [0] recv: INFOTS(1711462110.206657137) -1711462110.206819 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #14 L(:1c1 16957.866303)) -1711462110.206862 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:904}} -1711462110.206891 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:904 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:904) => EVERYONE -1711462110.206935 [0] dq.builtin: - deleting -1711462110.206951 [0] dq.builtin: delete -1711462110.206956 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.208029 [0] gc: gc 0x77041c014c20: deleting -1711462110.208048 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:904) -1711462110.208062 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=19 -1711462110.208208 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.208222 [0] recv: INFOTS(1711462110.208064226) -1711462110.208238 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #16 L(:1c1 16957.867724)) -1711462110.208294 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:803}} -1711462110.208311 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:803 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:803) - deleting -1711462110.208324 [0] dq.builtin: => EVERYONE -1711462110.208355 [0] dq.builtin: delete -1711462110.208364 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.208442 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 244 from udp/10.101.12.71:58212 -1711462110.208457 [0] recvMC: INFOTS(1711462110.208197991) -1711462110.208477 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #18 L(:1c1 16957.867959) => EVERYONE -1711462110.208541 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.208555 [0] recvMC: HEARTBEAT(F#25:18..18 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@18(sync)) -1711462110.209413 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.209425 [0] recv: INFOTS(1711462110.209265145) -1711462110.209438 [0] gc: gc 0x77041c014c20: deleting -1711462110.209459 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:803) -1711462110.209449 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #15 L(:1c1 16957.868927)) -1711462110.209487 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:803) -1711462110.209512 [0] gc: gc 0x77041c014c20: deleting -1711462110.209537 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:803) -1711462110.209513 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #15: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1504}} -1711462110.209557 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=18 -1711462110.209588 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1504 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:1504) => EVERYONE -1711462110.209606 [0] dq.builtin: - deleting -1711462110.209621 [0] dq.builtin: delete -1711462110.209627 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.209657 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 220 from udp/10.101.12.71:58212 -1711462110.209684 [0] recvMC: INFOTS(1711462110.209425688) -1711462110.209730 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #19 L(:1c1 16957.869184) => EVERYONE -1711462110.209857 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.209880 [0] recvMC: HEARTBEAT(F#26:19..19 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@19(sync)) -1711462110.210621 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.210639 [0] recv: INFOTS(1711462110.210493781) -1711462110.210657 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #17 L(:1c1 16957.870141)) -1711462110.210707 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:703}} -1711462110.210708 [0] gc: gc 0x77041c014c20: deleting -1711462110.210755 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:1504) -1711462110.210768 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=17 -1711462110.210736 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:703 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:703) - deleting -1711462110.210812 [0] dq.builtin: => EVERYONE -1711462110.210833 [0] dq.builtin: delete -1711462110.210841 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.210874 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 196 from udp/10.101.12.71:58212 -1711462110.210918 [0] recvMC: INFOTS(1711462110.210679198) -1711462110.210961 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #20 L(:1c1 16957.870418) => EVERYONE -1711462110.210994 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.211010 [0] recvMC: HEARTBEAT(F#27:20..20 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@20(sync)) -1711462110.211805 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.211825 [0] recv: INFOTS(1711462110.211688403) -1711462110.211841 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #18 L(:1c1 16957.871326)) -1711462110.211895 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1403}} -1711462110.211914 [0] gc: gc 0x77041c014c20: deleting -1711462110.211933 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:703) -1711462110.211916 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1403 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:1403) - deleting -1711462110.211959 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:703) -1711462110.211964 [0] dq.builtin: => EVERYONE -1711462110.211984 [0] gc: gc 0x77041c014c20: deleting -1711462110.212009 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:703) -1711462110.212026 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=16 -1711462110.211995 [0] dq.builtin: delete -1711462110.212043 [0] gc: gc 0x77041c02cf40: not yet, shortsleep -1711462110.211997 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 172 from udp/10.101.12.71:58212 -1711462110.212083 [0] recvMC: INFOTS(1711462110.211821961) -1711462110.212107 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #21 L(:1c1 16957.871582) => EVERYONE -1711462110.212144 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #21: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{},{}}}}) -1711462110.212161 [0] recvMC: HEARTBEAT(F#28:21..21 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@21(sync)) -1711462110.213041 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.213054 [0] recv: INFOTS(1711462110.212917857) -1711462110.213074 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #19 L(:1c1 16957.872556)) -1711462110.213127 [0] gc: gc 0x77041c02cf40: deleting -1711462110.213140 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #19: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:603}} -1711462110.213145 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02cf40, 110e21c:d0762c48:a9f0fb28:1403) -1711462110.213171 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:603 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:603) - deleting -1711462110.213195 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02cf40, 110e21c:d0762c48:a9f0fb28:1403) -1711462110.213222 [0] dq.builtin: => EVERYONE -1711462110.213203 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 120 from udp/10.101.12.71:58212 -1711462110.213250 [0] gc: gc 0x77041c02cf40: deleting -1711462110.213270 [0] recvMC: INFOTS(1711462110.213044269) -1711462110.213258 [0] dq.builtin: delete -1711462110.213289 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #22 L(:1c1 16957.872771) => EVERYONE -1711462110.213331 [0] gc: gc_delete_proxy_writer (0x77041c02cf40, 110e21c:d0762c48:a9f0fb28:1403) -1711462110.213343 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=15 -1711462110.213370 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #22: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{}}) -1711462110.213410 [0] gc: gc 0x77041c014c20: deleting -1711462110.213420 [0] recvMC: HEARTBEAT(F#29:22..22 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@22(sync)) -1711462110.213422 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:603) -1711462110.213446 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2096 from udp/10.101.12.71:40473 -1711462110.213456 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110e21c:d0762c48:a9f0fb28:603) -1711462110.213461 [0] recvMC: INFOTS(1711462110.213069639) -1711462110.213488 [0] gc: gc 0x77041c014c20: deleting -1711462110.213505 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #46 L(:1c1 16957.872963) => EVERYONE -1711462110.213511 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110e21c:d0762c48:a9f0fb28:603) -1711462110.213533 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=14 -1711462110.213676 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #46: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462110.213719 [0] recvMC: HEARTBEAT(#55:46..46 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@46(sync)) -1711462110.213789 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#6:47/0: -1711462110.213803 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.213814 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.213846 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003c88:44 [ udp/10.101.12.71:7415@2 ] -1711462110.213854 [0] tev: traffic-xmit (1) 64 -1711462110.214569 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.214581 [0] recv: INFOTS(1711462110.214373670) -1711462110.214597 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #41 L(:1c1 16957.874082)) -1711462110.214631 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #41: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2704}} -1711462110.214658 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2704 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2704) => EVERYONE -1711462110.214681 [0] dq.builtin: - deleting -1711462110.214698 [0] dq.builtin: delete -1711462110.214710 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.215807 [0] gc: gc 0x77041c014c20: deleting -1711462110.215835 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2704) -1711462110.215844 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=96 -1711462110.215904 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.215916 [0] recv: INFOTS(1711462110.215754782) -1711462110.215942 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #46 L(:1c1 16957.875418)) -1711462110.215987 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #46: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2603}} -1711462110.216014 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2603) - deleting -1711462110.216031 [0] dq.builtin: => EVERYONE -1711462110.216054 [0] dq.builtin: delete -1711462110.216069 [0] gc: gc 0x77041c014c20: deleting -1711462110.216081 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2603) -1711462110.216098 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2603) -1711462110.216115 [0] gc: gc 0x77041c014c20: deleting -1711462110.216126 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2603) -1711462110.216125 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2048 from udp/10.101.12.71:40473 -1711462110.216144 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=95 -1711462110.216164 [0] recvMC: INFOTS(1711462110.215865725) -1711462110.216195 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #47 L(:1c1 16957.875665) => EVERYONE -1711462110.216349 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #47: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462110.216368 [0] recvMC: HEARTBEAT(F#56:47..47 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@47(sync)) -1711462110.217103 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.217120 [0] recv: INFOTS(1711462110.216976537) -1711462110.217143 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #42 L(:1c1 16957.876622)) -1711462110.217206 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #42: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1304}} -1711462110.217255 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1304) => EVERYONE -1711462110.217274 [0] dq.builtin: - deleting -1711462110.217290 [0] dq.builtin: delete -1711462110.217300 [0] gc: gc 0x77041c014c20: deleting -1711462110.217327 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1304) -1711462110.217340 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=94 -1711462110.218585 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.218601 [0] recv: INFOTS(1711462110.218401795) -1711462110.218623 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #47 L(:1c1 16957.878102)) -1711462110.218663 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #47: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1203}} -1711462110.218683 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1203) - deleting -1711462110.218691 [0] dq.builtin: => EVERYONE -1711462110.218710 [0] dq.builtin: delete -1711462110.218723 [0] gc: gc 0x77041c014c20: deleting -1711462110.218736 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1203) -1711462110.218760 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1203) -1711462110.218788 [0] gc: gc 0x77041c014c20: deleting -1711462110.218802 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1203) -1711462110.218815 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=93 -1711462110.218951 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2000 from udp/10.101.12.71:40473 -1711462110.218967 [0] recvMC: INFOTS(1711462110.218561783) -1711462110.218990 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #48 L(:1c1 16957.878468) => EVERYONE -1711462110.219166 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #48: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462110.219182 [0] recvMC: HEARTBEAT(F#57:48..48 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@48(sync)) -1711462110.219813 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.219841 [0] recv: INFOTS(1711462110.219653728) -1711462110.219868 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #43 L(:1c1 16957.879343)) -1711462110.219912 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #43: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1104}} -1711462110.219925 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1104) => EVERYONE -1711462110.219950 [0] dq.builtin: - deleting -1711462110.219976 [0] dq.builtin: delete -1711462110.219992 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.221066 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.221088 [0] recv: INFOTS(1711462110.220906100) -1711462110.221112 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #48 L(:1c1 16957.880589)) -1711462110.221189 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #48: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1003}} -1711462110.221215 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1003) - deleting -1711462110.221231 [0] dq.builtin: => EVERYONE -1711462110.221270 [0] dq.builtin: delete -1711462110.221272 [0] gc: gc 0x77041c014c20: deleting -1711462110.221302 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1104) -1711462110.221313 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=92 -1711462110.221327 [0] gc: gc 0x77041c026db0: deleting -1711462110.221335 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c026db0, 110d7b4:17c5b8c5:94bd0ff4:1003) -1711462110.221359 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c026db0, 110d7b4:17c5b8c5:94bd0ff4:1003) -1711462110.221389 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1952 from udp/10.101.12.71:40473 -1711462110.221408 [0] recvMC: INFOTS(1711462110.221033854) -1711462110.221394 [0] gc: gc 0x77041c026db0: deleting -1711462110.221435 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #49 L(:1c1 16957.880909) => EVERYONE -1711462110.221460 [0] gc: gc_delete_proxy_writer (0x77041c026db0, 110d7b4:17c5b8c5:94bd0ff4:1003) -1711462110.221479 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=91 -1711462110.221620 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #49: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,4) -1711462110.221634 [0] recvMC: HEARTBEAT(F#58:49..49 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@49(sync)) -1711462110.222317 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.222341 [0] recv: INFOTS(1711462110.222150811) -1711462110.222365 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #44 L(:1c1 16957.881842)) -1711462110.222410 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #44: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:f04}} -1711462110.222441 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:f04) => EVERYONE -1711462110.222471 [0] dq.builtin: - deleting -1711462110.222512 [0] dq.builtin: delete -1711462110.222525 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.223608 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.223625 [0] recv: INFOTS(1711462110.223428697) -1711462110.223634 [0] gc: gc 0x77041c014c20: deleting -1711462110.223667 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:f04) -1711462110.223689 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=90 -1711462110.223645 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #49 L(:1c1 16957.883126)) -1711462110.223751 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #49: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:e03}} -1711462110.223775 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:e03) - deleting -1711462110.223790 [0] dq.builtin: => EVERYONE -1711462110.223815 [0] dq.builtin: delete -1711462110.223830 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.223902 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1904 from udp/10.101.12.71:40473 -1711462110.223947 [0] recvMC: INFOTS(1711462110.223556659) -1711462110.223967 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #50 L(:1c1 16957.883449) => EVERYONE -1711462110.224136 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #50: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,4) -1711462110.224149 [0] recvMC: HEARTBEAT(F#59:50..50 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@50(sync)) -1711462110.224808 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.224840 [0] recv: INFOTS(1711462110.224684003) -1711462110.224864 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #45 L(:1c1 16957.884336)) -1711462110.224923 [0] gc: gc 0x77041c014c20: deleting -1711462110.224945 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:e03) -1711462110.224929 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #45: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:d04}} -1711462110.224979 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:e03) -1711462110.225010 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:d04) => EVERYONE -1711462110.225060 [0] dq.builtin: - deleting -1711462110.225066 [0] gc: gc 0x77041c014c20: deleting -1711462110.225075 [0] dq.builtin: delete -1711462110.225097 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:e03) -1711462110.225137 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=89 -1711462110.225164 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.225175 [0] gc: gc_delete_proxy_reader (0x77041c02e1f0, 110d7b4:17c5b8c5:94bd0ff4:d04) -1711462110.225187 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=88 -1711462110.226096 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.226119 [0] recv: INFOTS(1711462110.225949014) -1711462110.226142 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #50 L(:1c1 16957.885620)) -1711462110.226194 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #50: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:c03}} -1711462110.226214 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:c03) - deleting -1711462110.226231 [0] dq.builtin: => EVERYONE -1711462110.226266 [0] dq.builtin: delete -1711462110.226281 [0] gc: gc 0x77041c014c20: deleting -1711462110.226301 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:c03) -1711462110.226329 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:c03) -1711462110.226353 [0] gc: gc 0x77041c014c20: deleting -1711462110.226363 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:c03) -1711462110.226373 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=87 -1711462110.226426 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1856 from udp/10.101.12.71:40473 -1711462110.226462 [0] recvMC: INFOTS(1711462110.226054803) -1711462110.226491 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #51 L(:1c1 16957.885961) => EVERYONE -1711462110.226719 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #51: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,4) -1711462110.226745 [0] recvMC: HEARTBEAT(F#60:51..51 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@51(sync)) -1711462110.227322 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.227359 [0] recv: INFOTS(1711462110.227176717) -1711462110.227382 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #46 L(:1c1 16957.886860)) -1711462110.227442 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #46: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:b04}} -1711462110.227480 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:b04) => EVERYONE -1711462110.227500 [0] dq.builtin: - deleting -1711462110.227519 [0] dq.builtin: delete -1711462110.227529 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.228599 [0] gc: gc 0x77041c014c20: deleting -1711462110.228617 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:b04) -1711462110.228625 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=86 -1711462110.228814 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.228841 [0] recv: INFOTS(1711462110.228522592) -1711462110.228954 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #51 L(:1c1 16957.888328)) -1711462110.229017 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #51: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:a03}} -1711462110.229037 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:a03) - deleting -1711462110.229049 [0] dq.builtin: => EVERYONE -1711462110.229077 [0] dq.builtin: delete -1711462110.229089 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.229249 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1808 from udp/10.101.12.71:40473 -1711462110.229275 [0] recvMC: INFOTS(1711462110.228719825) -1711462110.229301 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #52 L(:1c1 16957.888775) => EVERYONE -1711462110.229531 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #52: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.229553 [0] recvMC: HEARTBEAT(F#61:52..52 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@52(sync)) -1711462110.230029 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.230049 [0] recv: INFOTS(1711462110.229849806) -1711462110.230073 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #47 L(:1c1 16957.889550)) -1711462110.230150 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #47: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:904}} -1711462110.230172 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:904) => EVERYONE -1711462110.230183 [0] dq.builtin: - deleting -1711462110.230191 [0] dq.builtin: delete -1711462110.230209 [0] gc: gc 0x77041c014c20: deleting -1711462110.230218 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:a03) -1711462110.230227 [0] gc: gc 0x77041c02d8b0: deleting -1711462110.230234 [0] gc: gc_delete_proxy_reader (0x77041c02d8b0, 110d7b4:17c5b8c5:94bd0ff4:904) -1711462110.230245 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=85 -1711462110.230237 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:a03) -1711462110.230281 [0] gc: gc 0x77041c014c20: deleting -1711462110.230294 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:a03) -1711462110.230309 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=84 -1711462110.231138 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.231150 [0] recv: INFOTS(1711462110.231018837) -1711462110.231164 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #52 L(:1c1 16957.890652)) -1711462110.231207 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #52: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:803}} -1711462110.231228 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:803) - deleting -1711462110.231235 [0] dq.builtin: => EVERYONE -1711462110.231247 [0] dq.builtin: delete -1711462110.231253 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.231423 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1784 from udp/10.101.12.71:40473 -1711462110.231444 [0] recvMC: INFOTS(1711462110.231152392) -1711462110.231463 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #53 L(:1c1 16957.890945) => EVERYONE -1711462110.231634 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #53: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.231651 [0] recvMC: HEARTBEAT(F#62:53..53 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@53(sync)) -1711462110.232332 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.232357 [0] recv: INFOTS(1711462110.232191115) -1711462110.232354 [0] gc: gc 0x77041c014c20: deleting -1711462110.232387 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:803) -1711462110.232375 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #53 L(:1c1 16957.891858)) -1711462110.232442 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:803) -1711462110.232445 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #53: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:703}} -1711462110.232486 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:703 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:703) - deleting -1711462110.232520 [0] dq.builtin: => EVERYONE -1711462110.232576 [0] dq.builtin: delete -1711462110.232486 [0] gc: gc 0x77041c014c20: deleting -1711462110.232600 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:803) -1711462110.232614 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=83 -1711462110.232631 [0] gc: gc 0x77041c02ab50: deleting -1711462110.232641 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02ab50, 110d7b4:17c5b8c5:94bd0ff4:703) -1711462110.232663 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02ab50, 110d7b4:17c5b8c5:94bd0ff4:703) -1711462110.232685 [0] gc: gc 0x77041c02ab50: deleting -1711462110.232695 [0] gc: gc_delete_proxy_writer (0x77041c02ab50, 110d7b4:17c5b8c5:94bd0ff4:703) -1711462110.232704 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=82 -1711462110.232724 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1736 from udp/10.101.12.71:40473 -1711462110.232734 [0] recvMC: INFOTS(1711462110.232345337) -1711462110.232754 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #54 L(:1c1 16957.892236) => EVERYONE -1711462110.232888 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #54: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.232901 [0] recvMC: HEARTBEAT(F#63:54..54 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@54(sync)) -1711462110.233481 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.233503 [0] recv: INFOTS(1711462110.233337855) -1711462110.233517 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #48 L(:1c1 16957.893004)) -1711462110.233572 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #48: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1504}} -1711462110.233602 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1504) => EVERYONE -1711462110.233654 [0] dq.builtin: - deleting -1711462110.233677 [0] dq.builtin: delete -1711462110.233693 [0] gc: gc 0x77041c014c20: deleting -1711462110.233708 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1504) -1711462110.233721 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=81 -1711462110.234715 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.234743 [0] recv: INFOTS(1711462110.234533470) -1711462110.234768 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #54 L(:1c1 16957.894244)) -1711462110.234825 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #54: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1403}} -1711462110.234859 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1403) - deleting -1711462110.234875 [0] dq.builtin: => EVERYONE -1711462110.234918 [0] dq.builtin: delete -1711462110.234935 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.235145 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1688 from udp/10.101.12.71:40473 -1711462110.235156 [0] recvMC: INFOTS(1711462110.234735835) -1711462110.235171 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #55 L(:1c1 16957.894659) => EVERYONE -1711462110.235301 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #55: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.235311 [0] recvMC: HEARTBEAT(F#64:55..55 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@55(sync)) -1711462110.235939 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.235958 [0] recv: INFOTS(1711462110.235766500) -1711462110.235975 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #49 L(:1c1 16957.895460)) -1711462110.236011 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #49: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1704}} -1711462110.236015 [0] gc: gc 0x77041c014c20: deleting -1711462110.236035 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1704 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1704) => EVERYONE -1711462110.236051 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1403) -1711462110.236082 [0] dq.builtin: - deleting -1711462110.236093 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1403) -1711462110.236109 [0] gc: gc 0x77041c0313c0: not yet, shortsleep -1711462110.236101 [0] dq.builtin: delete -1711462110.237146 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.237163 [0] recv: INFOTS(1711462110.236982638) -1711462110.237197 [0] gc: gc 0x77041c0313c0: deleting -1711462110.237213 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #55 L(:1c1 16957.896666)) -1711462110.237263 [0] gc: gc_delete_proxy_reader (0x77041c0313c0, 110d7b4:17c5b8c5:94bd0ff4:1704) -1711462110.237300 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=80 -1711462110.237304 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #55: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1603}} -1711462110.237316 [0] gc: gc 0x77041c014c20: deleting -1711462110.237333 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1603) - deleting -1711462110.237348 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1403) -1711462110.237373 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=79 -1711462110.237421 [0] dq.builtin: => EVERYONE -1711462110.237452 [0] dq.builtin: delete -1711462110.237477 [0] gc: gc 0x77041c014c20: deleting -1711462110.237488 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1603) -1711462110.237502 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1640 from udp/10.101.12.71:40473 -1711462110.237512 [0] recvMC: INFOTS(1711462110.237105267) -1711462110.237521 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1603) -1711462110.237528 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #56 L(:1c1 16957.897014) => EVERYONE -1711462110.237563 [0] gc: gc 0x77041c014c20: deleting -1711462110.237584 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1603) -1711462110.237596 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=78 -1711462110.237678 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #56: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.237689 [0] recvMC: HEARTBEAT(F#65:56..56 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@56(sync)) -1711462110.238382 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.238401 [0] recv: INFOTS(1711462110.238231579) -1711462110.238415 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #50 L(:1c1 16957.897903)) -1711462110.238451 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #50: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1904}} -1711462110.238482 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1904) => EVERYONE -1711462110.238512 [0] dq.builtin: - deleting -1711462110.238530 [0] dq.builtin: delete -1711462110.238542 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.239523 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.239534 [0] recv: INFOTS(1711462110.239384619) -1711462110.239552 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #56 L(:1c1 16957.899036)) -1711462110.239581 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #56: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1803}} -1711462110.239600 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1803) - deleting -1711462110.239609 [0] dq.builtin: => EVERYONE -1711462110.239627 [0] dq.builtin: delete -1711462110.239640 [0] gc: gc 0x77041c014c20: deleting -1711462110.239663 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1904) -1711462110.239675 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=77 -1711462110.239694 [0] gc: gc 0x77041c02a1c0: deleting -1711462110.239701 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c02a1c0, 110d7b4:17c5b8c5:94bd0ff4:1803) -1711462110.239730 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c02a1c0, 110d7b4:17c5b8c5:94bd0ff4:1803) -1711462110.239756 [0] gc: gc 0x77041c02a1c0: deleting -1711462110.239765 [0] gc: gc_delete_proxy_writer (0x77041c02a1c0, 110d7b4:17c5b8c5:94bd0ff4:1803) -1711462110.239773 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=76 -1711462110.239851 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1592 from udp/10.101.12.71:40473 -1711462110.239862 [0] recvMC: INFOTS(1711462110.239529783) -1711462110.239877 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #57 L(:1c1 16957.899365) => EVERYONE -1711462110.239976 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #57: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.239984 [0] recvMC: HEARTBEAT(F#66:57..57 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@57(sync)) -1711462110.240675 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.240694 [0] recv: INFOTS(1711462110.240557023) -1711462110.240709 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #51 L(:1c1 16957.900196)) -1711462110.240745 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #51: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1b04}} -1711462110.240767 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1b04) => EVERYONE -1711462110.240785 [0] dq.builtin: - deleting -1711462110.240799 [0] dq.builtin: delete -1711462110.240807 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.240921 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.240930 [0] recv: INFOTS(1711462110.240780572) -1711462110.240940 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #57 L(:1c1 16957.900433)) -1711462110.240976 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #57: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1a03}} -1711462110.240991 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1a03) - deleting -1711462110.240999 [0] dq.builtin: => EVERYONE -1711462110.241014 [0] dq.builtin: delete -1711462110.241156 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1544 from udp/10.101.12.71:40473 -1711462110.241163 [0] recvMC: INFOTS(1711462110.240869006) -1711462110.241172 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #58 L(:1c1 16957.900666) => EVERYONE -1711462110.241284 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #58: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.241292 [0] recvMC: HEARTBEAT(F#67:58..58 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@58(sync)) -1711462110.241893 [0] gc: gc 0x77041c014c20: deleting -1711462110.241919 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1b04) -1711462110.241932 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=75 -1711462110.241946 [0] gc: gc 0x77041c031bc0: deleting -1711462110.241954 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c031bc0, 110d7b4:17c5b8c5:94bd0ff4:1a03) -1711462110.241982 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c031bc0, 110d7b4:17c5b8c5:94bd0ff4:1a03) -1711462110.242008 [0] gc: gc 0x77041c031bc0: deleting -1711462110.242019 [0] gc: gc_delete_proxy_writer (0x77041c031bc0, 110d7b4:17c5b8c5:94bd0ff4:1a03) -1711462110.242029 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=74 -1711462110.242034 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.242045 [0] recv: INFOTS(1711462110.241943611) -1711462110.242058 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #52 L(:1c1 16957.901547)) -1711462110.242095 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #52: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1d04}} -1711462110.242115 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1d04) => EVERYONE -1711462110.242130 [0] dq.builtin: - deleting -1711462110.242144 [0] dq.builtin: delete -1711462110.242150 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.243203 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.243219 [0] recv: INFOTS(1711462110.243107441) -1711462110.243246 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #58 L(:1c1 16957.902720)) -1711462110.243250 [0] gc: gc 0x77041c014c20: deleting -1711462110.243266 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1d04) -1711462110.243270 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #58: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1c03}} -1711462110.243273 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=73 -1711462110.243287 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1c03) - deleting -1711462110.243311 [0] dq.builtin: => EVERYONE -1711462110.243332 [0] dq.builtin: delete -1711462110.243353 [0] gc: gc 0x77041c014c20: deleting -1711462110.243374 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1c03) -1711462110.243403 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1c03) -1711462110.243432 [0] gc: gc 0x77041c014c20: deleting -1711462110.243442 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1c03) -1711462110.243452 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=72 -1711462110.243498 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1496 from udp/10.101.12.71:40473 -1711462110.243504 [0] recvMC: INFOTS(1711462110.243228417) -1711462110.243513 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #59 L(:1c1 16957.903007) => EVERYONE -1711462110.243626 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #59: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.243633 [0] recvMC: HEARTBEAT(F#68:59..59 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@59(sync)) -1711462110.244401 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.244413 [0] recv: INFOTS(1711462110.244270576) -1711462110.244424 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #53 L(:1c1 16957.903915)) -1711462110.244482 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #53: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1f04}} -1711462110.244527 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1f04) => EVERYONE -1711462110.244568 [0] dq.builtin: - deleting -1711462110.244580 [0] dq.builtin: delete -1711462110.244593 [0] gc: gc 0x77041c014c20: deleting -1711462110.244606 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1f04) -1711462110.244615 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=71 -1711462110.245507 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.245518 [0] recv: INFOTS(1711462110.245398636) -1711462110.245532 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #59 L(:1c1 16957.905020)) -1711462110.245582 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #59: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1e03}} -1711462110.245608 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1e03) - deleting -1711462110.245620 [0] dq.builtin: => EVERYONE -1711462110.245652 [0] dq.builtin: delete -1711462110.245674 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.245805 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1448 from udp/10.101.12.71:40473 -1711462110.245815 [0] recvMC: INFOTS(1711462110.245476405) -1711462110.245827 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #60 L(:1c1 16957.905317) => EVERYONE -1711462110.245931 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #60: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.245942 [0] recvMC: HEARTBEAT(F#69:60..60 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@60(sync)) -1711462110.246616 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.246625 [0] recv: INFOTS(1711462110.246534188) -1711462110.246641 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #54 L(:1c1 16957.906127)) -1711462110.246694 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #54: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2104}} -1711462110.246726 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2104) => EVERYONE -1711462110.246749 [0] dq.builtin: - deleting -1711462110.246762 [0] dq.builtin: delete -1711462110.246784 [0] gc: gc 0x77041c014c20: deleting -1711462110.246796 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1e03) -1711462110.246805 [0] gc: gc 0x77041c030430: deleting -1711462110.246810 [0] gc: gc_delete_proxy_reader (0x77041c030430, 110d7b4:17c5b8c5:94bd0ff4:2104) -1711462110.246816 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=70 -1711462110.246818 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1e03) -1711462110.246829 [0] gc: gc 0x77041c014c20: deleting -1711462110.246835 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:1e03) -1711462110.246840 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=69 -1711462110.246899 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.246920 [0] recv: INFOTS(1711462110.246821907) -1711462110.246939 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #60 L(:1c1 16957.906422)) -1711462110.246975 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #60: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2003}} -1711462110.247001 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2003) - deleting -1711462110.247013 [0] dq.builtin: => EVERYONE -1711462110.247038 [0] dq.builtin: delete -1711462110.247042 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.247207 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1400 from udp/10.101.12.71:40473 -1711462110.247217 [0] recvMC: INFOTS(1711462110.246924796) -1711462110.247231 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #61 L(:1c1 16957.906720) => EVERYONE -1711462110.247338 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #61: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.247347 [0] recvMC: HEARTBEAT(F#70:61..61 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@61(sync)) -1711462110.248078 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.248093 [0] recv: INFOTS(1711462110.248003659) -1711462110.248113 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #55 L(:1c1 16957.907596)) -1711462110.248124 [0] gc: gc 0x77041c014c20: deleting -1711462110.248141 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #55: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2304}} -1711462110.248143 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2003) -1711462110.248162 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2304) => EVERYONE -1711462110.248196 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2003) -1711462110.248201 [0] dq.builtin: - deleting -1711462110.248234 [0] gc: gc 0x77041c014c20: deleting -1711462110.248241 [0] dq.builtin: delete -1711462110.248244 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2003) -1711462110.248263 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=68 -1711462110.248274 [0] gc: gc 0x77041c02cf40: deleting -1711462110.248281 [0] gc: gc_delete_proxy_reader (0x77041c02cf40, 110d7b4:17c5b8c5:94bd0ff4:2304) -1711462110.248287 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=67 -1711462110.249242 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.249250 [0] recv: INFOTS(1711462110.249152398) -1711462110.249260 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #61 L(:1c1 16957.908752)) -1711462110.249290 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #61: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2203}} -1711462110.249327 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2203) - deleting -1711462110.249346 [0] dq.builtin: => EVERYONE -1711462110.249370 [0] dq.builtin: delete -1711462110.249387 [0] gc: gc 0x77041c014c20: deleting -1711462110.249405 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2203) -1711462110.249439 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2203) -1711462110.249459 [0] gc: gc 0x77041c014c20: deleting -1711462110.249464 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2203) -1711462110.249470 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=66 -1711462110.249566 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1352 from udp/10.101.12.71:40473 -1711462110.249575 [0] recvMC: INFOTS(1711462110.249287705) -1711462110.249591 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #62 L(:1c1 16957.909078) => EVERYONE -1711462110.249703 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #62: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.249713 [0] recvMC: HEARTBEAT(F#71:62..62 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@62(sync)) -1711462110.250402 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.250418 [0] recv: INFOTS(1711462110.250315538) -1711462110.250432 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #56 L(:1c1 16957.909921)) -1711462110.250464 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #56: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2504}} -1711462110.250482 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2504) => EVERYONE -1711462110.250500 [0] dq.builtin: - deleting -1711462110.250518 [0] dq.builtin: delete -1711462110.250537 [0] gc: gc 0x77041c014c20: deleting -1711462110.250555 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2504) -1711462110.250569 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=65 -1711462110.250655 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.250663 [0] recv: INFOTS(1711462110.250535418) -1711462110.250673 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #62 L(:1c1 16957.910166)) -1711462110.250726 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #62: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2403}} -1711462110.250757 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2403) - deleting -1711462110.250764 [0] dq.builtin: => EVERYONE -1711462110.250778 [0] dq.builtin: delete -1711462110.250787 [0] gc: gc 0x77041c014c20: deleting -1711462110.250798 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2403) -1711462110.250844 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2403) -1711462110.250865 [0] gc: gc 0x77041c014c20: deleting -1711462110.250872 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2403) -1711462110.250878 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=64 -1711462110.250922 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1304 from udp/10.101.12.71:40473 -1711462110.250931 [0] recvMC: INFOTS(1711462110.250646289) -1711462110.250946 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #63 L(:1c1 16957.910434) => EVERYONE -1711462110.251044 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #63: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.251052 [0] recvMC: HEARTBEAT(F#72:63..63 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@63(sync)) -1711462110.251642 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 52 from udp/10.101.12.71:50807 -1711462110.251671 [0] recv: HEARTBEAT(#9:1..10 L(:1c1 16957.911162)110cd0d:56a27910:57318171:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@10(sync)) -1711462110.251704 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:56a27910:57318171:3c2: F#3:11/0: -1711462110.251720 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110cd0d:56a27910:57318171:3c2) -1711462110.251742 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.251783 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003c88:44 [ udp/10.101.12.71:7410@2 ] -1711462110.251798 [0] tev: traffic-xmit (1) 64 -1711462110.251903 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.251911 [0] recv: INFOTS(1711462110.251787909) -1711462110.251919 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #57 L(:1c1 16957.911414)) -1711462110.251946 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #57: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5104}} -1711462110.251956 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:5104) => EVERYONE -1711462110.251971 [0] dq.builtin: - deleting -1711462110.251982 [0] dq.builtin: delete -1711462110.252001 [0] gc: gc 0x77041c014c20: deleting -1711462110.252019 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5104) -1711462110.252029 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=63 -1711462110.253294 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.253315 [0] recv: INFOTS(1711462110.253143913) -1711462110.253329 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #63 L(:1c1 16957.912817)) -1711462110.253356 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #63: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5003}} -1711462110.253366 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5003) - deleting -1711462110.253371 [0] dq.builtin: => EVERYONE -1711462110.253386 [0] dq.builtin: delete -1711462110.253395 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.253560 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1256 from udp/10.101.12.71:40473 -1711462110.253578 [0] recvMC: INFOTS(1711462110.253252814) -1711462110.253601 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #64 L(:1c1 16957.913078) => EVERYONE -1711462110.253762 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #64: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.253780 [0] recvMC: HEARTBEAT(F#73:64..64 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@64(sync)) -1711462110.254417 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.254435 [0] recv: INFOTS(1711462110.254311896) -1711462110.254453 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #58 L(:1c1 16957.913938)) -1711462110.254462 [0] gc: gc 0x77041c014c20: deleting -1711462110.254483 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5003) -1711462110.254485 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #58: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2904}} -1711462110.254505 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2904) => EVERYONE -1711462110.254510 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5003) -1711462110.254517 [0] dq.builtin: - deleting -1711462110.254534 [0] gc: gc 0x77041c014c20: deleting -1711462110.254537 [0] dq.builtin: delete -1711462110.254547 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5003) -1711462110.254558 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=62 -1711462110.254568 [0] gc: gc 0x77041c026db0: deleting -1711462110.254572 [0] gc: gc_delete_proxy_reader (0x77041c026db0, 110d7b4:17c5b8c5:94bd0ff4:2904) -1711462110.254577 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=61 -1711462110.255561 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.255833 [0] recv: INFOTS(1711462110.255450496) -1711462110.255849 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #64 L(:1c1 16957.915335)) -1711462110.255867 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #64: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2803}} -1711462110.255881 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2803) - deleting -1711462110.255887 [0] dq.builtin: => EVERYONE -1711462110.255914 [0] dq.builtin: delete -1711462110.255921 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.255997 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1208 from udp/10.101.12.71:40473 -1711462110.256014 [0] recvMC: INFOTS(1711462110.255527534) -1711462110.256032 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #65 L(:1c1 16957.915514) => EVERYONE -1711462110.256328 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #65: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.256347 [0] recvMC: HEARTBEAT(F#74:65..65 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@65(sync)) -1711462110.256647 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.256653 [0] recv: INFOTS(1711462110.256559919) -1711462110.256670 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #59 L(:1c1 16957.916156)) -1711462110.256709 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #59: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2b04}} -1711462110.256719 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2b04) => EVERYONE -1711462110.256729 [0] dq.builtin: - deleting -1711462110.256738 [0] dq.builtin: delete -1711462110.256988 [0] gc: gc 0x77041c014c20: deleting -1711462110.256996 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2803) -1711462110.257001 [0] gc: gc 0x77041c02e1f0: deleting -1711462110.257005 [0] gc: gc_delete_proxy_reader (0x77041c02e1f0, 110d7b4:17c5b8c5:94bd0ff4:2b04) -1711462110.257011 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=60 -1711462110.257016 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2803) -1711462110.257057 [0] gc: gc 0x77041c014c20: deleting -1711462110.257063 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2803) -1711462110.257080 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=59 -1711462110.257793 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.257810 [0] recv: INFOTS(1711462110.257696628) -1711462110.257822 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #65 L(:1c1 16957.917309)) -1711462110.257841 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #65: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2a03}} -1711462110.257848 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2a03) - deleting -1711462110.257858 [0] dq.builtin: => EVERYONE -1711462110.257869 [0] dq.builtin: delete -1711462110.257877 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.258082 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1160 from udp/10.101.12.71:40473 -1711462110.258107 [0] recvMC: INFOTS(1711462110.257775243) -1711462110.258128 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #66 L(:1c1 16957.917606) => EVERYONE -1711462110.258273 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #66: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.258290 [0] recvMC: HEARTBEAT(F#75:66..66 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@66(sync)) -1711462110.258940 [0] gc: gc 0x77041c014c20: deleting -1711462110.258947 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2a03) -1711462110.258968 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2a03) -1711462110.258977 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.258995 [0] recv: INFOTS(1711462110.258846924) -1711462110.258989 [0] gc: gc 0x77041c014c20: deleting -1711462110.259030 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2a03) -1711462110.259042 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=58 -1711462110.259018 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #60 L(:1c1 16957.918498)) -1711462110.259066 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #60: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2d04}} -1711462110.259076 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2d04) => EVERYONE -1711462110.259096 [0] dq.builtin: - deleting -1711462110.259104 [0] dq.builtin: delete -1711462110.259109 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.260198 [0] gc: gc 0x77041c014c20: deleting -1711462110.260219 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2d04) -1711462110.260238 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=57 -1711462110.260404 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.260430 [0] recv: INFOTS(1711462110.260256234) -1711462110.260457 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #66 L(:1c1 16957.919930)) -1711462110.260479 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #66: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2c03}} -1711462110.260516 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2c03) - deleting -1711462110.260529 [0] dq.builtin: => EVERYONE -1711462110.260543 [0] dq.builtin: delete -1711462110.260552 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.260737 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1112 from udp/10.101.12.71:40473 -1711462110.260758 [0] recvMC: INFOTS(1711462110.260382757) -1711462110.260786 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #67 L(:1c1 16957.920258) => EVERYONE -1711462110.260978 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #67: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.261000 [0] recvMC: HEARTBEAT(F#76:67..67 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@67(sync)) -1711462110.261628 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.261649 [0] recv: INFOTS(1711462110.261474906) -1711462110.261631 [0] gc: gc 0x77041c014c20: deleting -1711462110.261697 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2c03) -1711462110.261681 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #61 L(:1c1 16957.921152)) -1711462110.261753 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2c03) -1711462110.261767 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #61: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2f04}} -1711462110.261792 [0] gc: gc 0x77041c014c20: deleting -1711462110.261809 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2f04) => EVERYONE -1711462110.261821 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2c03) -1711462110.261839 [0] dq.builtin: - deleting -1711462110.261869 [0] dq.builtin: delete -1711462110.261853 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=56 -1711462110.261892 [0] gc: gc 0x77041c014c20: deleting -1711462110.261900 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2f04) -1711462110.261906 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=55 -1711462110.262873 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.262889 [0] recv: INFOTS(1711462110.262693665) -1711462110.262921 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #67 L(:1c1 16957.922391)) -1711462110.262962 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #67: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2e03}} -1711462110.262987 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2e03) - deleting -1711462110.262996 [0] dq.builtin: => EVERYONE -1711462110.263013 [0] dq.builtin: delete -1711462110.263021 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.263197 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1064 from udp/10.101.12.71:40473 -1711462110.263224 [0] recvMC: INFOTS(1711462110.262818245) -1711462110.263258 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #68 L(:1c1 16957.922723) => EVERYONE -1711462110.263468 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #68: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.263491 [0] recvMC: HEARTBEAT(F#77:68..68 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@68(sync)) -1711462110.264121 [0] gc: gc 0x77041c014c20: deleting -1711462110.264136 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2e03) -1711462110.264163 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.264175 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2e03) -1711462110.264178 [0] recv: INFOTS(1711462110.263960068) -1711462110.264226 [0] gc: gc 0x77041c014c20: deleting -1711462110.264240 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #62 L(:1c1 16957.923681)) -1711462110.264244 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:2e03) -1711462110.264293 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=54 -1711462110.264305 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #62: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3104}} -1711462110.264351 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3104) => EVERYONE -1711462110.264374 [0] dq.builtin: - deleting -1711462110.264392 [0] dq.builtin: delete -1711462110.264399 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.265416 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.265433 [0] recv: INFOTS(1711462110.265251831) -1711462110.265468 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #68 L(:1c1 16957.924935)) -1711462110.265492 [0] gc: gc 0x77041c014c20: deleting -1711462110.265513 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3104) -1711462110.265520 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #68: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3003}} -1711462110.265525 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=53 -1711462110.265539 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3003) - deleting -1711462110.265575 [0] dq.builtin: => EVERYONE -1711462110.265587 [0] dq.builtin: delete -1711462110.265594 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.265786 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1016 from udp/10.101.12.71:40473 -1711462110.265815 [0] recvMC: INFOTS(1711462110.265383925) -1711462110.265858 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #69 L(:1c1 16957.925313) => EVERYONE -1711462110.266068 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #69: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.266093 [0] recvMC: HEARTBEAT(F#78:69..69 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@69(sync)) -1711462110.266656 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.266669 [0] recv: INFOTS(1711462110.266492303) -1711462110.266697 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #63 L(:1c1 16957.926172)) -1711462110.266712 [0] gc: gc 0x77041c014c20: deleting -1711462110.266743 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3003) -1711462110.266749 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #63: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3304}} -1711462110.266782 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3304) => EVERYONE -1711462110.266796 [0] dq.builtin: - deleting -1711462110.266782 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3003) -1711462110.266830 [0] dq.builtin: delete -1711462110.266892 [0] gc: gc 0x77041c02d8b0: deleting -1711462110.266910 [0] gc: gc_delete_proxy_reader (0x77041c02d8b0, 110d7b4:17c5b8c5:94bd0ff4:3304) -1711462110.266921 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=52 -1711462110.266935 [0] gc: gc 0x77041c014c20: deleting -1711462110.266941 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3003) -1711462110.266949 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=51 -1711462110.268354 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.268374 [0] recv: INFOTS(1711462110.268216434) -1711462110.268395 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #69 L(:1c1 16957.927877)) -1711462110.268433 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #69: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3203}} -1711462110.268456 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3203) - deleting -1711462110.268463 [0] dq.builtin: => EVERYONE -1711462110.268478 [0] dq.builtin: delete -1711462110.268486 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.268649 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 968 from udp/10.101.12.71:40473 -1711462110.268677 [0] recvMC: INFOTS(1711462110.268318557) -1711462110.268698 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #70 L(:1c1 16957.928176) => EVERYONE -1711462110.268868 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #70: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.268889 [0] recvMC: HEARTBEAT(F#79:70..70 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@70(sync)) -1711462110.269548 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.269558 [0] recv: INFOTS(1711462110.269427108) -1711462110.269575 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #64 L(:1c1 16957.929060)) -1711462110.269576 [0] gc: gc 0x77041c014c20: deleting -1711462110.269609 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #64: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3504}} -1711462110.269613 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3203) -1711462110.269634 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3504) => EVERYONE -1711462110.269664 [0] dq.builtin: - deleting -1711462110.269674 [0] dq.builtin: delete -1711462110.269680 [0] gc: gc 0x77041c056250: not yet, shortsleep -1711462110.269679 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3203) -1711462110.270725 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.270744 [0] recv: INFOTS(1711462110.270594373) -1711462110.270763 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #70 L(:1c1 16957.930247)) -1711462110.270763 [0] gc: gc 0x77041c056250: deleting -1711462110.270781 [0] gc: gc_delete_proxy_reader (0x77041c056250, 110d7b4:17c5b8c5:94bd0ff4:3504) -1711462110.270790 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #70: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3403}} -1711462110.270794 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=50 -1711462110.270817 [0] gc: gc 0x77041c014c20: deleting -1711462110.270807 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3403) - deleting -1711462110.270843 [0] dq.builtin: => EVERYONE -1711462110.270832 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3203) -1711462110.270857 [0] dq.builtin: delete -1711462110.270871 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=49 -1711462110.270902 [0] gc: gc 0x77041c056250: deleting -1711462110.270940 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c056250, 110d7b4:17c5b8c5:94bd0ff4:3403) -1711462110.270970 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c056250, 110d7b4:17c5b8c5:94bd0ff4:3403) -1711462110.270987 [0] gc: gc 0x77041c056250: deleting -1711462110.270993 [0] gc: gc_delete_proxy_writer (0x77041c056250, 110d7b4:17c5b8c5:94bd0ff4:3403) -1711462110.270999 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=48 -1711462110.271033 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 920 from udp/10.101.12.71:40473 -1711462110.271064 [0] recvMC: INFOTS(1711462110.270694611) -1711462110.271084 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #71 L(:1c1 16957.930564) => EVERYONE -1711462110.271255 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #71: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.271273 [0] recvMC: HEARTBEAT(F#80:71..71 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@71(sync)) -1711462110.271921 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.271948 [0] recv: INFOTS(1711462110.271774725) -1711462110.271961 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #65 L(:1c1 16957.931448)) -1711462110.271984 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #65: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3704}} -1711462110.271993 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3704 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3704) => EVERYONE -1711462110.272009 [0] dq.builtin: - deleting -1711462110.272018 [0] dq.builtin: delete -1711462110.272025 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.273111 [0] gc: gc 0x77041c014c20: deleting -1711462110.273149 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3704) -1711462110.273156 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=47 -1711462110.273343 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.273362 [0] recv: INFOTS(1711462110.273181976) -1711462110.273382 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #71 L(:1c1 16957.932863)) -1711462110.273402 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #71: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3603}} -1711462110.273414 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3603) - deleting -1711462110.273426 [0] dq.builtin: => EVERYONE -1711462110.273458 [0] dq.builtin: delete -1711462110.273464 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.273584 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 872 from udp/10.101.12.71:40473 -1711462110.273604 [0] recvMC: INFOTS(1711462110.273304522) -1711462110.273628 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #72 L(:1c1 16957.933104) => EVERYONE -1711462110.273808 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #72: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.273829 [0] recvMC: HEARTBEAT(F#81:72..72 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@72(sync)) -1711462110.274544 [0] gc: gc 0x77041c014c20: deleting -1711462110.274569 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3603) -1711462110.274612 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3603) -1711462110.274635 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.274650 [0] recv: INFOTS(1711462110.274472079) -1711462110.274640 [0] gc: gc 0x77041c014c20: deleting -1711462110.274684 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3603) -1711462110.274694 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=46 -1711462110.274663 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #66 L(:1c1 16957.934152)) -1711462110.274737 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #66: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3904}} -1711462110.274747 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3904) => EVERYONE -1711462110.274757 [0] dq.builtin: - deleting -1711462110.274765 [0] dq.builtin: delete -1711462110.274772 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.275867 [0] gc: gc 0x77041c014c20: deleting -1711462110.275885 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3904) -1711462110.275901 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=45 -1711462110.275986 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.276015 [0] recv: INFOTS(1711462110.275834015) -1711462110.276045 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #72 L(:1c1 16957.935518)) -1711462110.276074 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #72: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3803}} -1711462110.276085 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3803) - deleting -1711462110.276103 [0] dq.builtin: => EVERYONE -1711462110.276120 [0] dq.builtin: delete -1711462110.276127 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.276301 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 824 from udp/10.101.12.71:40473 -1711462110.276328 [0] recvMC: INFOTS(1711462110.275936014) -1711462110.276361 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #73 L(:1c1 16957.935828) => EVERYONE -1711462110.276595 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #73: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0) -1711462110.276618 [0] recvMC: HEARTBEAT(F#82:73..73 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@73(sync)) -1711462110.276695 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) suppressed, resched in 0.1 s (min-ack 13, avail-seq 24, xmit 24) -1711462110.277041 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.277081 [0] recv: HEARTBEAT(#8:1..13 L(:1c1 16957.936563)110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:4c7@13(sync)) -1711462110.277110 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#4:14/0: -1711462110.277143 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462110.277159 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.277223 [0] gc: gc 0x77041c014c20: deleting -1711462110.277234 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3803) -1711462110.277235 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003c88:44 [ udp/10.101.12.71:7416@2 ] -1711462110.277285 [0] tev: traffic-xmit (1) 64 -1711462110.277262 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3803) -1711462110.277335 [0] gc: gc 0x77041c014c20: deleting -1711462110.277350 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3803) -1711462110.277360 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=44 -1711462110.277481 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.277490 [0] recv: INFOTS(1711462110.277107873) -1711462110.277539 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #67 L(:1c1 16957.936991)) -1711462110.277574 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #67: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3b04}} -1711462110.277586 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3b04) => EVERYONE -1711462110.277600 [0] dq.builtin: - deleting -1711462110.277621 [0] dq.builtin: delete -1711462110.277630 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.278496 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:4c2 multicasting (rel-prd 6 seq-eq-max 6 seq 19 maxseq 10) -1711462110.278521 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:4c2) sent, resched in 0.1 s (min-ack 10, avail-seq 19, xmit 19) -1711462110.278535 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.278625 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.278650 [0] recv: HEARTBEAT(#7:1..19 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.278631 [0] tev: nn_xpack_send 52: 0x77041400139c:20 0x770424003c98:32 [ udp/239.255.0.1:7400@2 ] -1711462110.278684 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.278702 [0] recv: INFOTS(1711462110.278533644) -1711462110.278735 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #73 L(:1c1 16957.938204)) -1711462110.278688 [0] tev: traffic-xmit (1) 52 -1711462110.278752 [0] gc: gc 0x77041c014c20: deleting -1711462110.278779 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.278801 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.278762 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #73: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3a03}} -1711462110.278847 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3a03) - deleting -1711462110.278869 [0] dq.builtin: => EVERYONE -1711462110.278835 [0] recv: ACKNACK(F#4:20/0: L(:1c1 16957.938301) 110e21c:d0762c48:a9f0fb28:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0) -1711462110.278890 [0] dq.builtin: delete -1711462110.278853 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3b04) -1711462110.278913 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.278965 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.278944 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=43 -1711462110.278989 [0] recv: ACKNACK(F#4:20/0: L(:1c1 16957.938467) 110443d:bb7f10a5:18901533:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0) -1711462110.279015 [0] gc: gc 0x77041c05a250: deleting -1711462110.279036 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:3a03) -1711462110.279071 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:3a03) -1711462110.279075 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.279099 [0] gc: gc 0x77041c05a250: deleting -1711462110.279116 [0] gc: gc_delete_proxy_writer (0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:3a03) -1711462110.279147 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=42 -1711462110.279121 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 776 from udp/10.101.12.71:40473 -1711462110.279102 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.279179 [0] recvMC: INFOTS(1711462110.278642503) -1711462110.279213 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #74 L(:1c1 16957.938681) => EVERYONE -1711462110.279224 [0] recv: ACKNACK(F#4:20/0: 110d7b4:17c5b8c5:94bd0ff4:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0) -1711462110.279253 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.279268 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.279293 [0] recv: ACKNACK(F#4:20/0: L(:1c1 16957.938769) 110cd0d:56a27910:57318171:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0) -1711462110.279307 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.279326 [0] recv: ACKNACK(F#12:20/0: L(:1c1 16957.938769) 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM0) -1711462110.279337 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.279351 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #74: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.279373 [0] recvMC: HEARTBEAT(F#83:74..74 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@74(sync)) -1711462110.279374 [0] recv: ACKNACK(F#7:20/0: L(:1c1 16957.938769) 110cd0d:79d6eeac:ea4a8907:4c7 -> 110aba1:7b19badd:ce0adb73:4c2 ACK9 RM9) -1711462110.279487 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.279503 [0] recv: HEARTBEAT(#8:1..19 L(:1c1 16957.938996)110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@19(sync)) -1711462110.279531 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#4:20/0: -1711462110.279556 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.279579 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.279622 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003c88:44 [ udp/10.101.12.71:7416@2 ] -1711462110.279634 [0] tev: traffic-xmit (1) 64 -1711462110.279796 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:3c2 multicasting (rel-prd 6 seq-eq-max 6 seq 23 maxseq 12) -1711462110.279813 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:3c2) sent, resched in 0.1 s (min-ack 12, avail-seq 23, xmit 23) -1711462110.279825 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.279885 [0] tev: nn_xpack_send 52: 0x77041400139c:20 0x770424003c98:32 [ udp/239.255.0.1:7400@2 ] -1711462110.279894 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.279914 [0] recv: HEARTBEAT(#7:1..23 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.279898 [0] tev: traffic-xmit (1) 52 -1711462110.279980 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.279988 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.280010 [0] recv: ACKNACK(F#4:24/0: L(:1c1 16957.939491) 110443d:bb7f10a5:18901533:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK11 RM0) -1711462110.280027 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.280033 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.280042 [0] recv: ACKNACK(F#4:24/0: L(:1c1 16957.939536) 110e21c:d0762c48:a9f0fb28:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK11 RM0) -1711462110.280075 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.280090 [0] recv: INFOTS(1711462110.279820853) -1711462110.280111 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #68 L(:1c1 16957.939593)) -1711462110.280151 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #68: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3d04}} -1711462110.280168 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3d04) => EVERYONE -1711462110.280176 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.280205 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.280187 [0] dq.builtin: - deleting -1711462110.280246 [0] recv: ACKNACK(F#4:24/0: L(:1c1 16957.939707) 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK11 RM0) -1711462110.280267 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.280257 [0] dq.builtin: delete -1711462110.280271 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.280318 [0] recv: ACKNACK(F#14:24/0: L(:1c1 16957.939707) 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK11 RM0) -1711462110.280328 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.280338 [0] recv: ACKNACK(F#8:24/0: L(:1c1 16957.939707) 110cd0d:79d6eeac:ea4a8907:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK11 RM0) -1711462110.280350 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.280357 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.280372 [0] recv: ACKNACK(F#4:24/0: L(:1c1 16957.939860) 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK11 RM11) -1711462110.281289 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.281312 [0] recv: INFOTS(1711462110.281176913) -1711462110.281325 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #74 L(:1c1 16957.940811)) -1711462110.281354 [0] gc: gc 0x77041c014c20: deleting -1711462110.281381 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3d04) -1711462110.281391 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=41 -1711462110.281392 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #74: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3c03}} -1711462110.281460 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3c03) - deleting -1711462110.281478 [0] dq.builtin: => EVERYONE -1711462110.281515 [0] dq.builtin: delete -1711462110.281523 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.281607 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 728 from udp/10.101.12.71:40473 -1711462110.281621 [0] recvMC: INFOTS(1711462110.281294357) -1711462110.281645 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #75 L(:1c1 16957.941123) => EVERYONE -1711462110.281804 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #75: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,64,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.281818 [0] recvMC: HEARTBEAT(F#84:75..75 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@75(sync)) -1711462110.282617 [0] gc: gc 0x77041c014c20: deleting -1711462110.282643 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3c03) -1711462110.282622 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.282684 [0] recv: INFOTS(1711462110.282396477) -1711462110.282683 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3c03) -1711462110.282711 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #69 L(:1c1 16957.942185)) -1711462110.282749 [0] gc: gc 0x77041c014c20: deleting -1711462110.282781 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3c03) -1711462110.282787 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #69: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3f04}} -1711462110.282789 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=40 -1711462110.282825 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3f04) => EVERYONE -1711462110.282853 [0] dq.builtin: - deleting -1711462110.282872 [0] dq.builtin: delete -1711462110.282878 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.283771 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.283788 [0] recv: INFOTS(1711462110.283671990) -1711462110.283803 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #75 L(:1c1 16957.943291)) -1711462110.283870 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #75: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3e03}} -1711462110.283905 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3e03) - deleting -1711462110.283931 [0] dq.builtin: => EVERYONE -1711462110.283963 [0] dq.builtin: delete -1711462110.283984 [0] gc: gc 0x77041c014c20: deleting -1711462110.284021 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:3f04) -1711462110.284040 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=39 -1711462110.284061 [0] gc: gc 0x77041c05a250: deleting -1711462110.284085 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:3e03) -1711462110.284112 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:3e03) -1711462110.284139 [0] gc: gc 0x77041c05a250: deleting -1711462110.284150 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 680 from udp/10.101.12.71:40473 -1711462110.284163 [0] gc: gc_delete_proxy_writer (0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:3e03) -1711462110.284183 [0] recvMC: INFOTS(1711462110.283774544) -1711462110.284207 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #76 L(:1c1 16957.943684) => EVERYONE -1711462110.284215 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=38 -1711462110.284334 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #76: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,66,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,68,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.284350 [0] recvMC: HEARTBEAT(F#85:76..76 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@76(sync)) -1711462110.285001 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.285016 [0] recv: INFOTS(1711462110.284892655) -1711462110.285052 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #70 L(:1c1 16957.944519)) -1711462110.285096 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #70: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4104}} -1711462110.285121 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4104) => EVERYONE -1711462110.285154 [0] dq.builtin: - deleting -1711462110.285178 [0] dq.builtin: delete -1711462110.285196 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.286187 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.286197 [0] recv: INFOTS(1711462110.286072873) -1711462110.286224 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #76 L(:1c1 16957.945699)) -1711462110.286260 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #76: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4003}} -1711462110.286283 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4003) - deleting -1711462110.286289 [0] gc: gc 0x77041c014c20: deleting -1711462110.286317 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4104) -1711462110.286298 [0] dq.builtin: => EVERYONE -1711462110.286334 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=37 -1711462110.286379 [0] dq.builtin: delete -1711462110.286392 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.286559 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 632 from udp/10.101.12.71:40473 -1711462110.286572 [0] recvMC: INFOTS(1711462110.286193571) -1711462110.286589 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #77 L(:1c1 16957.946074) => EVERYONE -1711462110.286736 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #77: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,68,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,70,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.286747 [0] recvMC: HEARTBEAT(F#86:77..77 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@77(sync)) -1711462110.287348 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.287359 [0] recv: INFOTS(1711462110.287238788) -1711462110.287373 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #71 L(:1c1 16957.946861)) -1711462110.287414 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #71: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4304}} -1711462110.287437 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4304) => EVERYONE -1711462110.287461 [0] dq.builtin: - deleting -1711462110.287476 [0] dq.builtin: delete -1711462110.287486 [0] gc: gc 0x77041c014c20: deleting -1711462110.287528 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4003) -1711462110.287549 [0] gc: gc 0x77041c05a250: deleting -1711462110.287566 [0] gc: gc_delete_proxy_reader (0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:4304) -1711462110.287577 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=36 -1711462110.287577 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4003) -1711462110.287627 [0] gc: gc 0x77041c014c20: deleting -1711462110.287642 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4003) -1711462110.287650 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=35 -1711462110.288527 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.288542 [0] recv: INFOTS(1711462110.288409621) -1711462110.288562 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #77 L(:1c1 16957.948045)) -1711462110.288597 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #77: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4203}} -1711462110.288672 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4203) - deleting -1711462110.288689 [0] dq.builtin: => EVERYONE -1711462110.288734 [0] dq.builtin: delete -1711462110.288756 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.288970 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 584 from udp/10.101.12.71:40473 -1711462110.288986 [0] recvMC: INFOTS(1711462110.288540698) -1711462110.289007 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #78 L(:1c1 16957.948488) => EVERYONE -1711462110.289142 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #78: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,70,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.289154 [0] recvMC: HEARTBEAT(F#87:78..78 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@78(sync)) -1711462110.289703 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.289717 [0] recv: INFOTS(1711462110.289583439) -1711462110.289730 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #72 L(:1c1 16957.949219)) -1711462110.289787 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #72: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4504}} -1711462110.289814 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4504) => EVERYONE -1711462110.289836 [0] dq.builtin: - deleting -1711462110.289865 [0] dq.builtin: delete -1711462110.289848 [0] gc: gc 0x77041c014c20: deleting -1711462110.289935 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4203) -1711462110.289956 [0] gc: gc 0x77041c05a250: deleting -1711462110.289969 [0] gc: gc_delete_proxy_reader (0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:4504) -1711462110.289974 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4203) -1711462110.289983 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=34 -1711462110.290033 [0] gc: gc 0x77041c014c20: deleting -1711462110.290046 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4203) -1711462110.290058 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=33 -1711462110.290870 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.290883 [0] recv: INFOTS(1711462110.290746464) -1711462110.290902 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #78 L(:1c1 16957.950385)) -1711462110.290991 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #78: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4403}} -1711462110.291011 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4403) - deleting -1711462110.291020 [0] dq.builtin: => EVERYONE -1711462110.291049 [0] dq.builtin: delete -1711462110.291063 [0] gc: gc 0x77041c014c20: deleting -1711462110.291126 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4403) -1711462110.291134 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 536 from udp/10.101.12.71:40473 -1711462110.291143 [0] recvMC: INFOTS(1711462110.290874452) -1711462110.291155 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #79 L(:1c1 16957.950646) => EVERYONE -1711462110.291161 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4403) -1711462110.291229 [0] gc: gc 0x77041c014c20: deleting -1711462110.291239 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4403) -1711462110.291249 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=32 -1711462110.291302 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #79: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.291314 [0] recvMC: HEARTBEAT(F#88:79..79 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@79(sync)) -1711462110.292040 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.292049 [0] recv: INFOTS(1711462110.291919173) -1711462110.292070 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #73 L(:1c1 16957.951552)) -1711462110.292113 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #73: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4704}} -1711462110.292127 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4704 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4704) => EVERYONE -1711462110.292147 [0] dq.builtin: - deleting -1711462110.292169 [0] dq.builtin: delete -1711462110.292181 [0] gc: gc 0x77041c014c20: deleting -1711462110.292198 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4704) -1711462110.292210 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=31 -1711462110.292378 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.292395 [0] recv: INFOTS(1711462110.292287600) -1711462110.292421 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #79 L(:1c1 16957.951898)) -1711462110.292455 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #79: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4603}} -1711462110.292470 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4603) - deleting -1711462110.292484 [0] dq.builtin: => EVERYONE -1711462110.292572 [0] dq.builtin: delete -1711462110.292601 [0] gc: gc 0x77041c014c20: deleting -1711462110.292625 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4603) -1711462110.292671 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4603) -1711462110.292705 [0] gc: gc 0x77041c014c20: deleting -1711462110.292727 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4603) -1711462110.292748 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=30 -1711462110.292818 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 488 from udp/10.101.12.71:40473 -1711462110.292833 [0] recvMC: INFOTS(1711462110.292409838) -1711462110.292854 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #80 L(:1c1 16957.952336) => EVERYONE -1711462110.292983 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #80: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.292993 [0] recvMC: HEARTBEAT(F#89:80..80 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@80(sync)) -1711462110.293595 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.293619 [0] recv: INFOTS(1711462110.293464039) -1711462110.293642 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #74 L(:1c1 16957.953121)) -1711462110.293667 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.293674 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #74: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4904}} -1711462110.293699 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4904) => EVERYONE -1711462110.293702 [0] recv: HEARTBEAT(#10:1..15 L(:1c1 16957.953183)110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:4c7@15(sync)) -1711462110.293743 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#4:16/0: -1711462110.293769 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.293790 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.293718 [0] dq.builtin: - deleting -1711462110.293845 [0] dq.builtin: delete -1711462110.293863 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003c88:44 [ udp/10.101.12.71:7412@2 ] -1711462110.293876 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.293879 [0] tev: traffic-xmit (1) 64 -1711462110.294770 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.294782 [0] recv: INFOTS(1711462110.294634250) -1711462110.294794 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #80 L(:1c1 16957.954284)) -1711462110.294868 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #80: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4803}} -1711462110.294879 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4803) - deleting -1711462110.294884 [0] dq.builtin: => EVERYONE -1711462110.294896 [0] dq.builtin: delete -1711462110.294999 [0] gc: gc 0x77041c014c20: deleting -1711462110.295007 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4904) -1711462110.295016 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=29 -1711462110.295029 [0] gc: gc 0x77041c05a250: deleting -1711462110.295036 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:4803) -1711462110.295078 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:4803) -1711462110.295112 [0] gc: gc 0x77041c05a250: deleting -1711462110.295126 [0] gc: gc_delete_proxy_writer (0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:4803) -1711462110.295141 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=28 -1711462110.295161 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 440 from udp/10.101.12.71:40473 -1711462110.295175 [0] recvMC: INFOTS(1711462110.294787650) -1711462110.295204 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #81 L(:1c1 16957.954677) => EVERYONE -1711462110.295354 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #81: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.295368 [0] recvMC: HEARTBEAT(F#90:81..81 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@81(sync)) -1711462110.295502 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.295529 [0] recv: HEARTBEAT(#10:1..19 L(:1c1 16957.955017)110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@19(sync)) -1711462110.295565 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#4:20/0: -1711462110.295590 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.295616 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.295659 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003c88:44 [ udp/10.101.12.71:7412@2 ] -1711462110.295694 [0] tev: traffic-xmit (1) 64 -1711462110.295915 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.295922 [0] recv: INFOTS(1711462110.295791082) -1711462110.295943 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #75 L(:1c1 16957.955425)) -1711462110.295975 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #75: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4b04}} -1711462110.295985 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4b04) => EVERYONE -1711462110.296003 [0] dq.builtin: - deleting -1711462110.296012 [0] dq.builtin: delete -1711462110.296036 [0] gc: gc 0x77041c014c20: deleting -1711462110.296059 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4b04) -1711462110.296073 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=27 -1711462110.297069 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.297089 [0] recv: INFOTS(1711462110.296972092) -1711462110.297110 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #81 L(:1c1 16957.956580)) -1711462110.297142 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #81: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4a03}} -1711462110.297236 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4a03) - deleting -1711462110.297255 [0] dq.builtin: => EVERYONE -1711462110.297269 [0] dq.builtin: delete -1711462110.297292 [0] gc: gc 0x77041c014c20: deleting -1711462110.297323 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4a03) -1711462110.297362 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4a03) -1711462110.297392 [0] gc: gc 0x77041c014c20: deleting -1711462110.297406 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4a03) -1711462110.297424 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 392 from udp/10.101.12.71:40473 -1711462110.297427 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=26 -1711462110.297456 [0] recvMC: INFOTS(1711462110.297082585) -1711462110.297469 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #82 L(:1c1 16957.956958) => EVERYONE -1711462110.297559 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #82: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.297569 [0] recvMC: HEARTBEAT(F#91:82..82 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@82(sync)) -1711462110.298237 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.298244 [0] recv: INFOTS(1711462110.298143983) -1711462110.298254 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #76 L(:1c1 16957.957746)) -1711462110.298287 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #76: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4d04}} -1711462110.298300 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4d04) => EVERYONE -1711462110.298311 [0] dq.builtin: - deleting -1711462110.298318 [0] dq.builtin: delete -1711462110.298327 [0] gc: gc 0x77041c014c20: deleting -1711462110.298340 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4d04) -1711462110.298354 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=25 -1711462110.298573 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.298587 [0] recv: INFOTS(1711462110.298471822) -1711462110.298601 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #82 L(:1c1 16957.958090)) -1711462110.298638 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #82: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4c03}} -1711462110.298653 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4c03) - deleting -1711462110.298661 [0] dq.builtin: => EVERYONE -1711462110.298678 [0] dq.builtin: delete -1711462110.298699 [0] gc: gc 0x77041c014c20: deleting -1711462110.298721 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4c03) -1711462110.298749 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4c03) -1711462110.298774 [0] gc: gc 0x77041c014c20: deleting -1711462110.298785 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4c03) -1711462110.298795 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=24 -1711462110.298834 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462110.298841 [0] recvMC: INFOTS(1711462110.298593713) -1711462110.298851 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #83 L(:1c1 16957.958344) => EVERYONE -1711462110.298928 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #83: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.298937 [0] recvMC: HEARTBEAT(F#92:83..83 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@83(sync)) -1711462110.299877 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.299888 [0] recv: INFOTS(1711462110.299733268) -1711462110.299899 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #77 L(:1c1 16957.959390)) -1711462110.299936 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #77: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4f04}} -1711462110.299959 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4f04) => EVERYONE -1711462110.299981 [0] dq.builtin: - deleting -1711462110.299993 [0] dq.builtin: delete -1711462110.300000 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.301093 [0] gc: gc 0x77041c014c20: deleting -1711462110.301123 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4f04) -1711462110.301136 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=23 -1711462110.301135 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.301208 [0] recv: INFOTS(1711462110.301005028) -1711462110.301223 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #83 L(:1c1 16957.960710)) -1711462110.301249 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #83: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4e03}} -1711462110.301268 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4e03) - deleting -1711462110.301281 [0] dq.builtin: => EVERYONE -1711462110.301302 [0] dq.builtin: delete -1711462110.301309 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.301361 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 296 from udp/10.101.12.71:40473 -1711462110.301370 [0] recvMC: INFOTS(1711462110.301158893) -1711462110.301382 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #84 L(:1c1 16957.960872) => EVERYONE -1711462110.301459 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #84: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.301469 [0] recvMC: HEARTBEAT(F#93:84..84 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@84(sync)) -1711462110.302373 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.302395 [0] recv: INFOTS(1711462110.302235006) -1711462110.302380 [0] gc: gc 0x77041c014c20: deleting -1711462110.302478 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4e03) -1711462110.302495 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #78 L(:1c1 16957.961896)) -1711462110.302525 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4e03) -1711462110.302574 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #78: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5304}} -1711462110.302617 [0] gc: gc 0x77041c014c20: deleting -1711462110.302629 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:4e03) -1711462110.302640 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=22 -1711462110.302621 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:5304) => EVERYONE -1711462110.302672 [0] dq.builtin: - deleting -1711462110.302681 [0] dq.builtin: delete -1711462110.302687 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.303585 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.303603 [0] recv: INFOTS(1711462110.303466791) -1711462110.303622 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #84 L(:1c1 16957.963106)) -1711462110.303666 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #84: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5203}} -1711462110.303689 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5203) - deleting -1711462110.303716 [0] dq.builtin: => EVERYONE -1711462110.303743 [0] dq.builtin: delete -1711462110.303761 [0] gc: gc 0x77041c014c20: deleting -1711462110.303778 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5304) -1711462110.303788 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=21 -1711462110.303799 [0] gc: gc 0x77041c05a250: deleting -1711462110.303806 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:5203) -1711462110.303838 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:5203) -1711462110.303846 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 248 from udp/10.101.12.71:40473 -1711462110.303875 [0] recvMC: INFOTS(1711462110.303608296) -1711462110.303886 [0] gc: gc 0x77041c05a250: deleting -1711462110.303894 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #85 L(:1c1 16957.963377) => EVERYONE -1711462110.303908 [0] gc: gc_delete_proxy_writer (0x77041c05a250, 110d7b4:17c5b8c5:94bd0ff4:5203) -1711462110.303953 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=20 -1711462110.303974 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #85: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.303987 [0] recvMC: HEARTBEAT(F#94:85..85 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@85(sync)) -1711462110.304771 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.304780 [0] recv: INFOTS(1711462110.304665223) -1711462110.304798 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #79 L(:1c1 16957.964283)) -1711462110.304861 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #79: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5504}} -1711462110.304889 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:5504) => EVERYONE -1711462110.304917 [0] dq.builtin: - deleting -1711462110.304940 [0] dq.builtin: delete -1711462110.304954 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.306037 [0] gc: gc 0x77041c014c20: deleting -1711462110.306062 [0] gc: gc_delete_proxy_reader (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5504) -1711462110.306075 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=19 -1711462110.306087 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.306128 [0] recv: INFOTS(1711462110.305944071) -1711462110.306153 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #85 L(:1c1 16957.965628)) -1711462110.306269 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #85: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5403}} -1711462110.306290 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5403) - deleting -1711462110.306302 [0] dq.builtin: => EVERYONE -1711462110.306327 [0] dq.builtin: delete -1711462110.306339 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.307419 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 224 from udp/10.101.12.71:40473 -1711462110.307436 [0] recvMC: INFOTS(1711462110.307152640) -1711462110.307440 [0] gc: gc 0x77041c014c20: deleting -1711462110.307498 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5403) -1711462110.307551 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5403) -1711462110.307617 [0] gc: gc 0x77041c014c20: deleting -1711462110.307637 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5403) -1711462110.307470 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #86 L(:1c1 16957.966937) => EVERYONE -1711462110.307652 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=18 -1711462110.307710 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #86: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.307725 [0] recvMC: HEARTBEAT(F#95:86..86 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@86(sync)) -1711462110.307893 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.307917 [0] recv: INFOTS(1711462110.307705837) -1711462110.307945 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #86 L(:1c1 16957.967418)) -1711462110.307984 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #86: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5603}} -1711462110.307996 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5603) - deleting -1711462110.308016 [0] dq.builtin: => EVERYONE -1711462110.308042 [0] dq.builtin: delete -1711462110.308057 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.308371 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 200 from udp/10.101.12.71:40473 -1711462110.308403 [0] recvMC: INFOTS(1711462110.308113998) -1711462110.308432 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #87 L(:1c1 16957.967901) => EVERYONE -1711462110.308569 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #87: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.308591 [0] recvMC: HEARTBEAT(F#96:87..87 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@87(sync)) -1711462110.308871 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.308889 [0] recv: INFOTS(1711462110.308696854) -1711462110.308926 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #87 L(:1c1 16957.968390)) -1711462110.308987 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #87: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5703}} -1711462110.309019 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5703 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5703) - deleting -1711462110.309034 [0] dq.builtin: => EVERYONE -1711462110.309066 [0] dq.builtin: delete -1711462110.309120 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 176 from udp/10.101.12.71:40473 -1711462110.309145 [0] gc: gc 0x77041c014c20: deleting -1711462110.309195 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462110.309162 [0] recvMC: INFOTS(1711462110.308810509) -1711462110.309257 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462110.309273 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #88 L(:1c1 16957.968661) => EVERYONE -1711462110.309224 [0] gc: gc 0x77041c04e320: deleting -1711462110.309361 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c04e320, 110d7b4:17c5b8c5:94bd0ff4:5703) -1711462110.309389 [0] gc: gc 0x77041c014c20: deleting -1711462110.309404 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462110.309405 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c04e320, 110d7b4:17c5b8c5:94bd0ff4:5703) -1711462110.309424 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=17 -1711462110.309367 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #88: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.309463 [0] gc: gc 0x77041c04e320: deleting -1711462110.309486 [0] recvMC: HEARTBEAT(F#97:88..88 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@88(sync)) -1711462110.309488 [0] gc: gc_delete_proxy_writer (0x77041c04e320, 110d7b4:17c5b8c5:94bd0ff4:5703) -1711462110.309516 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=16 -1711462110.310003 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.310016 [0] recv: INFOTS(1711462110.309878806) -1711462110.310038 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #88 L(:1c1 16957.969517)) -1711462110.310106 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #88: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5803}} -1711462110.310126 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5803) - deleting -1711462110.310175 [0] dq.builtin: => EVERYONE -1711462110.310208 [0] dq.builtin: delete -1711462110.310229 [0] gc: gc 0x77041c014c20: deleting -1711462110.310245 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5803) -1711462110.310277 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5803) -1711462110.310299 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 152 from udp/10.101.12.71:40473 -1711462110.310304 [0] gc: gc 0x77041c014c20: deleting -1711462110.310356 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:5803) -1711462110.310338 [0] recvMC: INFOTS(1711462110.310031935) -1711462110.310376 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=15 -1711462110.310410 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #89 L(:1c1 16957.969836) => EVERYONE -1711462110.310466 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #89: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{}}}}) -1711462110.310486 [0] recvMC: HEARTBEAT(F#98:89..89 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@89(sync)) -1711462110.311188 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.311198 [0] recv: INFOTS(1711462110.311071865) -1711462110.311218 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #89 L(:1c1 16957.970700)) -1711462110.311281 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #89: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:603}} -1711462110.311316 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:603) - deleting -1711462110.311332 [0] dq.builtin: => EVERYONE -1711462110.311369 [0] dq.builtin: delete -1711462110.311333 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 120 from udp/10.101.12.71:40473 -1711462110.311387 [0] gc: gc 0x77041c014c20: not yet, shortsleep -1711462110.311454 [0] recvMC: INFOTS(1711462110.311185386) -1711462110.311480 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #90 L(:1c1 16957.970902) => EVERYONE -1711462110.311514 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #90: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{}}) -1711462110.311529 [0] recvMC: HEARTBEAT(F#99:90..90 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@90(sync)) -1711462110.312481 [0] gc: gc 0x77041c014c20: deleting -1711462110.312489 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:603) -1711462110.312514 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:603) -1711462110.312541 [0] gc: gc 0x77041c014c20: deleting -1711462110.312550 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110d7b4:17c5b8c5:94bd0ff4:603) -1711462110.312559 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=14 -1711462110.314590 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.314630 [0] recv: HEARTBEAT(#9:1..79 L(:1c1 16957.974112)110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:4c7@79(sync)) -1711462110.314664 [0] tev: acknack 110aba1:7b19badd:ce0adb73:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#4:80/0: -1711462110.314688 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.314709 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.314772 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003c88:44 [ udp/10.101.12.71:7414@2 ] -1711462110.314785 [0] tev: traffic-xmit (1) 64 -1711462110.315979 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.316006 [0] recv: HEARTBEAT(#10:1..89 L(:1c1 16957.975493)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@89(sync)) -1711462110.316044 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#5:90/0: -1711462110.316099 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.316119 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.316175 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003c88:44 [ udp/10.101.12.71:7414@2 ] -1711462110.316185 [0] tev: traffic-xmit (1) 64 -1711462110.376709 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.376778 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:403 multicasting (rel-prd 6 seq-eq-max 6 seq 24 maxseq 13) -1711462110.376795 [0] recvMC: HEARTBEAT(#29:20..20 L(:1c1 16958.036265)110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:504@20(sync)) -1711462110.376833 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:403) sent, resched in 0.1 s (min-ack 13, avail-seq 24, xmit 24) -1711462110.376885 [0] tev: xpack_addmsg 0x770414001390 0x770424003ba0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.376916 [0] tev: acknack 110aba1:7b19badd:ce0adb73:504 -> 110443d:bb7f10a5:18901533:403: F#5:21/0: -1711462110.376937 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.377054 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.377081 [0] recvMC: HEARTBEAT(#35:24..24 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462110.377214 [0] tev: nn_xpack_send 52: 0x77041400139c:20 0x770424003c98:32 [ udp/239.255.0.1:7401@2 ] -1711462110.377252 [0] tev: traffic-xmit (1) 52 -1711462110.377274 [0] tev: xpack_addmsg 0x770414001390 0x770424003990 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.377351 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003a78:44 [ udp/10.101.12.71:7417@2 ] -1711462110.377382 [0] tev: traffic-xmit (1) 64 -1711462110.377528 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.377566 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.377605 [0] recvUC: ACKNACK(F#5:25/0: L(:1c1 16958.037060) 110e21c:d0762c48:a9f0fb28:504 -> 110aba1:7b19badd:ce0adb73:403 ACK11 RM0) -1711462110.377639 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.377646 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.377663 [0] recvUC: ACKNACK(F#5:25/0: L(:1c1 16958.037148) 110d7b4:17c5b8c5:94bd0ff4:504 -> 110aba1:7b19badd:ce0adb73:403 ACK11 RM0) -1711462110.377678 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.377690 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.377705 [0] recvUC: ACKNACK(F#5:25/0: L(:1c1 16958.037189) 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403 ACK11 RM0) -1711462110.377713 [0] recvUC: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.377725 [0] recvUC: ACKNACK(F#21:25/0: L(:1c1 16958.037189) 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403 ACK11 RM0) -1711462110.377732 [0] recvUC: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.377744 [0] recvUC: ACKNACK(F#12:25/0: L(:1c1 16958.037189) 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403 ACK11 RM0) -1711462110.378044 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.378100 [0] recvUC: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.378143 [0] recvUC: ACKNACK(F#5:25/0: L(:1c1 16958.037594) 110443d:bb7f10a5:18901533:504 -> 110aba1:7b19badd:ce0adb73:403remove_acked_messages: deleting lingering writer 110aba1:7b19badd:ce0adb73:403 -1711462110.378156 [0] recvUC: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:403) ... -1711462110.378180 [0] recvUC: => EVERYONE -1711462110.378227 [0] recvUC: writer_set_state(110aba1:7b19badd:ce0adb73:403) state transition 2 -> 3 -1711462110.378252 [0] recvUC: ACK11 RM0) -1711462110.378307 [0] gc: gc 0x770404002690: deleting -1711462110.378335 [0] gc: gc_delete_writer(0x770404002690, 110aba1:7b19badd:ce0adb73:403) -1711462110.378401 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:3c2 #24: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:403}} -1711462110.378422 [0] gc: non-timed queue now has 1 items -1711462110.378458 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:403 @ 0x653a1b487c64) user 5 builtin 12 -1711462110.378534 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:4c2) suppressed, resched in inf s (min-ack 19, avail-seq 1, xmit 19) -1711462110.378537 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:504) ... -1711462110.378638 [0] python3: => EVERYONE -1711462110.378601 [0] tev: xpack_addmsg 0x770414001390 0x77041c049320 0(data(110aba1:7b19badd:ce0adb73:3c2:#24/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.378802 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x770424003d48:48 0x770424003544:28 [ udp/239.255.0.1:7400@2 ] -1711462110.378825 [0] tev: traffic-xmit (1) 96 -1711462110.378800 [0] gc: gc 0x653a1b29e680: not yet, shortsleep -1711462110.378848 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.379048 [0] recv: INFOTS(1711462110.378453601) -1711462110.379082 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #20 L(:1c1 16958.038540)) -1711462110.379135 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.379158 [0] recv: INFOTS(1711462110.378380676) -1711462110.379175 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #24 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.379231 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #20: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:403}} -1711462110.379286 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:403 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:403) - deleting -1711462110.379311 [0] dq.builtin: => EVERYONE -1711462110.379362 [0] dq.builtin: delete -1711462110.379684 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.379732 [0] recv: HEARTBEAT(#9:20..20 L(:1c1 16958.039212)110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0: 110aba1:7b19badd:ce0adb73:3c7@20(sync)) -1711462110.379761 [0] tev: acknack 110aba1:7b19badd:ce0adb73:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#5:21/0: -1711462110.379779 [0] tev: send acknack(rd 110aba1:7b19badd:ce0adb73:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.379794 [0] tev: xpack_addmsg 0x770414001390 0x77041c049320 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.379810 [0] tev: writer_hbcontrol: wr 110aba1:7b19badd:ce0adb73:3c2 multicasting (rel-prd 6 seq-eq-max 6 seq 24 maxseq 23) -1711462110.379831 [0] tev: heartbeat(wr 110aba1:7b19badd:ce0adb73:3c2) sent, resched in 0.1 s (min-ack 23, avail-seq 24, xmit 24) -1711462110.379882 [0] tev: nn_xpack_send 64: 0x77041400139c:20 0x770424003d38:44 [ udp/10.101.12.71:7416@2 ] -1711462110.379897 [0] tev: traffic-xmit (1) 64 -1711462110.379918 [0] tev: xpack_addmsg 0x770414001390 0x770424003990 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.379918 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.379978 [0] gc: gc 0x653a1b29e680: deleting -1711462110.379982 [0] recv: INFOTS(1711462110.379581375) -1711462110.380019 [0] tev: nn_xpack_send 52: 0x77041400139c:20 0x770424003a88:32 [ udp/239.255.0.1:7400@2 ] -1711462110.380042 [0] tev: traffic-xmit (1) 52 -1711462110.380026 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #14 L(:1c1 16958.039482)) -1711462110.380064 [0] gc: gc_delete_reader(0x653a1b29e680, 110aba1:7b19badd:ce0adb73:504) -1711462110.380122 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.380146 [0] recv: HEARTBEAT(#8:24..24 110aba1:7b19badd:ce0adb73:3c2? -> 0:0:0:0) -1711462110.380159 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:504}} -1711462110.380148 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:504, 5): reader no longer exists -1711462110.380193 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:504 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:504) => EVERYONE -1711462110.380232 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:504, 3): reader no longer exists -1711462110.380269 [0] dq.builtin: - deleting -1711462110.380313 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.380329 [0] dq.builtin: delete -1711462110.380355 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.380335 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:504, 3): reader no longer exists -1711462110.380382 [0] recv: ACKNACK(F#5:25/0: L(:1c1 16958.039854) 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK1 RM0) -1711462110.380396 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:504, 4): reader no longer exists -1711462110.380415 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:504, 7): reader no longer exists -1711462110.380426 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.380434 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.380447 [0] recv: ACKNACK(F#5:25/0: L(:1c1 16958.039937) 110e21c:d0762c48:a9f0fb28:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK1 RM0) -1711462110.380449 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:4c2 #20: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:504}} -1711462110.380491 [0] gc: non-timed queue now has 1 items -1711462110.380556 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:504 @ 0x653a1b2c9994) user 4 builtin 12 -1711462110.380593 [0] gc: gc 0x77041c014c20: deleting -1711462110.380622 [0] gc: gc_delete_proxy_writer_dqueue(0x77041c014c20, 110443d:bb7f10a5:18901533:403) -1711462110.380646 [0] gc: gc 0x77041c052a60: deleting -1711462110.380574 [0] tev: xpack_addmsg 0x770414001390 0x77041c066c20 0(data(110aba1:7b19badd:ce0adb73:4c2:#20/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.380661 [0] gc: gc_delete_proxy_reader (0x77041c052a60, 110443d:bb7f10a5:18901533:504) -1711462110.380703 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.380735 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.380673 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x77041c014c20, 110443d:bb7f10a5:18901533:403) -1711462110.380737 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:207) ... -1711462110.380792 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x77041c033088:48 0x770424002d04:28 [ udp/239.255.0.1:7400@2 ] -1711462110.380797 [0] recv: ACKNACK(F#5:25/0: L(:1c1 16958.040234) 110443d:bb7f10a5:18901533:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK1 RM0) -1711462110.380838 [0] python3: => EVERYONE -1711462110.380916 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.380873 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=13 -1711462110.380967 [0] recv: INFOTS(1711462110.380434543) -1711462110.381008 [0] gc: gc 0x77041c014c20: deleting -1711462110.381032 [0] gc: gc_delete_proxy_writer (0x77041c014c20, 110443d:bb7f10a5:18901533:403) -1711462110.381019 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #20 110aba1:7b19badd:ce0adb73:4c2? -> 0:0:0:0) -1711462110.381050 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:504, 6): reader no longer exists -1711462110.380927 [0] tev: traffic-xmit (1) 96 -1711462110.381091 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=12 -1711462110.381109 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.381115 [0] gc: gc 0x77041000d490: not yet, shortsleep -1711462110.381122 [0] recv: INFODST(110aba1:7b19badd:ce0adb73) -1711462110.381173 [0] recv: ACKNACK(F#9:25/0: L(:1c1 16958.040623) 110cd0d:79d6eeac:ea4a8907:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK1 RM0) -1711462110.381188 [0] recv: INFOSRC(110cd0d:56a27910:57318171 vendor 1.16) -1711462110.381200 [0] recv: ACKNACK(F#5:25/0: L(:1c1 16958.040623) 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK1 RM0) -1711462110.381211 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.381231 [0] recv: ACKNACK(F#15:25/0: L(:1c1 16958.040623) 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110aba1:7b19badd:ce0adb73:3c2 ACK1 RM1) -1711462110.382211 [0] gc: gc 0x77041000d490: deleting -1711462110.382228 [0] gc: gc_delete_reader(0x77041000d490, 110aba1:7b19badd:ce0adb73:207) -1711462110.382253 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:207 @ 0x653a1b3fe704) user 3 builtin 12 -1711462110.382281 [0] gc: gc 0x653a1ae98420: not yet, shortsleep -1711462110.382379 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:307) ... -1711462110.382396 [0] python3: => EVERYONE -1711462110.383352 [0] gc: gc 0x653a1ae98420: deleting -1711462110.383364 [0] gc: gc 0x653a1b2d9610: deleting -1711462110.383372 [0] gc: gc 0x653a1b3a1360: deleting -1711462110.383378 [0] gc: gc 0x653a1b314040: deleting -1711462110.383385 [0] gc: gc 0x653a1b39cbc0: deleting -1711462110.383392 [0] gc: gc 0x653a1b339890: deleting -1711462110.383398 [0] gc: gc 0x653a1b387570: deleting -1711462110.383404 [0] gc: gc 0x653a1b392270: deleting -1711462110.383410 [0] gc: gc 0x653a1b386a30: deleting -1711462110.383417 [0] gc: gc 0x653a1b3de770: deleting -1711462110.383423 [0] gc: gc 0x653a1b38e4e0: deleting -1711462110.383429 [0] gc: gc 0x653a1b4586d0: deleting -1711462110.383435 [0] gc: gc 0x653a1b38d9d0: deleting -1711462110.383441 [0] gc: gc 0x653a1b39ea10: deleting -1711462110.383447 [0] gc: gc 0x653a1b2a04e0: deleting -1711462110.383453 [0] gc: gc 0x653a1b466af0: deleting -1711462110.383459 [0] gc: gc 0x653a1b3f7c50: deleting -1711462110.383465 [0] gc: gc 0x653a1b39e730: deleting -1711462110.383474 [0] gc: gc 0x653a1b48b850: deleting -1711462110.383480 [0] gc: gc 0x653a1b316180: deleting -1711462110.383487 [0] gc: gc 0x653a1b39bcc0: deleting -1711462110.383493 [0] gc: gc 0x653a1b2cf300: deleting -1711462110.383500 [0] gc: gc 0x653a1b380160: deleting -1711462110.383508 [0] gc: gc 0x653a1b45a150: deleting -1711462110.383515 [0] gc: gc 0x653a1b39c050: deleting -1711462110.383523 [0] gc: gc 0x653a1b459ab0: deleting -1711462110.383531 [0] gc: gc 0x653a1b3898f0: deleting -1711462110.383539 [0] gc: gc 0x653a1b389b20: deleting -1711462110.383546 [0] gc: gc 0x653a1b3831a0: deleting -1711462110.383553 [0] gc: gc 0x653a1b3833d0: deleting -1711462110.383561 [0] gc: gc 0x653a1b454930: deleting -1711462110.383569 [0] gc: gc 0x653a1b454b60: deleting -1711462110.383576 [0] gc: gc 0x653a1b45a730: deleting -1711462110.383583 [0] gc: gc 0x653a1b45a960: deleting -1711462110.383590 [0] gc: gc 0x653a1b39d5c0: deleting -1711462110.383598 [0] gc: gc 0x653a1b39d7f0: deleting -1711462110.383606 [0] gc: gc 0x653a1b390ea0: deleting -1711462110.383613 [0] gc: gc 0x653a1b3910d0: deleting -1711462110.383620 [0] gc: gc 0x653a1b2cb460: deleting -1711462110.383628 [0] gc: gc 0x653a1b2cb690: deleting -1711462110.383635 [0] gc: gc 0x653a1b3a0200: deleting -1711462110.383642 [0] gc: gc 0x653a1b3a0430: deleting -1711462110.383650 [0] gc: gc 0x653a1b3a0710: deleting -1711462110.383657 [0] gc: gc 0x653a1b3a0940: deleting -1711462110.383664 [0] gc: gc 0x653a1b495ff0: deleting -1711462110.383671 [0] gc: gc_delete_reader(0x653a1b495ff0, 110aba1:7b19badd:ce0adb73:307) -1711462110.383688 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:307 @ 0x653a1b3ea574) user 2 builtin 12 -1711462110.383701 [0] gc: gc 0x653a1b314040: not yet, shortsleep -1711462110.383770 [0] python3: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:107) ... -1711462110.383784 [0] python3: => EVERYONE -1711462110.384772 [0] gc: gc 0x653a1b314040: deleting -1711462110.384788 [0] gc: gc 0x653a1b3a1360: deleting -1711462110.384796 [0] gc: gc 0x653a1b39cbc0: deleting -1711462110.384803 [0] gc: gc 0x653a1b339890: deleting -1711462110.384811 [0] gc: gc 0x653a1b387570: deleting -1711462110.384821 [0] gc: gc 0x653a1b392270: deleting -1711462110.384829 [0] gc: gc 0x653a1b386a30: deleting -1711462110.384835 [0] gc: gc 0x653a1b3de770: deleting -1711462110.384842 [0] gc: gc 0x653a1b38e4e0: deleting -1711462110.384849 [0] gc: gc 0x653a1b4586d0: deleting -1711462110.384855 [0] gc: gc 0x653a1b38d9d0: deleting -1711462110.384864 [0] gc: gc 0x653a1b39ea10: deleting -1711462110.384872 [0] gc: gc 0x653a1b2a04e0: deleting -1711462110.384879 [0] gc: gc 0x653a1b466af0: deleting -1711462110.384885 [0] gc: gc 0x653a1b3f7c50: deleting -1711462110.384892 [0] gc: gc 0x653a1b495ff0: deleting -1711462110.384898 [0] gc: gc 0x653a1b39e730: deleting -1711462110.384905 [0] gc: gc 0x653a1b48b850: deleting -1711462110.384912 [0] gc: gc 0x653a1b316180: deleting -1711462110.384919 [0] gc: gc 0x653a1b46acd0: deleting -1711462110.384926 [0] gc: gc 0x653a1b39bcc0: deleting -1711462110.384933 [0] gc: gc 0x653a1b2cf300: deleting -1711462110.384939 [0] gc: gc 0x653a1b380160: deleting -1711462110.384947 [0] gc: gc 0x653a1b45a150: deleting -1711462110.384953 [0] gc: gc 0x653a1b39c050: deleting -1711462110.384960 [0] gc: gc 0x653a1b459ab0: deleting -1711462110.384966 [0] gc: gc 0x653a1b3831a0: deleting -1711462110.384972 [0] gc: gc 0x653a1b3833d0: deleting -1711462110.384979 [0] gc: gc 0x653a1b454930: deleting -1711462110.384986 [0] gc: gc 0x653a1b454b60: deleting -1711462110.384992 [0] gc: gc 0x653a1b45a730: deleting -1711462110.384998 [0] gc: gc 0x653a1b45a960: deleting -1711462110.385006 [0] gc: gc 0x653a1b39d5c0: deleting -1711462110.385012 [0] gc: gc 0x653a1b39d7f0: deleting -1711462110.385018 [0] gc: gc 0x653a1b390ea0: deleting -1711462110.385025 [0] gc: gc 0x653a1b3910d0: deleting -1711462110.385031 [0] gc: gc 0x653a1b3898f0: deleting -1711462110.385037 [0] gc: gc 0x653a1b389b20: deleting -1711462110.385044 [0] gc: gc 0x653a1b2cb460: deleting -1711462110.385050 [0] gc: gc 0x653a1b2cb690: deleting -1711462110.385058 [0] gc: gc 0x653a1b3a0200: deleting -1711462110.385064 [0] gc: gc 0x653a1b3a0430: deleting -1711462110.385071 [0] gc: gc 0x653a1b3a0710: deleting -1711462110.385078 [0] gc: gc 0x653a1b3a0940: deleting -1711462110.385085 [0] gc: gc 0x653a1b45b310: deleting -1711462110.385092 [0] gc: gc 0x653a1b45b540: deleting -1711462110.385099 [0] gc: gc 0x653a1b39ee50: deleting -1711462110.385107 [0] gc: gc 0x653a1b39f080: deleting -1711462110.385113 [0] gc: gc 0x653a1b2c97f0: deleting -1711462110.385119 [0] gc: gc 0x653a1b2c9a20: deleting -1711462110.385125 [0] gc: gc 0x653a1b2c9c50: deleting -1711462110.385132 [0] gc: gc 0x653a1b443ed0: deleting -1711462110.385138 [0] gc: gc_delete_reader(0x653a1b443ed0, 110aba1:7b19badd:ce0adb73:107) -1711462110.385156 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:107 @ 0x653a1b295fa4) user 1 builtin 12 -1711462110.385168 [0] gc: gc 0x653a1b314040: deleting -1711462110.385183 [0] python3: ddsi_delete_participant (110aba1:7b19badd:ce0adb73:1c1) -1711462110.385193 [0] python3: => EVERYONE -1711462110.385215 [0] gc: gc 0x653a1b485a00: deleting -1711462110.385227 [0] gc: gc_delete_participant (0x653a1b485a00, 110aba1:7b19badd:ce0adb73:1c1) -1711462110.385219 [0] python3: trigger_recv_threads: 0 many 0x653a1b314bf0 -1711462110.385238 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 0:0:0:0 @ (nil)) user 0 builtin 12 -1711462110.385264 [0] python3: trigger_recv_threads: 1 single udp/239.255.0.1:7401 -1711462110.385285 [0] gc: write_sample 110aba1:7b19badd:ce0adb73:100c2 #2: ST3 DCPSParticipant/ParticipantBuiltinTopicData:{participant_guid={110aba1:7b19badd:ce0adb73:1c1}} -1711462110.385301 [0] recv: done -1711462110.385315 [0] recvMC: done -1711462110.385323 [0] gc: non-timed queue now has 1 items -1711462110.385332 [0] python3: trigger_recv_threads: 2 single udp/10.101.12.71:7419 -1711462110.385336 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:100c2) ... -1711462110.385351 [0] tev: xpack_addmsg 0x770414001390 0x77041c030030 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.385355 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:100c2) ... -1711462110.385382 [0] gc: writer_set_state(110aba1:7b19badd:ce0adb73:100c2) state transition 0 -> 3 -1711462110.385385 [0] recvUC: done -1711462110.385390 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:3c2) ... -1711462110.385412 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:3c2) ... -1711462110.385421 [0] gc: writer_set_state(110aba1:7b19badd:ce0adb73:3c2) state transition 0 -> 3 -1711462110.385429 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:4c2) ... -1711462110.385436 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:4c2) ... -1711462110.385446 [0] gc: writer_set_state(110aba1:7b19badd:ce0adb73:4c2) state transition 0 -> 3 -1711462110.385629 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:200c2) ... -1711462110.385638 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:200c2) ... -1711462110.385652 [0] gc: writer_set_state(110aba1:7b19badd:ce0adb73:200c2) state transition 0 -> 3 -1711462110.385660 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:100c7) ... -1711462110.385671 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:2c2) - unknown guid -1711462110.385681 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:2c7) - unknown guid -1711462110.385691 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:3c7) ... -1711462110.385704 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:4c7) ... -1711462110.385725 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:200c7) ... -1711462110.385735 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:300c3) ... -1711462110.385743 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:300c3) ... -1711462110.385751 [0] gc: writer_set_state(110aba1:7b19badd:ce0adb73:300c3) state transition 0 -> 3 -1711462110.385770 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:301c3) ... -1711462110.385780 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:301c3) ... -1711462110.385788 [0] gc: writer_set_state(110aba1:7b19badd:ce0adb73:301c3) state transition 0 -> 3 -1711462110.385796 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:300c4) ... -1711462110.385813 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:301c4) ... -1711462110.385823 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:ff0003c2) - unknown guid -1711462110.385829 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:ff0003c7) - unknown guid -1711462110.385835 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:ff0004c2) - unknown guid -1711462110.385850 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:ff0004c7) - unknown guid -1711462110.385858 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:ff0200c2) - unknown guid -1711462110.385865 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:ff0200c7) - unknown guid -1711462110.385871 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:201c3) - unknown guid -1711462110.385878 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:201c4) - unknown guid -1711462110.385886 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:ff0101c2) - unknown guid -1711462110.385893 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:ff0101c7) - unknown guid -1711462110.385899 [0] gc: ddsi_delete_writer_nolinger(guid 110aba1:7b19badd:ce0adb73:ff0202c3) - unknown guid -1711462110.385909 [0] gc: delete_reader_guid(guid 110aba1:7b19badd:ce0adb73:ff0202c4) - unknown guid -1711462110.385915 [0] gc: gc 0x653a1b314040: not yet, shortsleep -1711462110.386668 [0] tev: nn_xpack_send 96: 0x77041400139c:20 0x77041c01d2b8:48 0x7704240028e4:28 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.0.(trunc) -1711462110.386683 [0] tev: traffic-xmit (101) 96 -1711462110.386793 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:56a27910:57318171:1c1) - deleting -1711462110.386806 [0] python3: => EVERYONE -1711462110.386816 [0] python3: delete_ppt(110cd0d:56a27910:57318171:1c1) - deleting dependent proxy participants -1711462110.386830 [0] python3: delete_ppt(110cd0d:56a27910:57318171:1c1) - deleting endpoints -1711462110.386839 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1604) => EVERYONE -1711462110.386848 [0] python3: - deleting -1711462110.386858 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1504) => EVERYONE -1711462110.386867 [0] python3: - deleting -1711462110.386875 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1404) => EVERYONE -1711462110.386890 [0] python3: - deleting -1711462110.386906 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1304) => EVERYONE -1711462110.386913 [0] python3: - deleting -1711462110.386922 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1104) => EVERYONE -1711462110.386933 [0] python3: - deleting -1711462110.386951 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:f04) => EVERYONE -1711462110.386965 [0] python3: - deleting -1711462110.386974 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:d04) => EVERYONE -1711462110.386983 [0] gc: gc 0x653a1b314040: deleting -1711462110.386999 [0] gc: gc_delete_writer(0x653a1b314040, 110aba1:7b19badd:ce0adb73:100c2) -1711462110.386986 [0] python3: - deleting -1711462110.387017 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:100c2 @ 0x653a1b3e3734) user 0 builtin 11 -1711462110.387030 [0] gc: gc 0x77041c0123e0: deleting -1711462110.387038 [0] gc: gc_delete_writer(0x77041c0123e0, 110aba1:7b19badd:ce0adb73:3c2) -1711462110.387021 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:b04) => EVERYONE -1711462110.387062 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:3c2 @ 0x653a1b3e4134) user 0 builtin 10 -1711462110.387076 [0] gc: gc 0x77041c00fb20: deleting -1711462110.387089 [0] gc: gc_delete_writer(0x77041c00fb20, 110aba1:7b19badd:ce0adb73:4c2) -1711462110.387065 [0] python3: - deleting -1711462110.387114 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:4c2 @ 0x653a1b3e3c14) user 0 builtin 9 -1711462110.387120 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:904) => EVERYONE -1711462110.387129 [0] gc: gc 0x77041c00b2e0: deleting -1711462110.387142 [0] python3: - deleting -1711462110.387146 [0] gc: gc_delete_writer(0x77041c00b2e0, 110aba1:7b19badd:ce0adb73:200c2) -1711462110.387155 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:504) => EVERYONE -1711462110.387183 [0] python3: - deleting -1711462110.387184 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:200c2 @ 0x653a1b3e4714) user 0 builtin 8 -1711462110.387204 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:1203) - deleting -1711462110.387220 [0] gc: gc 0x77041c006ae0: deleting -1711462110.387231 [0] gc: gc_delete_reader(0x77041c006ae0, 110aba1:7b19badd:ce0adb73:100c7) -1711462110.387225 [0] python3: => EVERYONE -1711462110.387252 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:100c7 @ 0x653a1af2dee4) user 0 builtin 7 -1711462110.387259 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:1003) - deleting -1711462110.387275 [0] python3: => EVERYONE -1711462110.387264 [0] gc: gc 0x77041c005430: deleting -1711462110.387309 [0] gc: gc_delete_reader(0x77041c005430, 110aba1:7b19badd:ce0adb73:3c7) -1711462110.387301 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:e03) - deleting -1711462110.387332 [0] python3: => EVERYONE -1711462110.387323 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:3c7, 5): reader no longer exists -1711462110.387345 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:c03) - deleting -1711462110.387355 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:3c7, 3): reader no longer exists -1711462110.387366 [0] python3: => EVERYONE -1711462110.387380 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:a03) - deleting -1711462110.387384 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:3c7, 3): reader no longer exists -1711462110.387393 [0] python3: => EVERYONE -1711462110.387408 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:3c7, 6): reader no longer exists -1711462110.387413 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:803) - deleting -1711462110.387428 [0] python3: => EVERYONE -1711462110.387419 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:3c7, 4): reader no longer exists -1711462110.387444 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:703) - deleting -1711462110.387457 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:3c7, 6): reader no longer exists -1711462110.387469 [0] python3: => EVERYONE -1711462110.387485 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:403) - deleting -1711462110.387489 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:3c7 @ 0x653a1af94554) user 0 builtin 6 -1711462110.387496 [0] python3: => EVERYONE -1711462110.387510 [0] gc: gc 0x653a1b47e890: deleting -1711462110.387519 [0] gc: gc_delete_reader(0x653a1b47e890, 110aba1:7b19badd:ce0adb73:4c7) -1711462110.387521 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:301c4) - deleting -1711462110.387545 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:301c3) - deleting -1711462110.387535 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:4c7, 5): reader no longer exists -1711462110.387565 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:300c4) - deleting -1711462110.387566 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:4c7, 3): reader no longer exists -1711462110.387580 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:300c3) - deleting -1711462110.387597 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:4c7, 3): reader no longer exists -1711462110.387615 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:200c7) - deleting -1711462110.387624 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:4c7, 5): reader no longer exists -1711462110.387637 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:200c2) - deleting -1711462110.387650 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:4c7, 3): reader no longer exists -1711462110.387667 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:4c7) - deleting -1711462110.387677 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:4c7, 5): reader no longer exists -1711462110.387688 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:4c2) - deleting -1711462110.387702 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:4c7 @ 0x653a1af72b04) user 0 builtin 5 -1711462110.387716 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:3c7) - deleting -1711462110.387728 [0] gc: gc 0x770424003dd0: deleting -1711462110.387753 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:3c2) - deleting -1711462110.387755 [0] gc: gc_delete_reader(0x770424003dd0, 110aba1:7b19badd:ce0adb73:200c7) -1711462110.387766 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:100c7) - deleting -1711462110.387782 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:200c7, 5): reader no longer exists -1711462110.387795 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting -1711462110.387807 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:200c7, 6): reader no longer exists -1711462110.387816 [0] python3: => EVERYONE -1711462110.387830 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:200c7, 6): reader no longer exists -1711462110.387834 [0] python3: delete_ppt(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting dependent proxy participants -1711462110.387847 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:200c7, 6): reader no longer exists -1711462110.387859 [0] python3: delete_ppt(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting endpoints -1711462110.387874 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:200c7, 5): reader no longer exists -1711462110.387886 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1604) => EVERYONE -1711462110.387898 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:200c7 @ 0x653a1af86bb4) user 0 builtin 4 -1711462110.387912 [0] python3: - deleting -1711462110.387926 [0] gc: gc 0x770424004000: deleting -1711462110.387945 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1504) => EVERYONE -1711462110.387947 [0] gc: gc_delete_writer(0x770424004000, 110aba1:7b19badd:ce0adb73:300c3) -1711462110.387959 [0] python3: - deleting -1711462110.387977 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1404) => EVERYONE -1711462110.387981 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:300c3 @ 0x653a1b3e4ea4) user 0 builtin 3 -1711462110.387989 [0] python3: - deleting -1711462110.388003 [0] gc: gc 0x770424004230: deleting -1711462110.388016 [0] gc: gc_delete_writer(0x770424004230, 110aba1:7b19badd:ce0adb73:301c3) -1711462110.388011 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1304) => EVERYONE -1711462110.388037 [0] python3: - deleting -1711462110.388045 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:301c3 @ 0x653a1b484114) user 0 builtin 2 -1711462110.388059 [0] gc: gc 0x770424004460: deleting -1711462110.388072 [0] gc: gc_delete_reader(0x770424004460, 110aba1:7b19badd:ce0adb73:300c4) -1711462110.388053 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1104) => EVERYONE -1711462110.388089 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:300c4, 3): reader no longer exists -1711462110.388134 [0] python3: - deleting -1711462110.388147 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:300c4, 2): reader no longer exists -1711462110.388164 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:f04) => EVERYONE -1711462110.388198 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:300c4, 2): reader no longer exists -1711462110.388200 [0] python3: - deleting -1711462110.388208 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:300c4, 3): reader no longer exists -1711462110.388224 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:d04) => EVERYONE -1711462110.388226 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:300c4, 2): reader no longer exists -1711462110.388235 [0] python3: - deleting -1711462110.388246 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:300c4 @ 0x653a1af22f84) user 0 builtin 1 -1711462110.388258 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:b04) => EVERYONE -1711462110.388264 [0] gc: gc 0x770424004690: deleting -1711462110.388271 [0] python3: - deleting -1711462110.388281 [0] gc: gc_delete_reader(0x770424004690, 110aba1:7b19badd:ce0adb73:301c4) -1711462110.388290 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:301c4, 3): reader no longer exists -1711462110.388296 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:904) => EVERYONE -1711462110.388298 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:301c4, 2): reader no longer exists -1711462110.388307 [0] python3: - deleting -1711462110.388317 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:301c4, 2): reader no longer exists -1711462110.388327 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:301c4, 3): reader no longer exists -1711462110.388334 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:504) => EVERYONE -1711462110.388338 [0] gc: ddsi_update_reader_init_acknack_count (110aba1:7b19badd:ce0adb73:301c4, 2): reader no longer exists -1711462110.388364 [0] python3: - deleting -1711462110.388434 [0] gc: ddsi_unref_participant(110aba1:7b19badd:ce0adb73:1c1 @ 0x653a1aecced0 <- 110aba1:7b19badd:ce0adb73:301c4 @ 0x653a1aeda794) user 0 builtin 0 -1711462110.388448 [0] gc: ddsi_remove_deleted_participant_guid(110aba1:7b19badd:ce0adb73:1c1 for_what=1) -1711462110.388454 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:1203) - deleting -1711462110.388456 [0] gc: gc 0x653a1b4855a0: not yet, shortsleep -1711462110.388469 [0] python3: => EVERYONE -1711462110.388488 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:1003) - deleting -1711462110.388518 [0] python3: => EVERYONE -1711462110.388528 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:e03) - deleting -1711462110.388534 [0] python3: => EVERYONE -1711462110.388547 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:c03) - deleting -1711462110.388554 [0] python3: => EVERYONE -1711462110.388561 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:a03) - deleting -1711462110.388568 [0] python3: => EVERYONE -1711462110.388577 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:803) - deleting -1711462110.388596 [0] python3: => EVERYONE -1711462110.388605 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:703) - deleting -1711462110.388615 [0] python3: => EVERYONE -1711462110.388625 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:403) - deleting -1711462110.388634 [0] python3: => EVERYONE -1711462110.388645 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:301c4) - deleting -1711462110.388652 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:301c3) - deleting -1711462110.388659 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:300c4) - deleting -1711462110.388664 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:300c3) - deleting -1711462110.388675 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:200c7) - deleting -1711462110.388684 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:200c2) - deleting -1711462110.388695 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:4c7) - deleting -1711462110.388704 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:4c2) - deleting -1711462110.388711 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:3c7) - deleting -1711462110.388716 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:3c2) - deleting -1711462110.388723 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:100c7) - deleting -1711462110.388731 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:79d6eeac:ea4a8907:1c1) - deleting -1711462110.388736 [0] python3: => EVERYONE -1711462110.388746 [0] python3: delete_ppt(110cd0d:79d6eeac:ea4a8907:1c1) - deleting dependent proxy participants -1711462110.388754 [0] python3: delete_ppt(110cd0d:79d6eeac:ea4a8907:1c1) - deleting endpoints -1711462110.388761 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1604) => EVERYONE -1711462110.388768 [0] python3: - deleting -1711462110.388776 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1504) => EVERYONE -1711462110.388781 [0] python3: - deleting -1711462110.388786 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1404) => EVERYONE -1711462110.388794 [0] python3: - deleting -1711462110.388798 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1304) => EVERYONE -1711462110.388803 [0] python3: - deleting -1711462110.388807 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1104) => EVERYONE -1711462110.388812 [0] python3: - deleting -1711462110.388817 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:f04) => EVERYONE -1711462110.388822 [0] python3: - deleting -1711462110.388829 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:d04) => EVERYONE -1711462110.388833 [0] python3: - deleting -1711462110.388839 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:b04) => EVERYONE -1711462110.388843 [0] python3: - deleting -1711462110.388851 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:904) => EVERYONE -1711462110.388855 [0] python3: - deleting -1711462110.388862 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:504) => EVERYONE -1711462110.388866 [0] python3: - deleting -1711462110.388877 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:1203) - deleting -1711462110.388882 [0] python3: => EVERYONE -1711462110.388889 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:1003) - deleting -1711462110.388894 [0] python3: => EVERYONE -1711462110.388901 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:e03) - deleting -1711462110.388906 [0] python3: => EVERYONE -1711462110.388912 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:c03) - deleting -1711462110.388917 [0] python3: => EVERYONE -1711462110.388924 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:a03) - deleting -1711462110.388928 [0] python3: => EVERYONE -1711462110.388934 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:803) - deleting -1711462110.388939 [0] python3: => EVERYONE -1711462110.388945 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:703) - deleting -1711462110.388949 [0] python3: => EVERYONE -1711462110.388955 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:403) - deleting -1711462110.388960 [0] python3: => EVERYONE -1711462110.388967 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:301c4) - deleting -1711462110.388976 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:301c3) - deleting -1711462110.388982 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:300c4) - deleting -1711462110.388986 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:300c3) - deleting -1711462110.388994 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:200c7) - deleting -1711462110.388999 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:200c2) - deleting -1711462110.389005 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:4c7) - deleting -1711462110.389011 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:4c2) - deleting -1711462110.389017 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:3c7) - deleting -1711462110.389021 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:3c2) - deleting -1711462110.389028 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:100c7) - deleting -1711462110.389035 [0] python3: ddsi_delete_proxy_participant_by_guid(110e21c:d0762c48:a9f0fb28:1c1) - deleting -1711462110.389038 [0] python3: => EVERYONE -1711462110.389044 [0] python3: delete_ppt(110e21c:d0762c48:a9f0fb28:1c1) - deleting dependent proxy participants -1711462110.389049 [0] python3: delete_ppt(110e21c:d0762c48:a9f0fb28:1c1) - deleting endpoints -1711462110.389054 [0] python3: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:504) => EVERYONE -1711462110.389060 [0] python3: - deleting -1711462110.389064 [0] python3: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:403) - deleting -1711462110.389071 [0] python3: => EVERYONE -1711462110.389079 [0] python3: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:301c4) - deleting -1711462110.389084 [0] python3: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:301c3) - deleting -1711462110.389090 [0] python3: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:300c4) - deleting -1711462110.389095 [0] python3: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:300c3) - deleting -1711462110.389101 [0] python3: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:200c7) - deleting -1711462110.389106 [0] python3: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:200c2) - deleting -1711462110.389113 [0] python3: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:4c7) - deleting -1711462110.389118 [0] python3: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:4c2) - deleting -1711462110.389124 [0] python3: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:3c7) - deleting -1711462110.389128 [0] python3: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:3c2) - deleting -1711462110.389134 [0] python3: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:100c7) - deleting -1711462110.389142 [0] python3: ddsi_delete_proxy_participant_by_guid(110443d:bb7f10a5:18901533:1c1) - deleting -1711462110.389148 [0] python3: => EVERYONE -1711462110.389156 [0] python3: delete_ppt(110443d:bb7f10a5:18901533:1c1) - deleting dependent proxy participants -1711462110.389165 [0] python3: delete_ppt(110443d:bb7f10a5:18901533:1c1) - deleting endpoints -1711462110.389171 [0] python3: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:301c4) - deleting -1711462110.389175 [0] python3: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:301c3) - deleting -1711462110.389183 [0] python3: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:300c4) - deleting -1711462110.389188 [0] python3: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:300c3) - deleting -1711462110.389195 [0] python3: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:200c7) - deleting -1711462110.389199 [0] python3: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:200c2) - deleting -1711462110.389206 [0] python3: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:4c7) - deleting -1711462110.389210 [0] python3: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:4c2) - deleting -1711462110.389221 [0] python3: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:3c7) - deleting -1711462110.389225 [0] python3: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:3c2) - deleting -1711462110.389232 [0] python3: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:100c7) - deleting -1711462110.389238 [0] python3: ddsi_delete_proxy_participant_by_guid(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting -1711462110.389242 [0] python3: => EVERYONE -1711462110.389247 [0] python3: delete_ppt(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting dependent proxy participants -1711462110.389253 [0] python3: delete_ppt(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting endpoints -1711462110.389259 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:504) => EVERYONE -1711462110.389264 [0] python3: - deleting -1711462110.389271 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:403) - deleting -1711462110.389276 [0] python3: => EVERYONE -1711462110.389283 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:301c4) - deleting -1711462110.389288 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:301c3) - deleting -1711462110.389293 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:300c4) - deleting -1711462110.389299 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:300c3) - deleting -1711462110.389304 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:200c7) - deleting -1711462110.389309 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:200c2) - deleting -1711462110.389315 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4c7) - deleting -1711462110.389320 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4c2) - deleting -1711462110.389327 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3c7) - deleting -1711462110.389333 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3c2) - deleting -1711462110.389339 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:100c7) - deleting -1711462110.389553 [0] gc: gc 0x653a1b4855a0: deleting -1711462110.389573 [0] gc: gc_delete_proxy_reader (0x653a1b4855a0, 110cd0d:56a27910:57318171:1604) -1711462110.389582 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=29 -1711462110.389588 [0] gc: gc 0x653a1b4857d0: deleting -1711462110.389595 [0] gc: gc_delete_proxy_reader (0x653a1b4857d0, 110cd0d:56a27910:57318171:1504) -1711462110.389599 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=28 -1711462110.389605 [0] gc: gc 0x653a1b485a00: deleting -1711462110.389610 [0] gc: gc_delete_proxy_reader (0x653a1b485a00, 110cd0d:56a27910:57318171:1404) -1711462110.389615 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=27 -1711462110.389621 [0] gc: gc 0x653a1b3a1360: deleting -1711462110.389625 [0] gc: gc_delete_proxy_reader (0x653a1b3a1360, 110cd0d:56a27910:57318171:1304) -1711462110.389631 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=26 -1711462110.389638 [0] gc: gc 0x653a1b39cbc0: deleting -1711462110.389644 [0] gc: gc_delete_proxy_reader (0x653a1b39cbc0, 110cd0d:56a27910:57318171:1104) -1711462110.389651 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=25 -1711462110.389658 [0] gc: gc 0x653a1b339890: deleting -1711462110.389661 [0] gc: gc_delete_proxy_reader (0x653a1b339890, 110cd0d:56a27910:57318171:f04) -1711462110.389669 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=24 -1711462110.389676 [0] gc: gc 0x653a1b387570: deleting -1711462110.389683 [0] gc: gc_delete_proxy_reader (0x653a1b387570, 110cd0d:56a27910:57318171:d04) -1711462110.389689 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=23 -1711462110.389695 [0] gc: gc 0x653a1b392270: deleting -1711462110.389700 [0] gc: gc_delete_proxy_reader (0x653a1b392270, 110cd0d:56a27910:57318171:b04) -1711462110.389705 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=22 -1711462110.389711 [0] gc: gc 0x653a1b386a30: deleting -1711462110.389717 [0] gc: gc_delete_proxy_reader (0x653a1b386a30, 110cd0d:56a27910:57318171:904) -1711462110.389721 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=21 -1711462110.389728 [0] gc: gc 0x653a1b3de770: deleting -1711462110.389732 [0] gc: gc_delete_proxy_reader (0x653a1b3de770, 110cd0d:56a27910:57318171:504) -1711462110.389737 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=20 -1711462110.389745 [0] gc: gc 0x653a1b38e4e0: deleting -1711462110.389750 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b38e4e0, 110cd0d:56a27910:57318171:1203) -1711462110.389758 [0] gc: gc 0x653a1b4586d0: deleting -1711462110.389763 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b4586d0, 110cd0d:56a27910:57318171:1003) -1711462110.389770 [0] gc: gc 0x653a1b38d9d0: deleting -1711462110.389774 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b38d9d0, 110cd0d:56a27910:57318171:e03) -1711462110.389791 [0] gc: gc 0x653a1b39ea10: deleting -1711462110.389795 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b39ea10, 110cd0d:56a27910:57318171:c03) -1711462110.389800 [0] gc: gc 0x653a1b2a04e0: deleting -1711462110.389805 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b2a04e0, 110cd0d:56a27910:57318171:a03) -1711462110.389813 [0] gc: gc 0x653a1b466af0: deleting -1711462110.389819 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b466af0, 110cd0d:56a27910:57318171:803) -1711462110.389806 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b38e4e0, 110cd0d:56a27910:57318171:1203) -1711462110.389825 [0] gc: gc 0x653a1b3f7c50: deleting -1711462110.389844 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b3f7c50, 110cd0d:56a27910:57318171:703) -1711462110.389850 [0] gc: gc 0x653a1b495ff0: deleting -1711462110.389854 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b495ff0, 110cd0d:56a27910:57318171:403) -1711462110.389863 [0] gc: gc 0x653a1b443ed0: deleting -1711462110.389869 [0] gc: gc_delete_proxy_reader (0x653a1b443ed0, 110cd0d:56a27910:57318171:301c4) -1711462110.389876 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=19 -1711462110.389883 [0] gc: gc 0x653a1b31b350: deleting -1711462110.389888 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b31b350, 110cd0d:56a27910:57318171:301c3) -1711462110.389838 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b4586d0, 110cd0d:56a27910:57318171:1003) -1711462110.389895 [0] gc: gc 0x653a1b39e730: deleting -1711462110.389908 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b31b350, 110cd0d:56a27910:57318171:301c3) -1711462110.389914 [0] gc: gc_delete_proxy_reader (0x653a1b39e730, 110cd0d:56a27910:57318171:300c4) -1711462110.389907 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b38d9d0, 110cd0d:56a27910:57318171:e03) -1711462110.389938 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=18 -1711462110.389955 [0] gc: gc 0x653a1b3e3730: deleting -1711462110.389966 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b3e3730, 110cd0d:56a27910:57318171:300c3) -1711462110.389948 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b39ea10, 110cd0d:56a27910:57318171:c03) -1711462110.389974 [0] gc: gc 0x653a1b3e4130: deleting -1711462110.389981 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b3e3730, 110cd0d:56a27910:57318171:300c3) -1711462110.389999 [0] gc: gc_delete_proxy_reader (0x653a1b3e4130, 110cd0d:56a27910:57318171:200c7) -1711462110.390006 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=17 -1711462110.389988 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b2a04e0, 110cd0d:56a27910:57318171:a03) -1711462110.390010 [0] gc: gc 0x653a1b3e3c10: deleting -1711462110.390029 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b3e3c10, 110cd0d:56a27910:57318171:200c2) -1711462110.390021 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b466af0, 110cd0d:56a27910:57318171:803) -1711462110.390036 [0] gc: gc 0x653a1b3e4710: deleting -1711462110.390050 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b3f7c50, 110cd0d:56a27910:57318171:703) -1711462110.390053 [0] gc: gc_delete_proxy_reader (0x653a1b3e4710, 110cd0d:56a27910:57318171:4c7) -1711462110.390061 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b495ff0, 110cd0d:56a27910:57318171:403) -1711462110.390076 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=16 -1711462110.390085 [0] gc: gc 0x653a1b48b850: deleting -1711462110.390093 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b48b850, 110cd0d:56a27910:57318171:4c2) -1711462110.390043 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b3e3c10, 110cd0d:56a27910:57318171:200c2) -1711462110.390098 [0] gc: gc 0x653a1b458330: deleting -1711462110.390113 [0] gc: gc_delete_proxy_reader (0x653a1b458330, 110cd0d:56a27910:57318171:3c7) -1711462110.390108 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b48b850, 110cd0d:56a27910:57318171:4c2) -1711462110.390119 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=15 -1711462110.390132 [0] gc: gc 0x653a1b47ab40: deleting -1711462110.390137 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b47ab40, 110cd0d:56a27910:57318171:3c2) -1711462110.390143 [0] gc: gc 0x653a1b46acd0: deleting -1711462110.390146 [0] gc: gc_delete_proxy_reader (0x653a1b46acd0, 110cd0d:56a27910:57318171:100c7) -1711462110.390149 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b47ab40, 110cd0d:56a27910:57318171:3c2) -1711462110.390151 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=14 -1711462110.390165 [0] gc: gc 0x653a1b39bcc0: deleting -1711462110.390171 [0] gc: gc_delete_proxy_participant(0x653a1b39bcc0, 110cd0d:56a27910:57318171:1c1) -1711462110.390176 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=13 -1711462110.390180 [0] gc: gc 0x653a1b2cf300: deleting -1711462110.390184 [0] gc: gc_delete_proxy_reader (0x653a1b2cf300, 110cd0d:e3b81b8d:1ccb65b1:1604) -1711462110.390188 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=29 -1711462110.390194 [0] gc: gc 0x653a1b3e6c30: deleting -1711462110.390198 [0] gc: gc_delete_proxy_reader (0x653a1b3e6c30, 110cd0d:e3b81b8d:1ccb65b1:1504) -1711462110.390202 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=28 -1711462110.390207 [0] gc: gc 0x653a1b3e4ea0: deleting -1711462110.390212 [0] gc: gc_delete_proxy_reader (0x653a1b3e4ea0, 110cd0d:e3b81b8d:1ccb65b1:1404) -1711462110.390216 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=27 -1711462110.390222 [0] gc: gc 0x653a1b380160: deleting -1711462110.390226 [0] gc: gc_delete_proxy_reader (0x653a1b380160, 110cd0d:e3b81b8d:1ccb65b1:1304) -1711462110.390230 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=26 -1711462110.390235 [0] gc: gc 0x653a1b484110: deleting -1711462110.390240 [0] gc: gc_delete_proxy_reader (0x653a1b484110, 110cd0d:e3b81b8d:1ccb65b1:1104) -1711462110.390243 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=25 -1711462110.390249 [0] gc: gc 0x653a1b39c050: deleting -1711462110.390254 [0] gc: gc_delete_proxy_reader (0x653a1b39c050, 110cd0d:e3b81b8d:1ccb65b1:f04) -1711462110.390260 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=24 -1711462110.390265 [0] gc: gc 0x653a1b459ab0: deleting -1711462110.390269 [0] gc: gc_delete_proxy_reader (0x653a1b459ab0, 110cd0d:e3b81b8d:1ccb65b1:d04) -1711462110.390272 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=23 -1711462110.390277 [0] gc: gc 0x653a1b455f00: deleting -1711462110.390281 [0] gc: gc_delete_proxy_reader (0x653a1b455f00, 110cd0d:e3b81b8d:1ccb65b1:b04) -1711462110.390285 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=22 -1711462110.390289 [0] gc: gc 0x653a1b45a150: deleting -1711462110.390293 [0] gc: gc_delete_proxy_reader (0x653a1b45a150, 110cd0d:e3b81b8d:1ccb65b1:904) -1711462110.390297 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=21 -1711462110.390302 [0] gc: gc 0x653a1b47e890: deleting -1711462110.390307 [0] gc: gc 0x653a1b316060: deleting -1711462110.390311 [0] gc: gc_delete_proxy_reader (0x653a1b316060, 110cd0d:e3b81b8d:1ccb65b1:504) -1711462110.390315 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=20 -1711462110.390320 [0] gc: gc 0x653a1b454930: deleting -1711462110.390324 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b454930, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.390329 [0] gc: gc 0x653a1b454b60: deleting -1711462110.390333 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b454b60, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.390336 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b454930, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.390337 [0] gc: gc 0x653a1b45a730: deleting -1711462110.390353 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b45a730, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.390360 [0] gc: gc 0x653a1b45a960: deleting -1711462110.390366 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b45a960, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.390372 [0] gc: gc 0x653a1b39d5c0: deleting -1711462110.390347 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b454b60, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.390376 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b39d5c0, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.390392 [0] gc: gc 0x653a1b39d7f0: deleting -1711462110.390398 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b39d7f0, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.390386 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b45a730, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.390403 [0] gc: gc 0x653a1b390ea0: deleting -1711462110.390418 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b390ea0, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.390424 [0] gc: gc 0x653a1b3910d0: deleting -1711462110.390430 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b3910d0, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.390435 [0] gc: gc 0x653a1b3898f0: deleting -1711462110.390442 [0] gc: gc_delete_proxy_reader (0x653a1b3898f0, 110cd0d:e3b81b8d:1ccb65b1:301c4) -1711462110.390412 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b45a960, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.390447 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=19 -1711462110.390456 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b39d5c0, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.390463 [0] gc: gc 0x653a1b389b20: deleting -1711462110.390476 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b39d7f0, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.390478 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b389b20, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.390485 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b390ea0, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.390495 [0] gc: gc 0x653a1b3831a0: deleting -1711462110.390501 [0] gc: gc_delete_proxy_reader (0x653a1b3831a0, 110cd0d:e3b81b8d:1ccb65b1:300c4) -1711462110.390508 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=18 -1711462110.390514 [0] gc: gc 0x653a1b3833d0: deleting -1711462110.390520 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b3833d0, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.390524 [0] gc: gc 0x653a1b2cb460: deleting -1711462110.390502 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b389b20, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.390528 [0] gc: gc_delete_proxy_reader (0x653a1b2cb460, 110cd0d:e3b81b8d:1ccb65b1:200c7) -1711462110.390537 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=17 -1711462110.390543 [0] gc: gc 0x653a1b2cb690: deleting -1711462110.390546 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b3833d0, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.390527 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x653a1b3910d0, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.390549 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b2cb690, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.390569 [0] gc: gc 0x653a1b3a0200: deleting -1711462110.390574 [0] gc: gc_delete_proxy_reader (0x653a1b3a0200, 110cd0d:e3b81b8d:1ccb65b1:4c7) -1711462110.390576 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b2cb690, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.390578 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=16 -1711462110.390592 [0] gc: gc 0x653a1b3a0430: deleting -1711462110.390597 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b3a0430, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.390603 [0] gc: gc 0x653a1b45b310: deleting -1711462110.390607 [0] gc: gc_delete_proxy_reader (0x653a1b45b310, 110cd0d:e3b81b8d:1ccb65b1:3c7) -1711462110.390609 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b3a0430, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.390611 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=15 -1711462110.390625 [0] gc: gc 0x653a1b45b540: deleting -1711462110.390629 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b45b540, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.390635 [0] gc: gc 0x653a1b3a0710: deleting -1711462110.390639 [0] gc: gc_delete_proxy_reader (0x653a1b3a0710, 110cd0d:e3b81b8d:1ccb65b1:100c7) -1711462110.390642 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x653a1b45b540, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.390644 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=14 -1711462110.390658 [0] gc: gc 0x653a1b3a0940: deleting -1711462110.390663 [0] gc: gc_delete_proxy_participant(0x653a1b3a0940, 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.390667 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=13 -1711462110.390670 [0] gc: gc 0x653a1b39ee50: deleting -1711462110.390674 [0] gc: gc_delete_proxy_reader (0x653a1b39ee50, 110cd0d:79d6eeac:ea4a8907:1604) -1711462110.390679 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=29 -1711462110.390686 [0] gc: gc 0x653a1b39f080: deleting -1711462110.390690 [0] gc: gc_delete_proxy_reader (0x653a1b39f080, 110cd0d:79d6eeac:ea4a8907:1504) -1711462110.390694 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=28 -1711462110.390700 [0] gc: gc 0x653a1b2c97f0: deleting -1711462110.390703 [0] gc: gc_delete_proxy_reader (0x653a1b2c97f0, 110cd0d:79d6eeac:ea4a8907:1404) -1711462110.390707 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=27 -1711462110.390712 [0] gc: gc 0x653a1b2c9a20: deleting -1711462110.390716 [0] gc: gc_delete_proxy_reader (0x653a1b2c9a20, 110cd0d:79d6eeac:ea4a8907:1304) -1711462110.390720 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=26 -1711462110.390726 [0] gc: gc 0x653a1b2c9c50: deleting -1711462110.390731 [0] gc: gc_delete_proxy_reader (0x653a1b2c9c50, 110cd0d:79d6eeac:ea4a8907:1104) -1711462110.390735 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=25 -1711462110.390740 [0] gc: gc 0x653a1b39f6c0: deleting -1711462110.390745 [0] gc: gc_delete_proxy_reader (0x653a1b39f6c0, 110cd0d:79d6eeac:ea4a8907:f04) -1711462110.390749 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=24 -1711462110.390754 [0] gc: gc 0x653a1b39f8f0: deleting -1711462110.390759 [0] gc: gc_delete_proxy_reader (0x653a1b39f8f0, 110cd0d:79d6eeac:ea4a8907:d04) -1711462110.390764 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=23 -1711462110.390768 [0] gc: gc 0x653a1b39fb20: deleting -1711462110.390773 [0] gc: gc_delete_proxy_reader (0x653a1b39fb20, 110cd0d:79d6eeac:ea4a8907:b04) -1711462110.390777 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=22 -1711462110.390782 [0] gc: gc 0x653a1b45b840: deleting -1711462110.390786 [0] gc: gc_delete_proxy_reader (0x653a1b45b840, 110cd0d:79d6eeac:ea4a8907:904) -1711462110.390790 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=21 -1711462110.390795 [0] gc: gc 0x653a1b45ba70: deleting -1711462110.390798 [0] gc: gc_delete_proxy_reader (0x653a1b45ba70, 110cd0d:79d6eeac:ea4a8907:504) -1711462110.390803 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=20 -1711462110.390808 [0] gc: gc 0x653a1b45bca0: deleting -1711462110.390815 [0] gc: gc_delete_proxy_writer_dqueue(0x653a1b45bca0, 110cd0d:79d6eeac:ea4a8907:1203) -1711462110.390822 [0] gc: gc 0x653a1b4576d0: deleting -1711462110.390828 [0] gc: gc_delete_proxy_writer_dq1711462110.391551 [0] tev: writer_hbcontrol: wr 110e21c:d0762c48:a9f0fb28:403 multicasting (rel-prd 4 seq-eq-max 4 seq 22 maxseq 12) -1711462110.391576 [0] tev: heartbeat(wr 110e21c:d0762c48:a9f0fb28:403) sent, resched in 0.1 s (min-ack 12, avail-seq 22, xmit 22) -1711462110.391588 [0] tev: xpack_addmsg 0x7a832c001390 0x7a8340003220 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.391647 [0] tev: nn_xpack_send 52: 0x7a832c00139c:20 0x7a8340003318:32 [ udp/239.255.0.1:7401@2 ] -1711462110.391660 [0] tev: traffic-xmit (1) 52 -1711462110.391648 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.391687 [0] recvMC: HEARTBEAT(#30:22..22 110e21c:d0762c48:a9f0fb28:403? -> 0:0:0:0) -1711462110.391848 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.391861 [0] recvUC: INFODST(110e21c:d0762c48:a9f0fb28) -1711462110.391876 [0] recvUC: ACKNACK(F#5:23/0: L(:1c1 16958.051362) 110d7b4:17c5b8c5:94bd0ff4:504 -> 110e21c:d0762c48:a9f0fb28:403 ACK10 RM0) -1711462110.391924 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.391931 [0] recvUC: INFODST(110e21c:d0762c48:a9f0fb28) -1711462110.391943 [0] recvUC: ACKNACK(F#6:23/0: L(:1c1 16958.051434) 110cd0d:56a27910:57318171:504 -> 110e21c:d0762c48:a9f0fb28:403 ACK10 RM0) -1711462110.391948 [0] recvUC: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.391956 [0] recvUC: ACKNACK(F#20:23/0: L(:1c1 16958.051434) 110cd0d:e3b81b8d:1ccb65b1:504 -> 110e21c:d0762c48:a9f0fb28:403 ACK10 RM0) -1711462110.391962 [0] recvUC: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.391970 [0] recvUC: ACKNACK(F#11:23/0: L(:1c1 16958.051434) 110cd0d:79d6eeac:ea4a8907:504 -> 110e21c:d0762c48:a9f0fb28:403remove_acked_messages: deleting lingering writer 110e21c:d0762c48:a9f0fb28:403 -1711462110.391974 [0] recvUC: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:403) ... -1711462110.391981 [0] recvUC: => EVERYONE -1711462110.391997 [0] recvUC: writer_set_state(110e21c:d0762c48:a9f0fb28:403) state transition 2 -> 3 -1711462110.392004 [0] recvUC: ACK10 RM0) -1711462110.392010 [0] gc: gc 0x7a83200031b0: deleting -1711462110.392016 [0] gc: gc_delete_writer(0x7a83200031b0, 110e21c:d0762c48:a9f0fb28:403) -1711462110.392042 [0] gc: write_sample 110e21c:d0762c48:a9f0fb28:3c2 #20: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:403}} -1711462110.392052 [0] gc: non-timed queue now has 1 items -1711462110.392071 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:403 @ 0x57079b0bf814) user 5 builtin 12 -1711462110.392081 [0] tev: xpack_addmsg 0x7a832c001390 0x7a833804c1f0 0(data(110e21c:d0762c48:a9f0fb28:3c2:#20/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.392105 [0] python3: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:504) ... -1711462110.392121 [0] python3: => EVERYONE -1711462110.392146 [0] tev: nn_xpack_send 96: 0x7a832c00139c:20 0x7a833804bd58:48 0x7a8340002fe4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.392157 [0] tev: traffic-xmit (1) 96 -1711462110.392159 [0] gc: gc 0x57079b1c1ea0: not yet, shortsleep -1711462110.392169 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.392184 [0] recv: INFOTS(1711462110.392032273) -1711462110.392204 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #20 110e21c:d0762c48:a9f0fb28:3c2? -> 0:0:0:0) -1711462110.392416 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.392422 [0] recv: INFOTS(1711462110.384844509) -1711462110.392429 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #2) -1711462110.392452 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.392454 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110443d:bb7f10a5:18901533:1c1}} -1711462110.392479 [0] recv: INFOTS(1711462110.384844509) -1711462110.392491 [0] dq.builtin: SPDP ST3 110443d:bb7f10a5:18901533:1c1ddsi_delete_proxy_participant_by_guid(110443d:bb7f10a5:18901533:1c1) - deleting -1711462110.392493 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #2) -1711462110.392519 [0] dq.builtin: => EVERYONE -1711462110.392535 [0] dq.builtin: delete_ppt(110443d:bb7f10a5:18901533:1c1) - deleting dependent proxy participants -1711462110.392546 [0] dq.builtin: delete_ppt(110443d:bb7f10a5:18901533:1c1) - deleting endpoints -1711462110.392560 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:301c4) - deleting -1711462110.392576 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:301c3) - deleting -1711462110.392588 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:300c4) - deleting -1711462110.392598 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:300c3) - deleting -1711462110.392611 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:200c7) - deleting -1711462110.392623 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:200c2) - deleting -1711462110.392633 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:4c7) - deleting -1711462110.392644 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:4c2) - deleting -1711462110.392656 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:3c7) - deleting -1711462110.392666 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:3c2) - deleting -1711462110.392679 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:100c7) - deleting -1711462110.392687 [0] dq.builtin: delete -1711462110.392703 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110443d:bb7f10a5:18901533:1c1}} -1711462110.392712 [0] dq.builtin: SPDP ST3 110443d:bb7f10a5:18901533:1c1 unknown not allowed -1711462110.393236 [0] gc: gc 0x57079b1c1ea0: deleting -1711462110.393242 [0] gc: gc_delete_reader(0x57079b1c1ea0, 110e21c:d0762c48:a9f0fb28:504) -1711462110.393250 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:504, 3): reader no longer exists -1711462110.393257 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:504, 3): reader no longer exists -1711462110.393263 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:504, 4): reader no longer exists -1711462110.393268 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:504, 8): reader no longer exists -1711462110.393279 [0] gc: write_sample 110e21c:d0762c48:a9f0fb28:4c2 #16: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:504}} -1711462110.393287 [0] gc: non-timed queue now has 1 items -1711462110.393305 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:504 @ 0x57079b0b5964) user 4 builtin 12 -1711462110.393309 [0] gc: gc 0x7a8338019a10: deleting -1711462110.393314 [0] gc: gc_delete_proxy_reader (0x7a8338019a10, 110443d:bb7f10a5:18901533:301c4) -1711462110.393315 [0] tev: xpack_addmsg 0x7a832c001390 0x7a833804cdc0 0(data(110e21c:d0762c48:a9f0fb28:4c2:#16/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.393330 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7414@2 -1711462110.393335 [0] gc: reduced nlocs=3 -1711462110.393341 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393348 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393356 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393367 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393380 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.393358 [0] tev: nn_xpack_send 96: 0x7a832c00139c:20 0x7a83380250e8:48 0x7a8338024b44:28 [ udp/239.255.0.1:7400@2 ] -1711462110.393389 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393376 [0] python3: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:107) ... -1711462110.393409 [0] python3: => EVERYONE -1711462110.393400 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393411 [0] recv: INFOTS(1711462110.393273698) -1711462110.393433 [0] tev: traffic-xmit (1) 96 -1711462110.393426 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393434 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #16 110e21c:d0762c48:a9f0fb28:4c2? -> 0:0:0:0) -1711462110.393457 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393471 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393475 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393480 [0] gc: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.393484 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393491 [0] gc: a b c d -1711462110.393523 [0] tev: writer_hbcontrol: wr 110e21c:d0762c48:a9f0fb28:4c2 multicasting (rel-prd 5 seq-eq-max 4 seq 16 maxseq 15) -1711462110.393567 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.393606 [0] tev: heartbeat(wr 110e21c:d0762c48:a9f0fb28:4c2) sent, resched in 0.1 s (min-ack 15, avail-seq 16, xmit 16) -1711462110.393609 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462110.393617 [0] tev: xpack_addmsg 0x7a832c001390 0x7a833804cdc0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.393626 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.393641 [0] gc: best = 2 -1711462110.393646 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.393660 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.393695 [0] gc: ddsi_rebuild_writer_addrset(110e21c:d0762c48:a9f0fb28:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.393663 [0] tev: nn_xpack_send 52: 0x7a832c00139c:20 0x7a83380250e8:32 [ udp/239.255.0.1:7400@2 ] -1711462110.393730 [0] tev: traffic-xmit (1) 52 -1711462110.393721 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=11 -1711462110.393719 [0] recv: HEARTBEAT(#11:16..16 110e21c:d0762c48:a9f0fb28:4c2? -> 0:0:0:0) -1711462110.393756 [0] gc: gc 0x7a833803ba00: deleting -1711462110.393771 [0] gc: gc_delete_proxy_writer_dqueue(0x7a833803ba00, 110443d:bb7f10a5:18901533:301c3) -1711462110.393777 [0] gc: gc 0x7a833804e9d0: deleting -1711462110.393781 [0] gc: gc_delete_proxy_reader (0x7a833804e9d0, 110443d:bb7f10a5:18901533:300c4) -1711462110.393786 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7a833803ba00, 110443d:bb7f10a5:18901533:301c3) -1711462110.393792 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7414@2 -1711462110.393805 [0] gc: reduced nlocs=3 -1711462110.393811 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393818 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393832 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393836 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393841 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393846 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393852 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393860 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393846 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.393866 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393883 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393891 [0] gc: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.393900 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393877 [0] recv: INFODST(110e21c:d0762c48:a9f0fb28) -1711462110.393913 [0] gc: a b c d -1711462110.393937 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.393938 [0] recv: ACKNACK(F#5:17/0: L(:1c1 16958.053378) 110d7b4:17c5b8c5:94bd0ff4:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2 ACK1 RM0) -1711462110.393946 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462110.393964 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.393974 [0] gc: best = 2 -1711462110.393980 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.393989 [0] gc: ddsi_rebuild_writer_addrset(110e21c:d0762c48:a9f0fb28:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.393993 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=10 -1711462110.393999 [0] gc: gc 0x7a833804df70: deleting -1711462110.394004 [0] gc: gc_delete_proxy_writer_dqueue(0x7a833804df70, 110443d:bb7f10a5:18901533:300c3) -1711462110.394011 [0] gc: gc 0x7a833804d510: deleting -1711462110.394018 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7a833804df70, 110443d:bb7f10a5:18901533:300c3) -1711462110.394021 [0] gc: gc_delete_proxy_reader (0x7a833804d510, 110443d:bb7f10a5:18901533:200c7) -1711462110.394044 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7414@2 -1711462110.394050 [0] gc: reduced nlocs=3 -1711462110.394059 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394065 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394072 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394079 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394088 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394100 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394088 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.394104 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394124 [0] recv: INFODST(110e21c:d0762c48:a9f0fb28) -1711462110.394128 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394144 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394149 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394157 [0] gc: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394165 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394149 [0] recv: ACKNACK(F#5:17/0: L(:1c1 16958.053624) 110cd0d:56a27910:57318171:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2 ACK1 RM0) -1711462110.394172 [0] gc: a b c d -1711462110.394182 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.394190 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.394208 [0] recv: ACKNACK(F#13:17/0: L(:1c1 16958.053624) 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2 ACK1 RM0) -1711462110.394209 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462110.394226 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.394239 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.394250 [0] recv: ACKNACK(F#8:17/0: L(:1c1 16958.053624) 110cd0d:79d6eeac:ea4a8907:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2 ACK1 RM1) -1711462110.394251 [0] gc: best = 2 -1711462110.394270 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394278 [0] gc: ddsi_rebuild_writer_addrset(110e21c:d0762c48:a9f0fb28:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394288 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=9 -1711462110.394292 [0] gc: gc 0x7a833804cab0: deleting -1711462110.394298 [0] gc: gc_delete_proxy_writer_dqueue(0x7a833804cab0, 110443d:bb7f10a5:18901533:200c2) -1711462110.394303 [0] gc: gc 0x7a833804bea0: deleting -1711462110.394308 [0] gc: gc_delete_proxy_reader (0x7a833804bea0, 110443d:bb7f10a5:18901533:4c7) -1711462110.394314 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7a833804cab0, 110443d:bb7f10a5:18901533:200c2) -1711462110.394321 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7414@2 -1711462110.394334 [0] gc: reduced nlocs=3 -1711462110.394339 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394346 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394352 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394356 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394365 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394370 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394374 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394382 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394389 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394393 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394399 [0] gc: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394405 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394412 [0] gc: a b c d -1711462110.394421 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.394428 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462110.394436 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.394441 [0] gc: best = 2 -1711462110.394448 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394458 [0] gc: ddsi_rebuild_writer_addrset(110e21c:d0762c48:a9f0fb28:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394464 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=8 -1711462110.394468 [0] gc: gc 0x7a833804b5c0: deleting -1711462110.394473 [0] gc: gc_delete_proxy_writer_dqueue(0x7a833804b5c0, 110443d:bb7f10a5:18901533:4c2) -1711462110.394482 [0] gc: gc 0x7a83380718f0: deleting -1711462110.394486 [0] gc: gc_delete_proxy_reader (0x7a83380718f0, 110443d:bb7f10a5:18901533:3c7) -1711462110.394495 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7a833804b5c0, 110443d:bb7f10a5:18901533:4c2) -1711462110.394498 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7414@2 -1711462110.394514 [0] gc: reduced nlocs=3 -1711462110.394519 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394530 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394537 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394542 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394550 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394555 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394559 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394564 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394570 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394577 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394583 [0] gc: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394588 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394599 [0] gc: a b c d -1711462110.394610 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.394619 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462110.394628 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.394633 [0] gc: best = 2 -1711462110.394640 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394652 [0] gc: ddsi_rebuild_writer_addrset(110e21c:d0762c48:a9f0fb28:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394657 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=7 -1711462110.394665 [0] gc: gc 0x7a8338066650: deleting -1711462110.394669 [0] gc: gc_delete_proxy_writer_dqueue(0x7a8338066650, 110443d:bb7f10a5:18901533:3c2) -1711462110.394674 [0] gc: gc 0x7a8338055590: deleting -1711462110.394684 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7a8338066650, 110443d:bb7f10a5:18901533:3c2) -1711462110.394685 [0] gc: gc_delete_proxy_reader (0x7a8338055590, 110443d:bb7f10a5:18901533:100c7) -1711462110.394702 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=6 -1711462110.394707 [0] gc: gc 0x7a8338073cf0: deleting -1711462110.394711 [0] gc: gc_delete_proxy_participant(0x7a8338073cf0, 110443d:bb7f10a5:18901533:1c1) -1711462110.394718 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=5 -1711462110.394723 [0] gc: gc 0x7a833000d280: deleting -1711462110.394728 [0] gc: gc_delete_reader(0x7a833000d280, 110e21c:d0762c48:a9f0fb28:107) -1711462110.394738 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:107 @ 0x57079b070714) user 3 builtin 12 -1711462110.394745 [0] gc: gc 0x7a833803ba00: deleting -1711462110.394749 [0] gc: gc_delete_proxy_writer (0x7a833803ba00, 110443d:bb7f10a5:18901533:301c3) -1711462110.394757 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:301c4, 3): 3 -> 3 -1711462110.394765 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=4 -1711462110.394770 [0] gc: gc 0x7a833804df70: deleting -1711462110.394777 [0] gc: gc_delete_proxy_writer (0x7a833804df70, 110443d:bb7f10a5:18901533:300c3) -1711462110.394771 [0] python3: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:307) ... -1711462110.394787 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:300c4, 3): 3 -> 3 -1711462110.394804 [0] python3: => EVERYONE -1711462110.394807 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=3 -1711462110.394825 [0] gc: gc 0x7a833804cab0: deleting -1711462110.394829 [0] gc: gc_delete_proxy_writer (0x7a833804cab0, 110443d:bb7f10a5:18901533:200c2) -1711462110.394836 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:200c7, 6): 6 -> 6 -1711462110.394846 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=2 -1711462110.394852 [0] gc: gc 0x7a833804b5c0: deleting -1711462110.394856 [0] gc: gc_delete_proxy_writer (0x7a833804b5c0, 110443d:bb7f10a5:18901533:4c2) -1711462110.394862 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:4c7, 5): 5 -> 5 -1711462110.394870 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=1 -1711462110.394876 [0] gc: gc 0x7a8338066650: deleting -1711462110.394881 [0] gc: gc_delete_proxy_writer (0x7a8338066650, 110443d:bb7f10a5:18901533:3c2) -1711462110.394888 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:3c7, 6): 6 -> 6 -1711462110.394894 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=0, freeing -1711462110.394903 [0] gc: lease_unregister(l 0x7a8338036100 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.394910 [0] gc: lease_free(l 0x7a8338036100 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.394919 [0] gc: lease_free(l 0x7a8338036080 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.394927 [0] gc: ddsi_remove_deleted_participant_guid(110443d:bb7f10a5:18901533:1c1 for_what=3) -1711462110.394932 [0] gc: gc 0x57079aaf1c00: deleting -1711462110.394937 [0] gc: gc 0x57079b1c1ea0: deleting -1711462110.394942 [0] gc: gc_delete_reader(0x57079b1c1ea0, 110e21c:d0762c48:a9f0fb28:307) -1711462110.394951 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:307 @ 0x57079b114fb4) user 2 builtin 12 -1711462110.394955 [0] gc: gc 0x7a833807dcb0: deleting -1711462110.394960 [0] gc: gc 0x7a8338013760: deleting -1711462110.394967 [0] gc: gc 0x57079b1b0b40: not yet, shortsleep -1711462110.395045 [0] python3: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:207) ... -1711462110.395058 [0] python3: => EVERYONE -1711462110.395423 [0] tev: writer_hbcontrol: wr 110e21c:d0762c48:a9f0fb28:3c2 multicasting (rel-prd 4 seq-eq-max 4 seq 20 maxseq 19) -1711462110.395556 [0] tev: heartbeat(wr 110e21c:d0762c48:a9f0fb28:3c2) sent, resched in 0.1 s (min-ack 19, avail-seq 20, xmit 20) -1711462110.395565 [0] tev: xpack_addmsg 0x7a832c001390 0x7a833804cdc0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.395597 [0] tev: nn_xpack_send 52: 0x7a832c00139c:20 0x7a83380250e8:32 [ udp/239.255.0.1:7400@2 ] -1711462110.395609 [0] tev: traffic-xmit (1) 52 -1711462110.395601 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.395630 [0] recv: HEARTBEAT(#11:20..20 110e21c:d0762c48:a9f0fb28:3c2? -> 0:0:0:0) -1711462110.395731 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.395739 [0] recv: INFODST(110e21c:d0762c48:a9f0fb28) -1711462110.395753 [0] recv: ACKNACK(F#5:21/0: L(:1c1 16958.055241) 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2 ACK1 RM0) -1711462110.395915 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.395923 [0] recv: INFODST(110e21c:d0762c48:a9f0fb28) -1711462110.395935 [0] recv: ACKNACK(F#5:21/0: L(:1c1 16958.055426) 110cd0d:56a27910:57318171:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2 ACK1 RM0) -1711462110.395941 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.395951 [0] recv: ACKNACK(F#15:21/0: L(:1c1 16958.055426) 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2 ACK1 RM0) -1711462110.395958 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.395971 [0] recv: ACKNACK(F#9:21/0: L(:1c1 16958.055426) 110cd0d:79d6eeac:ea4a8907:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2 ACK1 RM1) -1711462110.396044 [0] gc: gc 0x57079b1b0b40: deleting -1711462110.396062 [0] gc: gc 0x57079b036af0: deleting -1711462110.396078 [0] gc: gc 0x57079b196c50: deleting -1711462110.396085 [0] gc: gc 0x57079abe50e0: deleting -1711462110.396090 [0] gc: gc 0x57079b18d360: deleting -1711462110.396094 [0] gc: gc 0x57079b1928e0: deleting -1711462110.396100 [0] gc: gc 0x57079b12cae0: deleting -1711462110.396104 [0] gc: gc 0x57079b13e610: deleting -1711462110.396112 [0] gc: gc 0x57079b17b8f0: deleting -1711462110.396117 [0] gc: gc 0x57079b136cd0: deleting -1711462110.396122 [0] gc: gc 0x57079b19e880: deleting -1711462110.396128 [0] gc: gc 0x57079ac6b4c0: deleting -1711462110.396132 [0] gc: gc 0x57079aac0ae0: deleting -1711462110.396137 [0] gc: gc 0x57079ab12e10: deleting -1711462110.396141 [0] gc: gc 0x57079ab4f2e0: deleting -1711462110.396145 [0] gc: gc 0x57079ab4b370: deleting -1711462110.396152 [0] gc: gc 0x57079aac1e10: deleting -1711462110.396157 [0] gc: gc 0x57079aabd660: deleting -1711462110.396161 [0] gc: gc 0x57079ab544e0: deleting -1711462110.396166 [0] gc: gc 0x57079ac22c30: deleting -1711462110.396170 [0] gc: gc 0x57079abec600: deleting -1711462110.396175 [0] gc: gc 0x57079ac3b6e0: deleting -1711462110.396181 [0] gc: gc 0x57079abf1850: deleting -1711462110.396185 [0] gc: gc 0x57079abfb210: deleting -1711462110.396189 [0] gc: gc 0x57079ac009d0: deleting -1711462110.396193 [0] gc: gc 0x57079ad216e0: deleting -1711462110.396198 [0] gc: gc 0x57079b0bcd80: deleting -1711462110.396203 [0] gc: gc 0x57079aee5fa0: deleting -1711462110.396206 [0] gc: gc 0x57079af8db80: deleting -1711462110.396211 [0] gc: gc 0x57079b0b7d90: deleting -1711462110.396217 [0] gc: gc 0x57079b0b5d00: deleting -1711462110.396222 [0] gc: gc 0x57079b099170: deleting -1711462110.396226 [0] gc: gc 0x57079b0b7030: deleting -1711462110.396230 [0] gc: gc 0x57079b0b6d10: deleting -1711462110.396233 [0] gc: gc 0x57079b0b0070: deleting -1711462110.396237 [0] gc: gc 0x57079b0c74c0: deleting -1711462110.396241 [0] gc: gc 0x57079b0a9240: deleting -1711462110.396244 [0] gc: gc 0x57079b0638a0: deleting -1711462110.396248 [0] gc: gc 0x57079b0c5960: deleting -1711462110.396252 [0] gc: gc 0x57079b082a00: deleting -1711462110.396255 [0] gc: gc 0x57079b0c3380: deleting -1711462110.396259 [0] gc: gc 0x57079b069900: deleting -1711462110.396263 [0] gc: gc 0x57079b069d30: deleting -1711462110.396269 [0] gc: gc 0x57079b0cf4a0: deleting -1711462110.396272 [0] gc: gc 0x57079b0c5570: deleting -1711462110.396276 [0] gc: gc 0x57079b08ed60: deleting -1711462110.396280 [0] gc: gc 0x57079b07b4e0: deleting -1711462110.396284 [0] gc: gc 0x57079b0cf020: deleting -1711462110.396288 [0] gc: gc_delete_reader(0x57079b0cf020, 110e21c:d0762c48:a9f0fb28:207) -1711462110.396297 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:207 @ 0x57079b0c1aa4) user 1 builtin 12 -1711462110.396348 [0] gc: gc 0x57079b0d1fb0: not yet, shortsleep -1711462110.396415 [0] python3: ddsi_delete_participant (110e21c:d0762c48:a9f0fb28:1c1) -1711462110.396425 [0] python3: => EVERYONE -1711462110.396442 [0] python3: trigger_recv_threads: 0 many 0x57079b0c23b0 -1711462110.396461 [0] python3: trigger_recv_threads: 1 single udp/239.255.0.1:7401 -1711462110.396486 [0] recv: done -1711462110.396518 [0] python3: trigger_recv_threads: 2 single udp/10.101.12.71:7413 -1711462110.396507 [0] recvMC: done -1711462110.396577 [0] recvUC: done -1711462110.396714 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:56a27910:57318171:1c1) - deleting -1711462110.396729 [0] python3: => EVERYONE -1711462110.396739 [0] python3: delete_ppt(110cd0d:56a27910:57318171:1c1) - deleting dependent proxy participants -1711462110.396749 [0] python3: delete_ppt(110cd0d:56a27910:57318171:1c1) - deleting endpoints -1711462110.396757 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1604) => EVERYONE -1711462110.396765 [0] python3: - deleting -1711462110.396773 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1504) => EVERYONE -1711462110.396780 [0] python3: - deleting -1711462110.396787 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1404) => EVERYONE -1711462110.396794 [0] python3: - deleting -1711462110.396803 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1304) => EVERYONE -1711462110.396810 [0] python3: - deleting -1711462110.396817 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1104) => EVERYONE -1711462110.396823 [0] python3: - deleting -1711462110.396831 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:f04) => EVERYONE -1711462110.396838 [0] python3: - deleting -1711462110.396845 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:d04) => EVERYONE -1711462110.396853 [0] python3: - deleting -1711462110.396861 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:b04) => EVERYONE -1711462110.396867 [0] python3: - deleting -1711462110.396875 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:904) => EVERYONE -1711462110.396882 [0] python3: - deleting -1711462110.396889 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:504) => EVERYONE -1711462110.396896 [0] python3: - deleting -1711462110.396902 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:1203) - deleting -1711462110.396909 [0] python3: => EVERYONE -1711462110.396918 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:1003) - deleting -1711462110.396924 [0] python3: => EVERYONE -1711462110.396933 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:e03) - deleting -1711462110.396939 [0] python3: => EVERYONE -1711462110.396947 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:c03) - deleting -1711462110.396953 [0] python3: => EVERYONE -1711462110.396961 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:a03) - deleting -1711462110.396968 [0] python3: => EVERYONE -1711462110.396977 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:803) - deleting -1711462110.396983 [0] python3: => EVERYONE -1711462110.396994 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:703) - deleting -1711462110.397000 [0] python3: => EVERYONE -1711462110.397010 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:403) - deleting -1711462110.397019 [0] python3: => EVERYONE -1711462110.397030 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:301c4) - deleting -1711462110.397043 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:301c3) - deleting -1711462110.397053 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:300c4) - deleting -1711462110.397063 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:300c3) - deleting -1711462110.397074 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:200c7) - deleting -1711462110.397085 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:200c2) - deleting -1711462110.397097 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:4c7) - deleting -1711462110.397108 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:4c2) - deleting -1711462110.397117 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:3c7) - deleting -1711462110.397127 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:3c2) - deleting -1711462110.397136 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:100c7) - deleting -1711462110.397145 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting -1711462110.397151 [0] python3: => EVERYONE -1711462110.397159 [0] python3: delete_ppt(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting dependent proxy participants -1711462110.397167 [0] python3: delete_ppt(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting endpoints -1711462110.397174 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1604) => EVERYONE -1711462110.397182 [0] python3: - deleting -1711462110.397190 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1504) => EVERYONE -1711462110.397197 [0] python3: - deleting -1711462110.397205 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1404) => EVERYONE -1711462110.397212 [0] python3: - deleting -1711462110.397220 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1304) => EVERYONE -1711462110.397226 [0] python3: - deleting -1711462110.397234 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1104) => EVERYONE -1711462110.397241 [0] python3: - deleting -1711462110.397249 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:f04) => EVERYONE -1711462110.397256 [0] python3: - deleting -1711462110.397264 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:d04) => EVERYONE -1711462110.397272 [0] python3: - deleting -1711462110.397280 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:b04) => EVERYONE -1711462110.397286 [0] python3: - deleting -1711462110.397294 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:904) => EVERYONE -1711462110.397301 [0] python3: - deleting -1711462110.397309 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:504) => EVERYONE -1711462110.397316 [0] python3: - deleting -1711462110.397323 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:1203) - deleting -1711462110.397329 [0] python3: => EVERYONE -1711462110.397338 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:1003) - deleting -1711462110.397345 [0] python3: => EVERYONE -1711462110.397354 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:e03) - deleting -1711462110.397360 [0] python3: => EVERYONE -1711462110.397369 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:c03) - deleting -1711462110.397375 [0] python3: => EVERYONE -1711462110.397383 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:a03) - deleting -1711462110.397389 [0] python3: => EVERYONE -1711462110.397398 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:803) - deleting -1711462110.397404 [0] python3: => EVERYONE -1711462110.397413 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:703) - deleting -1711462110.397420 [0] python3: => EVERYONE -1711462110.397426 [0] gc: gc 0x57079b0d1fb0: deleting -1711462110.397429 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:403) - deleting -1711462110.397448 [0] gc: gc 0x57079b0d83e0: deleting -1711462110.397454 [0] gc: gc 0x57079b0d1c90: deleting -1711462110.397460 [0] gc: gc 0x57079b0d8140: deleting -1711462110.397466 [0] gc: gc 0x57079b0df1a0: deleting -1711462110.397470 [0] gc: gc 0x57079b0ef820: deleting -1711462110.397475 [0] gc: gc 0x57079b0b4fe0: deleting -1711462110.397480 [0] gc: gc 0x57079b023580: deleting -1711462110.397484 [0] gc: gc 0x57079b09b390: deleting -1711462110.397490 [0] gc: gc 0x57079b09cff0: deleting -1711462110.397474 [0] python3: => EVERYONE -1711462110.397495 [0] gc: gc 0x57079b07fe90: deleting -1711462110.397511 [0] gc: gc 0x57079b1018e0: deleting -1711462110.397513 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:301c4) - deleting -1711462110.397515 [0] gc: gc 0x57079b122900: deleting -1711462110.397527 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:301c3) - deleting -1711462110.397529 [0] gc: gc 0x57079b070ec0: deleting -1711462110.397542 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:300c4) - deleting -1711462110.397544 [0] gc: gc 0x57079b1225e0: deleting -1711462110.397562 [0] gc: gc 0x57079b094f50: deleting -1711462110.397554 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:300c3) - deleting -1711462110.397567 [0] gc: gc 0x57079b17c500: deleting -1711462110.397581 [0] gc: gc 0x57079b0b36e0: deleting -1711462110.397588 [0] gc: gc 0x57079b0fb550: deleting -1711462110.397580 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:200c7) - deleting -1711462110.397592 [0] gc: gc 0x57079b128510: deleting -1711462110.397605 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:200c2) - deleting -1711462110.397607 [0] gc: gc 0x57079b0e8810: deleting -1711462110.397620 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:4c7) - deleting -1711462110.397622 [0] gc: gc 0x57079b0e47b0: deleting -1711462110.397633 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:4c2) - deleting -1711462110.397636 [0] gc: gc 0x57079b06dfc0: deleting -1711462110.397647 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:3c7) - deleting -1711462110.397649 [0] gc: gc 0x57079b110d00: deleting -1711462110.397660 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:3c2) - deleting -1711462110.397663 [0] gc: gc 0x57079b169860: deleting -1711462110.397675 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:100c7) - deleting -1711462110.397676 [0] gc: gc 0x57079b10d910: deleting -1711462110.397687 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:79d6eeac:ea4a8907:1c1) - deleting -1711462110.397691 [0] gc: gc 0x57079b0f2b00: deleting -1711462110.397705 [0] gc: gc 0x57079b0eed30: deleting -1711462110.397698 [0] python3: => EVERYONE -1711462110.397709 [0] gc: gc 0x57079b17a790: deleting -1711462110.397721 [0] python3: delete_ppt(110cd0d:79d6eeac:ea4a8907:1c1) - deleting dependent proxy participants -1711462110.397724 [0] gc: gc 0x57079b0ede60: deleting -1711462110.397738 [0] gc: gc 0x57079b168e30: deleting -1711462110.397732 [0] python3: delete_ppt(110cd0d:79d6eeac:ea4a8907:1c1) - deleting endpoints -1711462110.397742 [0] gc: gc 0x57079b103270: deleting -1711462110.397759 [0] gc: gc 0x57079b188db0: deleting -1711462110.397753 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1604) => EVERYONE -1711462110.397764 [0] gc: gc 0x57079b1817c0: deleting -1711462110.397779 [0] gc: gc 0x57079b203ae0: deleting -1711462110.397773 [0] python3: - deleting -1711462110.397783 [0] gc: gc 0x57079b1fe9f0: deleting -1711462110.397796 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1504) => EVERYONE -1711462110.397799 [0] gc: gc 0x57079b100f50: deleting -1711462110.397814 [0] gc: gc 0x57079b15ca40: deleting -1711462110.397818 [0] gc: gc 0x57079b1b2a90: deleting -1711462110.397824 [0] gc: gc 0x57079b0c01c0: deleting -1711462110.397807 [0] python3: - deleting -1711462110.397828 [0] gc: gc 0x57079abe50e0: deleting -1711462110.397841 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1404) => EVERYONE -1711462110.397842 [0] gc: gc 0x57079b18d360: deleting -1711462110.397858 [0] gc: gc 0x57079b1928e0: deleting -1711462110.397864 [0] gc: gc 0x57079b12cae0: deleting -1711462110.397870 [0] gc: gc 0x57079b13e610: deleting -1711462110.397851 [0] python3: - deleting -1711462110.397875 [0] gc: gc 0x57079b0bd430: deleting -1711462110.397888 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1304) => EVERYONE -1711462110.397889 [0] gc: gc_delete_participant (0x57079b0bd430, 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.397898 [0] python3: - deleting -1711462110.397908 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 0:0:0:0 @ (nil)) user 0 builtin 12 -1711462110.397915 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1104) => EVERYONE -1711462110.397924 [0] python3: - deleting -1711462110.397927 [0] gc: write_sample 110e21c:d0762c48:a9f0fb28:100c2 #2: ST3 DCPSParticipant/ParticipantBuiltinTopicData:{participant_guid={110e21c:d0762c48:a9f0fb28:1c1}} -1711462110.397933 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:f04) => EVERYONE -1711462110.397938 [0] gc: non-timed queue now has 1 items -1711462110.397941 [0] python3: - deleting -1711462110.397943 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:100c2) ... -1711462110.397961 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:100c2) ... -1711462110.397952 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:d04) => EVERYONE -1711462110.397971 [0] gc: writer_set_state(110e21c:d0762c48:a9f0fb28:100c2) state transition 0 -> 3 -1711462110.397988 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:3c2) ... -1711462110.397993 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:3c2) ... -1711462110.397981 [0] python3: - deleting -1711462110.397999 [0] gc: writer_set_state(110e21c:d0762c48:a9f0fb28:3c2) state transition 0 -> 3 -1711462110.398010 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:b04) => EVERYONE -1711462110.398012 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:4c2) ... -1711462110.398027 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:4c2) ... -1711462110.398034 [0] gc: writer_set_state(110e21c:d0762c48:a9f0fb28:4c2) state transition 0 -> 3 -1711462110.398039 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:200c2) ... -1711462110.398045 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:200c2) ... -1711462110.398020 [0] python3: - deleting -1711462110.398050 [0] gc: writer_set_state(110e21c:d0762c48:a9f0fb28:200c2) state transition 0 -> 3 -1711462110.398061 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:904) => EVERYONE -1711462110.398063 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:100c7) ... -1711462110.398072 [0] python3: - deleting -1711462110.398081 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:2c2) - unknown guid -1711462110.398095 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:2c7) - unknown guid -1711462110.398101 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:3c7) ... -1711462110.398095 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:504) => EVERYONE -1711462110.398107 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:4c7) ... -1711462110.398125 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:200c7) ... -1711462110.398133 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:300c3) ... -1711462110.398117 [0] python3: - deleting -1711462110.398136 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:300c3) ... -1711462110.398148 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:1203) - deleting -1711462110.398153 [0] gc: writer_set_state(110e21c:d0762c48:a9f0fb28:300c3) state transition 0 -> 3 -1711462110.398167 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:301c3) ... -1711462110.398172 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:301c3) ... -1711462110.398159 [0] python3: => EVERYONE -1711462110.398177 [0] gc: writer_set_state(110e21c:d0762c48:a9f0fb28:301c3) state transition 0 -> 3 -1711462110.398190 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:1003) - deleting -1711462110.398191 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:300c4) ... -1711462110.398200 [0] python3: => EVERYONE -1711462110.398209 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:301c4) ... -1711462110.398213 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:ff0003c2) - unknown guid -1711462110.398218 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:ff0003c7) - unknown guid -1711462110.398226 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:ff0004c2) - unknown guid -1711462110.398229 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:ff0004c7) - unknown guid -1711462110.398235 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:ff0200c2) - unknown guid -1711462110.398219 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:e03) - deleting -1711462110.398239 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:ff0200c7) - unknown guid -1711462110.398254 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:201c3) - unknown guid -1711462110.398248 [0] python3: => EVERYONE -1711462110.398259 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:201c4) - unknown guid -1711462110.398271 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:c03) - deleting -1711462110.398273 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:ff0101c2) - unknown guid -1711462110.398288 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:ff0101c7) - unknown guid -1711462110.398293 [0] gc: ddsi_delete_writer_nolinger(guid 110e21c:d0762c48:a9f0fb28:ff0202c3) - unknown guid -1711462110.398281 [0] python3: => EVERYONE -1711462110.398297 [0] gc: delete_reader_guid(guid 110e21c:d0762c48:a9f0fb28:ff0202c4) - unknown guid -1711462110.398309 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:a03) - deleting -1711462110.398312 [0] gc: gc 0x57079b0bcfd0: not yet, shortsleep -1711462110.398319 [0] python3: => EVERYONE -1711462110.398336 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:803) - deleting -1711462110.398344 [0] python3: => EVERYONE -1711462110.398353 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:703) - deleting -1711462110.398359 [0] python3: => EVERYONE -1711462110.398367 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:403) - deleting -1711462110.398373 [0] python3: => EVERYONE -1711462110.398382 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:301c4) - deleting -1711462110.398392 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:301c3) - deleting -1711462110.398401 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:300c4) - deleting -1711462110.398409 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:300c3) - deleting -1711462110.398420 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:200c7) - deleting -1711462110.398428 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:200c2) - deleting -1711462110.398437 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:4c7) - deleting -1711462110.398445 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:4c2) - deleting -1711462110.398453 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:3c7) - deleting -1711462110.398461 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:3c2) - deleting -1711462110.398469 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:100c7) - deleting -1711462110.398477 [0] python3: ddsi_delete_proxy_participant_by_guid(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting -1711462110.398483 [0] python3: => EVERYONE -1711462110.398491 [0] python3: delete_ppt(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting dependent proxy participants -1711462110.398498 [0] python3: delete_ppt(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting endpoints -1711462110.398505 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:504) => EVERYONE -1711462110.398511 [0] python3: - deleting -1711462110.398519 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:403) - deleting -1711462110.398525 [0] python3: => EVERYONE -1711462110.398533 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:301c4) - deleting -1711462110.398541 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:301c3) - deleting -1711462110.398549 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:300c4) - deleting -1711462110.398557 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:300c3) - deleting -1711462110.398565 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:200c7) - deleting -1711462110.398572 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:200c2) - deleting -1711462110.398581 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4c7) - deleting -1711462110.398588 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4c2) - deleting -1711462110.398596 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3c7) - deleting -1711462110.398603 [0] python3: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3c2) - deleting -1711462110.398611 [0] python3: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:100c7) - deleting -1711462110.399397 [0] gc: gc 0x57079b0bcfd0: deleting -1711462110.399414 [0] gc: gc_delete_proxy_reader (0x57079b0bcfd0, 110cd0d:56a27910:57318171:1604) -1711462110.399419 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=29 -1711462110.399426 [0] gc: gc 0x57079b0bd200: deleting -1711462110.399429 [0] gc: gc_delete_proxy_reader (0x57079b0bd200, 110cd0d:56a27910:57318171:1504) -1711462110.399433 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=28 -1711462110.399439 [0] gc: gc 0x57079b17b8f0: deleting -1711462110.399443 [0] gc: gc_delete_proxy_reader (0x57079b17b8f0, 110cd0d:56a27910:57318171:1404) -1711462110.399446 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=27 -1711462110.399452 [0] gc: gc 0x57079b136cd0: deleting -1711462110.399455 [0] gc: gc_delete_proxy_reader (0x57079b136cd0, 110cd0d:56a27910:57318171:1304) -1711462110.399459 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=26 -1711462110.399464 [0] gc: gc 0x57079b19e880: deleting -1711462110.399468 [0] gc: gc_delete_proxy_reader (0x57079b19e880, 110cd0d:56a27910:57318171:1104) -1711462110.399472 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=25 -1711462110.399477 [0] gc: gc 0x57079ac6b4c0: deleting -1711462110.399481 [0] gc: gc_delete_proxy_reader (0x57079ac6b4c0, 110cd0d:56a27910:57318171:f04) -1711462110.399484 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=24 -1711462110.399491 [0] gc: gc 0x57079aac0ae0: deleting -1711462110.399495 [0] gc: gc_delete_proxy_reader (0x57079aac0ae0, 110cd0d:56a27910:57318171:d04) -1711462110.399499 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=23 -1711462110.399505 [0] gc: gc 0x57079ab12e10: deleting -1711462110.399508 [0] gc: gc_delete_proxy_reader (0x57079ab12e10, 110cd0d:56a27910:57318171:b04) -1711462110.399512 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=22 -1711462110.399517 [0] gc: gc 0x57079ab4f2e0: deleting -1711462110.399521 [0] gc: gc_delete_proxy_reader (0x57079ab4f2e0, 110cd0d:56a27910:57318171:904) -1711462110.399525 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=21 -1711462110.399530 [0] gc: gc 0x57079ab4b370: deleting -1711462110.399534 [0] gc: gc_delete_proxy_reader (0x57079ab4b370, 110cd0d:56a27910:57318171:504) -1711462110.399538 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=20 -1711462110.399543 [0] gc: gc 0x57079aac1e10: deleting -1711462110.399547 [0] gc: gc_delete_proxy_writer_dqueue(0x57079aac1e10, 110cd0d:56a27910:57318171:1203) -1711462110.399554 [0] gc: gc 0x57079aabd660: deleting -1711462110.399558 [0] gc: gc_delete_proxy_writer_dqueue(0x57079aabd660, 110cd0d:56a27910:57318171:1003) -1711462110.399562 [0] gc: gc 0x57079ab544e0: deleting -1711462110.399566 [0] gc: gc_delete_proxy_writer_dqueue(0x57079ab544e0, 110cd0d:56a27910:57318171:e03) -1711462110.399569 [0] gc: gc 0x57079ac22c30: deleting -1711462110.399573 [0] gc: gc_delete_proxy_writer_dqueue(0x57079ac22c30, 110cd0d:56a27910:57318171:c03) -1711462110.399579 [0] gc: gc 0x57079abec600: deleting -1711462110.399584 [0] gc: gc_delete_proxy_writer_dqueue(0x57079abec600, 110cd0d:56a27910:57318171:a03) -1711462110.399587 [0] gc: gc 0x57079ac3b6e0: deleting -1711462110.399591 [0] gc: gc_delete_proxy_writer_dqueue(0x57079ac3b6e0, 110cd0d:56a27910:57318171:803) -1711462110.399595 [0] gc: gc 0x57079abf1850: deleting -1711462110.399599 [0] gc: gc_delete_proxy_writer_dqueue(0x57079abf1850, 110cd0d:56a27910:57318171:703) -1711462110.399598 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079aac1e10, 110cd0d:56a27910:57318171:1203) -1711462110.399603 [0] gc: gc 0x57079abfb210: deleting -1711462110.399615 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079aabd660, 110cd0d:56a27910:57318171:1003) -1711462110.399623 [0] gc: gc_delete_proxy_writer_dqueue(0x57079abfb210, 110cd0d:56a27910:57318171:403) -1711462110.399629 [0] gc: gc 0x57079ac009d0: deleting -1711462110.399633 [0] gc: gc_delete_proxy_reader (0x57079ac009d0, 110cd0d:56a27910:57318171:301c4) -1711462110.399636 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079ab544e0, 110cd0d:56a27910:57318171:e03) -1711462110.399638 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=19 -1711462110.399643 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079ac22c30, 110cd0d:56a27910:57318171:c03) -1711462110.399651 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079abec600, 110cd0d:56a27910:57318171:a03) -1711462110.399657 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079ac3b6e0, 110cd0d:56a27910:57318171:803) -1711462110.399661 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079abf1850, 110cd0d:56a27910:57318171:703) -1711462110.399664 [0] gc: gc 0x57079ad216e0: deleting -1711462110.399670 [0] gc: gc_delete_proxy_writer_dqueue(0x57079ad216e0, 110cd0d:56a27910:57318171:301c3) -1711462110.399666 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079abfb210, 110cd0d:56a27910:57318171:403) -1711462110.399677 [0] gc: gc 0x57079b0bcd80: deleting -1711462110.399689 [0] gc: gc_delete_proxy_reader (0x57079b0bcd80, 110cd0d:56a27910:57318171:300c4) -1711462110.399694 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=18 -1711462110.399700 [0] gc: gc 0x57079aee5fa0: deleting -1711462110.399703 [0] gc: gc_delete_proxy_writer_dqueue(0x57079aee5fa0, 110cd0d:56a27910:57318171:300c3) -1711462110.399708 [0] gc: gc 0x57079af8db80: deleting -1711462110.399711 [0] gc: gc_delete_proxy_reader (0x57079af8db80, 110cd0d:56a27910:57318171:200c7) -1711462110.399715 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=17 -1711462110.399715 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079ad216e0, 110cd0d:56a27910:57318171:301c3) -1711462110.399720 [0] gc: gc 0x57079b0b7d90: deleting -1711462110.399756 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079aee5fa0, 110cd0d:56a27910:57318171:300c3) -1711462110.399757 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0b7d90, 110cd0d:56a27910:57318171:200c2) -1711462110.399776 [0] gc: gc 0x57079b0b5d00: deleting -1711462110.399780 [0] gc: gc_delete_proxy_reader (0x57079b0b5d00, 110cd0d:56a27910:57318171:4c7) -1711462110.399784 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=16 -1711462110.399789 [0] gc: gc 0x57079b099170: deleting -1711462110.399791 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b0b7d90, 110cd0d:56a27910:57318171:200c2) -1711462110.399793 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b099170, 110cd0d:56a27910:57318171:4c2) -1711462110.399810 [0] gc: gc 0x57079b0b7030: deleting -1711462110.399813 [0] gc: gc_delete_proxy_reader (0x57079b0b7030, 110cd0d:56a27910:57318171:3c7) -1711462110.399818 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=15 -1711462110.399824 [0] gc: gc 0x57079b0b6d10: deleting -1711462110.399817 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b099170, 110cd0d:56a27910:57318171:4c2) -1711462110.399828 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0b6d10, 110cd0d:56a27910:57318171:3c2) -1711462110.399844 [0] gc: gc 0x57079b0b0070: deleting -1711462110.399848 [0] gc: gc_delete_proxy_reader (0x57079b0b0070, 110cd0d:56a27910:57318171:100c7) -1711462110.399851 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b0b6d10, 110cd0d:56a27910:57318171:3c2) -1711462110.399852 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=14 -1711462110.399867 [0] gc: gc 0x57079b0c74c0: deleting -1711462110.399871 [0] gc: gc_delete_proxy_participant(0x57079b0c74c0, 110cd0d:56a27910:57318171:1c1) -1711462110.399875 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=13 -1711462110.399879 [0] gc: gc 0x57079b0a9240: deleting -1711462110.399883 [0] gc: gc_delete_proxy_reader (0x57079b0a9240, 110cd0d:e3b81b8d:1ccb65b1:1604) -1711462110.399888 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=29 -1711462110.399893 [0] gc: gc 0x57079b0638a0: deleting -1711462110.399897 [0] gc: gc_delete_proxy_reader (0x57079b0638a0, 110cd0d:e3b81b8d:1ccb65b1:1504) -1711462110.399901 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=28 -1711462110.399906 [0] gc: gc 0x57079b0c5960: deleting -1711462110.399910 [0] gc: gc_delete_proxy_reader (0x57079b0c5960, 110cd0d:e3b81b8d:1ccb65b1:1404) -1711462110.399913 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=27 -1711462110.399918 [0] gc: gc 0x57079b082a00: deleting -1711462110.399922 [0] gc: gc_delete_proxy_reader (0x57079b082a00, 110cd0d:e3b81b8d:1ccb65b1:1304) -1711462110.399926 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=26 -1711462110.399931 [0] gc: gc 0x57079b0c3380: deleting -1711462110.399935 [0] gc: gc_delete_proxy_reader (0x57079b0c3380, 110cd0d:e3b81b8d:1ccb65b1:1104) -1711462110.399939 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=25 -1711462110.399945 [0] gc: gc 0x57079b069900: deleting -1711462110.399949 [0] gc: gc_delete_proxy_reader (0x57079b069900, 110cd0d:e3b81b8d:1ccb65b1:f04) -1711462110.399952 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=24 -1711462110.399957 [0] gc: gc 0x57079b0cf4a0: deleting -1711462110.399961 [0] gc: gc_delete_proxy_reader (0x57079b0cf4a0, 110cd0d:e3b81b8d:1ccb65b1:d04) -1711462110.399965 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=23 -1711462110.399970 [0] gc: gc 0x57079b0c5570: deleting -1711462110.399974 [0] gc: gc_delete_proxy_reader (0x57079b0c5570, 110cd0d:e3b81b8d:1ccb65b1:b04) -1711462110.399977 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=22 -1711462110.399983 [0] gc: gc 0x57079b08ed60: deleting -1711462110.399987 [0] gc: gc_delete_proxy_reader (0x57079b08ed60, 110cd0d:e3b81b8d:1ccb65b1:904) -1711462110.399991 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=21 -1711462110.399996 [0] gc: gc 0x57079b07b4e0: deleting -1711462110.400000 [0] gc: gc_delete_proxy_reader (0x57079b07b4e0, 110cd0d:e3b81b8d:1ccb65b1:504) -1711462110.400003 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=20 -1711462110.400008 [0] gc: gc 0x57079b0cf020: deleting -1711462110.400012 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0cf020, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.400018 [0] gc: gc 0x57079b183b70: deleting -1711462110.400022 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b183b70, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.400026 [0] gc: gc 0x57079b2089b0: deleting -1711462110.400029 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b2089b0, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.400033 [0] gc: gc 0x57079b205de0: deleting -1711462110.400035 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b0cf020, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.400037 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b205de0, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.400056 [0] gc: gc 0x57079b201610: deleting -1711462110.400062 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b201610, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.400067 [0] gc: gc 0x57079b200930: deleting -1711462110.400055 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b183b70, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.400072 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b200930, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.400077 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b2089b0, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.400085 [0] gc: gc 0x57079b1fc2c0: deleting -1711462110.400090 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b1fc2c0, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.400096 [0] gc: gc 0x57079b0d1fb0: deleting -1711462110.400101 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0d1fb0, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.400106 [0] gc: gc 0x57079b0d83e0: deleting -1711462110.400111 [0] gc: gc_delete_proxy_reader (0x57079b0d83e0, 110cd0d:e3b81b8d:1ccb65b1:301c4) -1711462110.400117 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=19 -1711462110.400123 [0] gc: gc 0x57079b0d1c90: deleting -1711462110.400128 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0d1c90, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.400134 [0] gc: gc 0x57079b0d8140: deleting -1711462110.400137 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b205de0, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.400142 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b0d1c90, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.400144 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b201610, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.400138 [0] gc: gc_delete_proxy_reader (0x57079b0d8140, 110cd0d:e3b81b8d:1ccb65b1:300c4) -1711462110.400160 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b200930, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.400165 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=18 -1711462110.400175 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b1fc2c0, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.400181 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b0d1fb0, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.400192 [0] gc: gc 0x57079b0df1a0: deleting -1711462110.400197 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0df1a0, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.400202 [0] gc: gc 0x57079b0ef820: deleting -1711462110.400206 [0] gc: gc_delete_proxy_reader (0x57079b0ef820, 110cd0d:e3b81b8d:1ccb65b1:200c7) -1711462110.400210 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=17 -1711462110.400216 [0] gc: gc 0x57079b0b4fe0: deleting -1711462110.400220 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0b4fe0, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.400210 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b0df1a0, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.400225 [0] gc: gc 0x57079b023580: deleting -1711462110.400241 [0] gc: gc_delete_proxy_reader (0x57079b023580, 110cd0d:e3b81b8d:1ccb65b1:4c7) -1711462110.400235 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b0b4fe0, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.400246 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=16 -1711462110.400261 [0] gc: gc 0x57079b09b390: deleting -1711462110.400265 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b09b390, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.400270 [0] gc: gc 0x57079b09cff0: deleting -1711462110.400274 [0] gc: gc_delete_proxy_reader (0x57079b09cff0, 110cd0d:e3b81b8d:1ccb65b1:3c7) -1711462110.400277 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b09b390, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.400278 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=15 -1711462110.400292 [0] gc: gc 0x57079b1018e0: deleting -1711462110.400296 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b1018e0, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.400301 [0] gc: gc 0x57079b122900: deleting -1711462110.400305 [0] gc: gc_delete_proxy_reader (0x57079b122900, 110cd0d:e3b81b8d:1ccb65b1:100c7) -1711462110.400308 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b1018e0, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.400309 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=14 -1711462110.400325 [0] gc: gc 0x57079b070ec0: deleting -1711462110.400329 [0] gc: gc_delete_proxy_participant(0x57079b070ec0, 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.400333 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=13 -1711462110.400337 [0] gc: gc 0x57079b1225e0: deleting -1711462110.400340 [0] gc: gc_delete_proxy_reader (0x57079b1225e0, 110cd0d:79d6eeac:ea4a8907:1604) -1711462110.400345 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=29 -1711462110.400350 [0] gc: gc 0x57079b094f50: deleting -1711462110.400354 [0] gc: gc_delete_proxy_reader (0x57079b094f50, 110cd0d:79d6eeac:ea4a8907:1504) -1711462110.400358 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=28 -1711462110.400363 [0] gc: gc 0x57079b17c500: deleting -1711462110.400367 [0] gc: gc_delete_proxy_reader (0x57079b17c500, 110cd0d:79d6eeac:ea4a8907:1404) -1711462110.400370 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=27 -1711462110.400375 [0] gc: gc 0x57079b0bd430: deleting -1711462110.400379 [0] gc: gc_delete_proxy_reader (0x57079b0bd430, 110cd0d:79d6eeac:ea4a8907:1304) -1711462110.400383 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=26 -1711462110.400388 [0] gc: gc 0x57079b0b36e0: deleting -1711462110.400393 [0] gc: gc_delete_proxy_reader (0x57079b0b36e0, 110cd0d:79d6eeac:ea4a8907:1104) -1711462110.400397 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=25 -1711462110.400402 [0] gc: gc 0x57079b0fb550: deleting -1711462110.400406 [0] gc: gc_delete_proxy_reader (0x57079b0fb550, 110cd0d:79d6eeac:ea4a8907:f04) -1711462110.400410 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=24 -1711462110.400414 [0] gc: gc 0x57079b1c1ea0: deleting -1711462110.400418 [0] gc: gc_delete_writer(0x57079b1c1ea0, 110e21c:d0762c48:a9f0fb28:100c2) -1711462110.400427 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:100c2 @ 0x57079b1147c4) user 0 builtin 11 -1711462110.400431 [0] gc: gc 0x57079b128510: deleting -1711462110.400435 [0] gc: gc_delete_proxy_reader (0x57079b128510, 110cd0d:79d6eeac:ea4a8907:d04) -1711462110.400439 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=23 -1711462110.400445 [0] gc: gc 0x57079aaf1c00: deleting -1711462110.400449 [0] gc: gc_delete_writer(0x57079aaf1c00, 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.400459 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:3c2 @ 0x57079b126a44) user 0 builtin 10 -1711462110.400463 [0] gc: gc 0x7a833800f9b0: deleting -1711462110.400466 [0] gc: gc_delete_writer(0x7a833800f9b0, 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.400475 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:4c2 @ 0x57079b1264f4) user 0 builtin 9 -1711462110.400479 [0] gc: gc 0x57079b0e8810: deleting -1711462110.400482 [0] gc: gc_delete_proxy_reader (0x57079b0e8810, 110cd0d:79d6eeac:ea4a8907:b04) -1711462110.400487 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=22 -1711462110.400492 [0] gc: gc 0x7a833800b200: deleting -1711462110.400507 [0] gc: gc_delete_writer(0x7a833800b200, 110e21c:d0762c48:a9f0fb28:200c2) -1711462110.400526 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:200c2 @ 0x57079b127024) user 0 builtin 8 -1711462110.400530 [0] gc: gc 0x7a8338006ae0: deleting -1711462110.400534 [0] gc: gc_delete_reader(0x7a8338006ae0, 110e21c:d0762c48:a9f0fb28:100c7) -1711462110.400540 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:100c7 @ 0x57079b026f94) user 0 builtin 7 -1711462110.400544 [0] gc: gc 0x57079b0e47b0: deleting -1711462110.400548 [0] gc: gc_delete_proxy_reader (0x57079b0e47b0, 110cd0d:79d6eeac:ea4a8907:904) -1711462110.400552 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=21 -1711462110.400557 [0] gc: gc 0x7a8338005430: deleting -1711462110.400560 [0] gc: gc_delete_reader(0x7a8338005430, 110e21c:d0762c48:a9f0fb28:3c7) -1711462110.400567 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:3c7 @ 0x57079ae13da4) user 0 builtin 6 -1711462110.400571 [0] gc: gc 0x57079b1143b0: deleting -1711462110.400575 [0] gc: gc_delete_reader(0x57079b1143b0, 110e21c:d0762c48:a9f0fb28:4c7) -1711462110.400582 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:4c7 @ 0x57079ac95bd4) user 0 builtin 5 -1711462110.400586 [0] gc: gc 0x7a83400033a0: deleting -1711462110.400589 [0] gc: gc_delete_reader(0x7a83400033a0, 110e21c:d0762c48:a9f0fb28:200c7) -1711462110.400595 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:200c7 @ 0x57079acfde94) user 0 builtin 4 -1711462110.400599 [0] gc: gc 0x57079b06dfc0: deleting -1711462110.400603 [0] gc: gc_delete_proxy_reader (0x57079b06dfc0, 110cd0d:79d6eeac:ea4a8907:504) -1711462110.400608 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=20 -1711462110.400613 [0] gc: gc 0x7a83400035d0: deleting -1711462110.400616 [0] gc: gc_delete_writer(0x7a83400035d0, 110e21c:d0762c48:a9f0fb28:300c3) -1711462110.400624 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:300c3 @ 0x57079b127614) user 0 builtin 3 -1711462110.400628 [0] gc: gc 0x57079b110d00: deleting -1711462110.400632 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b110d00, 110cd0d:79d6eeac:ea4a8907:1203) -1711462110.400638 [0] gc: gc 0x7a8340003800: deleting -1711462110.400642 [0] gc: gc_delete_writer(0x7a8340003800, 110e21c:d0762c48:a9f0fb28:301c3) -1711462110.400653 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:301c3 @ 0x57079b127bf4) user 0 builtin 2 -1711462110.400656 [0] gc: gc 0x7a8340003a30: deleting -1711462110.400660 [0] gc: gc_delete_reader(0x7a8340003a30, 110e21c:d0762c48:a9f0fb28:300c4) -1711462110.400658 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b110d00, 110cd0d:79d6eeac:ea4a8907:1203) -1711462110.400668 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:300c4 @ 0x57079ae262f4) user 0 builtin 1 -1711462110.400691 [0] gc: gc 0x7a8340003c60: deleting -1711462110.400695 [0] gc: gc_delete_reader(0x7a8340003c60, 110e21c:d0762c48:a9f0fb28:301c4) -1711462110.400701 [0] gc: ddsi_unref_participant(110e21c:d0762c48:a9f0fb28:1c1 @ 0x57079ac6b340 <- 110e21c:d0762c48:a9f0fb28:301c4 @ 0x57079ad7d3a4) user 0 builtin 0 -1711462110.400710 [0] gc: ddsi_remove_deleted_participant_guid(110e21c:d0762c48:a9f0fb28:1c1 for_what=1) -1711462110.400715 [0] gc: gc 0x57079b169860: deleting -1711462110.400718 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b169860, 110cd0d:79d6eeac:ea4a8907:1003) -1711462110.400723 [0] gc: gc 0x57079b10d910: deleting -1711462110.400727 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b10d910, 110cd0d:79d6eeac:ea4a8907:e03) -1711462110.400729 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b169860, 110cd0d:79d6eeac:ea4a8907:1003) -1711462110.400738 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b10d910, 110cd0d:79d6eeac:ea4a8907:e03) -1711462110.400731 [0] gc: gc 0x57079b0f2b00: deleting -1711462110.400750 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0f2b00, 110cd0d:79d6eeac:ea4a8907:c03) -1711462110.400755 [0] gc: gc 0x57079b0eed30: deleting -1711462110.400759 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0eed30, 110cd0d:79d6eeac:ea4a8907:a03) -1711462110.400764 [0] gc: gc 0x57079b17a790: deleting -1711462110.400769 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b17a790, 110cd0d:79d6eeac:ea4a8907:803) -1711462110.400774 [0] gc: gc 0x57079b0ede60: deleting -1711462110.400780 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b0ede60, 110cd0d:79d6eeac:ea4a8907:703) -1711462110.400760 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b0f2b00, 110cd0d:79d6eeac:ea4a8907:c03) -1711462110.400783 [0] gc: gc 0x57079b168e30: deleting -1711462110.400797 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b168e30, 110cd0d:79d6eeac:ea4a8907:403) -1711462110.400802 [0] gc: gc 0x57079b103270: deleting -1711462110.400808 [0] gc: gc_delete_proxy_reader (0x57079b103270, 110cd0d:79d6eeac:ea4a8907:301c4) -1711462110.400814 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=19 -1711462110.400820 [0] gc: gc 0x57079b188db0: deleting -1711462110.400825 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b188db0, 110cd0d:79d6eeac:ea4a8907:301c3) -1711462110.400791 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b0eed30, 110cd0d:79d6eeac:ea4a8907:a03) -1711462110.400833 [0] gc: gc 0x57079b1817c0: deleting -1711462110.400841 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b17a790, 110cd0d:79d6eeac:ea4a8907:803) -1711462110.400849 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b0ede60, 110cd0d:79d6eeac:ea4a8907:703) -1711462110.400856 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b168e30, 110cd0d:79d6eeac:ea4a8907:403) -1711462110.400842 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b188db0, 110cd0d:79d6eeac:ea4a8907:301c3) -1711462110.400891 [0] gc: gc_delete_proxy_reader (0x57079b1817c0, 110cd0d:79d6eeac:ea4a8907:300c4) -1711462110.400896 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=18 -1711462110.400901 [0] gc: gc 0x57079b203ae0: deleting -1711462110.400904 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b203ae0, 110cd0d:79d6eeac:ea4a8907:300c3) -1711462110.400910 [0] gc: gc 0x57079b1fe9f0: deleting -1711462110.400913 [0] gc: gc_delete_proxy_reader (0x57079b1fe9f0, 110cd0d:79d6eeac:ea4a8907:200c7) -1711462110.400917 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=17 -1711462110.400917 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b203ae0, 110cd0d:79d6eeac:ea4a8907:300c3) -1711462110.400924 [0] gc: gc 0x57079b100f50: deleting -1711462110.400930 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b100f50, 110cd0d:79d6eeac:ea4a8907:200c2) -1711462110.400933 [0] gc: gc 0x57079b15ca40: deleting -1711462110.400937 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b100f50, 110cd0d:79d6eeac:ea4a8907:200c2) -1711462110.400938 [0] gc: gc_delete_proxy_reader (0x57079b15ca40, 110cd0d:79d6eeac:ea4a8907:4c7) -1711462110.400952 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=16 -1711462110.400958 [0] gc: gc 0x57079b1b2a90: deleting -1711462110.400961 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b1b2a90, 110cd0d:79d6eeac:ea4a8907:4c2) -1711462110.400966 [0] gc: gc 0x57079b0c01c0: deleting -1711462110.400970 [0] gc: gc_delete_proxy_reader (0x57079b0c01c0, 110cd0d:79d6eeac:ea4a8907:3c7) -1711462110.400973 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b1b2a90, 110cd0d:79d6eeac:ea4a8907:4c2) -1711462110.400974 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=15 -1711462110.400990 [0] gc: gc 0x57079abe50e0: deleting -1711462110.400993 [0] gc: gc_delete_proxy_writer_dqueue(0x57079abe50e0, 110cd0d:79d6eeac:ea4a8907:3c2) -1711462110.400998 [0] gc: gc 0x57079b18d360: deleting -1711462110.401002 [0] gc: gc_delete_proxy_reader (0x57079b18d360, 110cd0d:79d6eeac:ea4a8907:100c7) -1711462110.401005 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079abe50e0, 110cd0d:79d6eeac:ea4a8907:3c2) -1711462110.401006 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=14 -1711462110.401022 [0] gc: gc 0x57079b1928e0: deleting -1711462110.401025 [0] gc: gc_delete_proxy_participant(0x57079b1928e0, 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.401029 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=13 -1711462110.401033 [0] gc: gc 0x57079b12cae0: deleting -1711462110.401037 [0] gc: gc_delete_proxy_reader (0x57079b12cae0, 110d7b4:17c5b8c5:94bd0ff4:504) -1711462110.401041 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=13 -1711462110.401046 [0] gc: gc 0x57079b13e610: deleting -1711462110.401050 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b13e610, 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.401055 [0] gc: gc 0x57079b1fb6c0: deleting -1711462110.401059 [0] gc: gc_delete_proxy_reader (0x57079b1fb6c0, 110d7b4:17c5b8c5:94bd0ff4:301c4) -1711462110.401065 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=12 -1711462110.401071 [0] gc: gc 0x57079b1aeb00: deleting -1711462110.401060 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x57079b13e610, 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.401075 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b1aeb00, 110d7b4:17c5b8c5:94bd0ff4:301c3) -1711462110.401090 [0] gc: gc 0x57079b1adfa0: deleting -1711462110.401094 [0] gc: gc_delete_proxy_reader (0x57079b1adfa0, 110d7b4:17c5b8c5:94bd0ff4:300c4) -1711462110.401097 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b1aeb00, 110d7b4:17c5b8c5:94bd0ff4:301c3) -1711462110.401098 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=11 -1711462110.401114 [0] gc: gc 0x57079b1a84f0: deleting -1711462110.401118 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b1a84f0, 110d7b4:17c5b8c5:94bd0ff4:300c3) -1711462110.401123 [0] gc: gc 0x57079b1a7a90: deleting -1711462110.401126 [0] gc: gc_delete_proxy_reader (0x57079b1a7a90, 110d7b4:17c5b8c5:94bd0ff4:200c7) -1711462110.401130 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b1a84f0, 110d7b4:17c5b8c5:94bd0ff4:300c3) -1711462110.401131 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=10 -1711462110.401146 [0] gc: gc 0x57079b1b18f0: deleting -1711462110.401149 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b1b18f0, 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462110.401154 [0] gc: gc 0x57079b19a3e0: deleting -1711462110.401158 [0] gc: gc_delete_proxy_reader (0x57079b19a3e0, 110d7b4:17c5b8c5:94bd0ff4:4c7) -1711462110.401161 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b1b18f0, 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462110.401162 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=9 -1711462110.401178 [0] gc: gc 0x57079b08aff0: deleting -1711462110.401182 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b08aff0, 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.401187 [0] gc: gc 0x57079b0aaaa0: deleting -1711462110.401191 [0] gc: gc_delete_proxy_reader (0x57079b0aaaa0, 110d7b4:17c5b8c5:94bd0ff4:3c7) -1711462110.401194 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b08aff0, 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.401196 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=8 -1711462110.401211 [0] gc: gc 0x57079b07cd60: deleting -1711462110.401214 [0] gc: gc_delete_proxy_writer_dqueue(0x57079b07cd60, 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.401219 [0] gc: gc 0x57079b09a780: deleting -1711462110.401223 [0] gc: gc_delete_proxy_reader (0x57079b09a780, 110d7b4:17c5b8c5:94bd0ff4:100c7) -1711462110.401226 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x57079b07cd60, 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.401227 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=7 -1711462110.401242 [0] gc: gc 0x57079b1af970: deleting -1711462110.401246 [0] gc: gc_delete_proxy_participant(0x57079b1af970, 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.401250 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=6 -1711462110.401253 [0] gc: gc 0x57079b0bcfd0: deleting -1711462110.401257 [0] gc: gc 0x57079b0bd200: deleting -1711462110.401261 [0] gc: gc 0x57079b17b8f0: deleting -1711462110.401265 [0] gc: gc 0x57079b136cd0: deleting -1711462110.401269 [0] gc: gc 0x57079b19e880: deleting -1711462110.401272 [0] gc: gc 0x57079ac6b4c0: deleting -1711462110.401277 [0] gc: gc 0x57079aac0ae0: deleting -1711462110.401281 [0] gc: gc 0x57079ab12e10: deleting -1711462110.401284 [0] gc: gc 0x57079ab4f2e0: deleting -1711462110.401288 [0] gc: gc 0x57079ab4b370: deleting -1711462110.401292 [0] gc: gc 0x57079aac1e10: deleting -1711462110.401296 [0] gc: gc_delete_proxy_writer (0x57079aac1e10, 110cd0d:56a27910:57318171:1203) -1711462110.401300 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=12 -1711462110.401306 [0] gc: gc 0x57079aabd660: deleting -1711462110.401310 [0] gc: gc_delete_proxy_writer (0x57079aabd660, 110cd0d:56a27910:57318171:1003) -1711462110.401314 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=11 -1711462110.401320 [0] gc: gc 0x57079ab544e0: deleting -1711462110.401324 [0] gc: gc_delete_proxy_writer (0x57079ab544e0, 110cd0d:56a27910:57318171:e03) -1711462110.401328 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=10 -1711462110.401333 [0] gc: gc 0x57079ac22c30: deleting -1711462110.401337 [0] gc: gc_delete_proxy_writer (0x57079ac22c30, 110cd0d:56a27910:57318171:c03) -1711462110.401341 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=9 -1711462110.401346 [0] gc: gc 0x57079abec600: deleting -1711462110.401350 [0] gc: gc_delete_proxy_writer (0x57079abec600, 110cd0d:56a27910:57318171:a03) -1711462110.401354 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=8 -1711462110.401359 [0] gc: gc 0x57079ac3b6e0: deleting -1711462110.401362 [0] gc: gc_delete_proxy_writer (0x57079ac3b6e0, 110cd0d:56a27910:57318171:803) -1711462110.401367 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=7 -1711462110.401372 [0] gc: gc 0x57079abf1850: deleting -1711462110.401375 [0] gc: gc_delete_proxy_writer (0x57079abf1850, 110cd0d:56a27910:57318171:703) -1711462110.401379 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=6 -1711462110.401384 [0] gc: gc 0x57079abfb210: deleting -1711462110.401388 [0] gc: gc_delete_proxy_writer (0x57079abfb210, 110cd0d:56a27910:57318171:403) -1711462110.401392 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=5 -1711462110.401397 [0] gc: gc 0x57079ad216e0: deleting -1711462110.401401 [0] gc: gc_delete_proxy_writer (0x57079ad216e0, 110cd0d:56a27910:57318171:301c3) -1711462110.401406 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:301c4, 2): reader no longer exists -1711462110.401410 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=4 -1711462110.401414 [0] gc: gc 0x57079aee5fa0: deleting -1711462110.401418 [0] gc: gc_delete_proxy_writer (0x57079aee5fa0, 110cd0d:56a27910:57318171:300c3) -1711462110.401422 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:300c4, 2): reader no longer exists -1711462110.401426 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=3 -1711462110.401431 [0] gc: gc 0x57079b0b7d90: deleting -1711462110.401435 [0] gc: gc_delete_proxy_writer (0x57079b0b7d90, 110cd0d:56a27910:57318171:200c2) -1711462110.401439 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:200c7, 5): reader no longer exists -1711462110.401443 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=2 -1711462110.401447 [0] gc: gc 0x57079b099170: deleting -1711462110.401450 [0] gc: gc_delete_proxy_writer (0x57079b099170, 110cd0d:56a27910:57318171:4c2) -1711462110.401455 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:4c7, 3): reader no longer exists -1711462110.401459 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=1 -1711462110.401463 [0] gc: gc 0x57079b0b6d10: deleting -1711462110.401466 [0] gc: gc_delete_proxy_writer (0x57079b0b6d10, 110cd0d:56a27910:57318171:3c2) -1711462110.401471 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:3c7, 4): reader no longer exists -1711462110.401475 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=0, freeing -1711462110.401479 [0] gc: lease_unregister(l 0x7a8338001c90 guid 110cd0d:56a27910:57318171:1c1) -1711462110.401484 [0] gc: lease_free(l 0x7a8338001c90 guid 110cd0d:56a27910:57318171:1c1) -1711462110.401487 [0] gc: lease_free(l 0x7a8338001c10 guid 110cd0d:56a27910:57318171:1c1) -1711462110.401494 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:56a27910:57318171:1c1 for_what=3) -1711462110.401500 [0] gc: gc 0x57079b0c74c0: deleting -1711462110.401504 [0] gc: gc 0x57079b0638a0: deleting -1711462110.401508 [0] gc: gc 0x57079b0c5960: deleting -1711462110.401512 [0] gc: gc 0x57079b082a00: deleting -1711462110.401516 [0] gc: gc 0x57079b0c3380: deleting -1711462110.401520 [0] gc: gc 0x57079b069900: deleting -1711462110.401523 [0] gc: gc 0x57079b0cf4a0: deleting -1711462110.401527 [0] gc: gc 0x57079b0c5570: deleting -1711462110.401531 [0] gc: gc 0x57079b08ed60: deleting -1711462110.401534 [0] gc: gc 0x57079b07b4e0: deleting -1711462110.401538 [0] gc: gc 0x57079b0cf020: deleting -1711462110.401542 [0] gc: gc_delete_proxy_writer (0x57079b0cf020, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.401546 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=12 -1711462110.401551 [0] gc: gc 0x57079b183b70: deleting -1711462110.401555 [0] gc: gc_delete_proxy_writer (0x57079b183b70, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.401559 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=11 -1711462110.401564 [0] gc: gc 0x57079b2089b0: deleting -1711462110.401568 [0] gc: gc_delete_proxy_writer (0x57079b2089b0, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.401572 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=10 -1711462110.401577 [0] gc: gc 0x57079b205de0: deleting -1711462110.401581 [0] gc: gc_delete_proxy_writer (0x57079b205de0, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.401585 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=9 -1711462110.401589 [0] gc: gc 0x57079b0d1c90: deleting -1711462110.401593 [0] gc: gc_delete_proxy_writer (0x57079b0d1c90, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.401597 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:301c4, 2): reader no longer exists -1711462110.401601 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=8 -1711462110.401605 [0] gc: gc 0x57079b201610: deleting -1711462110.401609 [0] gc: gc_delete_proxy_writer (0x57079b201610, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.401613 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=7 -1711462110.401618 [0] gc: gc 0x57079b200930: deleting -1711462110.401622 [0] gc: gc_delete_proxy_writer (0x57079b200930, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.401626 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=6 -1711462110.401631 [0] gc: gc 0x57079b1fc2c0: deleting -1711462110.401635 [0] gc: gc_delete_proxy_writer (0x57079b1fc2c0, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.401639 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=5 -1711462110.401644 [0] gc: gc 0x57079b0d1fb0: deleting -1711462110.401648 [0] gc: gc_delete_proxy_writer (0x57079b0d1fb0, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.401652 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=4 -1711462110.401657 [0] gc: gc 0x57079b0df1a0: deleting -1711462110.401660 [0] gc: gc_delete_proxy_writer (0x57079b0df1a0, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.401664 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:300c4, 2): reader no longer exists -1711462110.401668 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=3 -1711462110.401673 [0] gc: gc 0x57079b0b4fe0: deleting -1711462110.401676 [0] gc: gc_delete_proxy_writer (0x57079b0b4fe0, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.401680 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:200c7, 6): reader no longer exists -1711462110.401684 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=2 -1711462110.401688 [0] gc: gc 0x57079b09b390: deleting -1711462110.401693 [0] gc: gc_delete_proxy_writer (0x57079b09b390, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.401697 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:4c7, 3): reader no longer exists -1711462110.401701 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=1 -1711462110.401705 [0] gc: gc 0x57079b1018e0: deleting -1711462110.401709 [0] gc: gc_delete_proxy_writer (0x57079b1018e0, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.401713 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:3c7, 3): reader no longer exists -1711462110.401717 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=0, freeing -1711462110.401721 [0] gc: lease_unregister(l 0x7a83380065d0 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.401725 [0] gc: lease_free(l 0x7a83380065d0 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.401729 [0] gc: lease_free(l 0x7a8338006550 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.401734 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:e3b81b8d:1ccb65b1:1c1 for_what=3) -1711462110.401739 [0] gc: gc 0x57079b0d83e0: deleting -1711462110.401742 [0] gc: gc 0x57079b094f50: deleting -1711462110.401746 [0] gc: gc 0x57079b17c500: deleting -1711462110.401750 [0] gc: gc 0x57079b0bd430: deleting -1711462110.401754 [0] gc: gc 0x57079b0b36e0: deleting -1711462110.401758 [0] gc: gc 0x57079b0fb550: deleting -1711462110.401761 [0] gc: gc 0x57079b1c1ea0: deleting -1711462110.401765 [0] gc: gc 0x57079aaf1c00: deleting -1711462110.401769 [0] gc: gc 0x7a833800b200: deleting -1711462110.401773 [0] gc: gc 0x7a8338005430: deleting -1711462110.401777 [0] gc: gc 0x57079b110d00: deleting -1711462110.401780 [0] gc: gc_delete_proxy_writer (0x57079b110d00, 110cd0d:79d6eeac:ea4a8907:1203) -1711462110.401785 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=12 -1711462110.401790 [0] gc: gc 0x7a83400035d0: deleting -1711462110.401794 [0] gc: gc 0x57079b169860: deleting -1711462110.401798 [0] gc: gc_delete_proxy_writer (0x57079b169860, 110cd0d:79d6eeac:ea4a8907:1003) -1711462110.401802 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=11 -1711462110.401807 [0] gc: gc 0x57079b10d910: deleting -1711462110.401811 [0] gc: gc_delete_proxy_writer (0x57079b10d910, 110cd0d:79d6eeac:ea4a8907:e03) -1711462110.401815 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=10 -1711462110.401820 [0] gc: gc 0x57079b0f2b00: deleting -1711462110.401824 [0] gc: gc_delete_proxy_writer (0x57079b0f2b00, 110cd0d:79d6eeac:ea4a8907:c03) -1711462110.401828 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=9 -1711462110.401833 [0] gc: gc 0x57079b0eed30: deleting -1711462110.401836 [0] gc: gc_delete_proxy_writer (0x57079b0eed30, 110cd0d:79d6eeac:ea4a8907:a03) -1711462110.401840 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=8 -1711462110.401846 [0] gc: gc 0x57079b17a790: deleting -1711462110.401849 [0] gc: gc_delete_proxy_writer (0x57079b17a790, 110cd0d:79d6eeac:ea4a8907:803) -1711462110.401853 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=7 -1711462110.401858 [0] gc: gc 0x57079b0ede60: deleting -1711462110.401862 [0] gc: gc_delete_proxy_writer (0x57079b0ede60, 110cd0d:79d6eeac:ea4a8907:703) -1711462110.401866 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=6 -1711462110.401871 [0] gc: gc 0x57079b168e30: deleting -1711462110.401874 [0] gc: gc_delete_proxy_writer (0x57079b168e30, 110cd0d:79d6eeac:ea4a8907:403) -1711462110.401879 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=5 -1711462110.401884 [0] gc: gc 0x57079b188db0: deleting -1711462110.401887 [0] gc: gc_delete_proxy_writer (0x57079b188db0, 110cd0d:79d6eeac:ea4a8907:301c3) -1711462110.401893 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:301c4, 2): reader no longer exists -1711462110.401897 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=4 -1711462110.401901 [0] gc: gc 0x57079b203ae0: deleting -1711462110.401904 [0] gc: gc_delete_proxy_writer (0x57079b203ae0, 110cd0d:79d6eeac:ea4a8907:300c3) -1711462110.401908 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:300c4, 2): reader no longer exists -1711462110.401912 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=3 -1711462110.401916 [0] gc: gc 0x57079b100f50: deleting -1711462110.401920 [0] gc: gc_delete_proxy_writer (0x57079b100f50, 110cd0d:79d6eeac:ea4a8907:200c2) -1711462110.401924 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:200c7, 6): reader no longer exists -1711462110.401928 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=2 -1711462110.401932 [0] gc: gc 0x57079b1b2a90: deleting -1711462110.401936 [0] gc: gc_delete_proxy_writer (0x57079b1b2a90, 110cd0d:79d6eeac:ea4a8907:4c2) -1711462110.401940 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:4c7, 3): reader no longer exists -1711462110.401944 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=1 -1711462110.401948 [0] gc: gc 0x57079abe50e0: deleting -1711462110.401952 [0] gc: gc_delete_proxy_writer (0x57079abe50e0, 110cd0d:79d6eeac:ea4a8907:3c2) -1711462110.401956 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:3c7, 3): reader no longer exists -1711462110.401960 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=0, freeing -1711462110.401963 [0] gc: lease_unregister(l 0x7a833800b8d0 guid 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.401967 [0] gc: lease_free(l 0x7a833800b8d0 guid 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.401971 [0] gc: lease_free(l 0x7a833800b850 guid 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.401976 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:79d6eeac:ea4a8907:1c1 for_what=3) -1711462110.401981 [0] gc: gc 0x57079b103270: deleting -1711462110.401985 [0] gc: gc 0x57079b13e610: deleting -1711462110.401989 [0] gc: gc_delete_proxy_writer (0x57079b13e610, 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.401993 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=5 -1711462110.401998 [0] gc: gc 0x57079b1aeb00: deleting -1711462110.402003 [0] gc: gc_delete_proxy_writer (0x57079b1aeb00, 110d7b4:17c5b8c5:94bd0ff4:301c3) -1711462110.402007 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:301c4, 2): reader no longer exists -1711462110.402011 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=4 -1711462110.402015 [0] gc: gc 0x57079b1a84f0: deleting -1711462110.402019 [0] gc: gc_delete_proxy_writer (0x57079b1a84f0, 110d7b4:17c5b8c5:94bd0ff4:300c3) -1711462110.402023 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:300c4, 2): reader no longer exists -1711462110.402027 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=3 -1711462110.402031 [0] gc: gc 0x57079b1b18f0: deleting -1711462110.402035 [0] gc: gc_delete_proxy_writer (0x57079b1b18f0, 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462110.402039 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:200c7, 5): reader no longer exists -1711462110.402043 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=2 -1711462110.402047 [0] gc: gc 0x57079b08aff0: deleting -1711462110.402050 [0] gc: gc_delete_proxy_writer (0x57079b08aff0, 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.402054 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:4c7, 5): reader no longer exists -1711462110.402059 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=1 -1711462110.402063 [0] gc: gc 0x57079b07cd60: deleting -1711462110.402067 [0] gc: gc_delete_proxy_writer (0x57079b07cd60, 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.402071 [0] gc: ddsi_update_reader_init_acknack_count (110e21c:d0762c48:a9f0fb28:3c7, 6): reader no longer exists -1711462110.402075 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=0, freeing -1711462110.402079 [0] gc: lease_unregister(l 0x7a833802f8b0 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.402082 [0] gc: lease_free(l 0x7a833802f8b0 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.402086 [0] gc: lease_free(l 0x7a833802f830 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.402091 [0] gc: ddsi_remove_deleted_participant_guid(110d7b4:17c5b8c5:94bd0ff4:1c1 for_what=3) -1711462110.402096 [0] gc: gc 0x57079b1fb6c0: deleting -1711462110.402100 [0] gc: gc 0x57079aabd660: deleting -1711462110.402104 [0] gc: gc 0x57079ab544e0: deleting -1711462110.402108 [0] gc: gc 0x57079ac22c30: deleting -1711462110.402114 [0] gc: gc 0x57079abec600: deleting -1711462110.402118 [0] gc: gc 0x57079ac3b6e0: deleting -1711462110.402122 [0] gc: gc 0x57079abf1850: deleting -1711462110.402126 [0] gc: gc 0x57079abfb210: deleting -1711462110.402130 [0] gc: gc 0x57079ad216e0: deleting -1711462110.402134 [0] gc: gc 0x57079b0b0070: deleting -1711462110.402138 [0] gc: gc 0x57079b0638a0: deleting -1711462110.402142 [0] gc: gc 0x57079b183b70: deleting -1711462110.402146 [0] gc: gc 0x57079b2089b0: deleting -1711462110.402150 [0] gc: gc 0x57079b205de0: deleting -1711462110.402153 [0] gc: gc 0x57079b0d1c90: deleting -1711462110.402157 [0] gc: gc 0x57079b200930: deleting -1711462110.402161 [0] gc: gc 0x57079b1fc2c0: deleting -1711462110.402165 [0] gc: gc 0x57079b0d1fb0: deleting -1711462110.402169 [0] gc: gc 0x57079b0df1a0: deleting -1711462110.402172 [0] gc: gc 0x57079b0c74c0: deleting -1711462110.402176 [0] gc: gc 0x57079b094f50: deleting -1711462110.402180 [0] gc: gc 0x7a83400035d0: deleting -1711462110.402185 [0] gc: gc 0x57079b10d910: deleting -1711462110.402188 [0] gc: gc 0x57079b0f2b00: deleting -1711462110.402192 [0] gc: gc 0x57079b0eed30: deleting -1711462110.402196 [0] gc: gc 0x57079b17a790: deleting -1711462110.402200 [0] gc: gc 0x57079b0ede60: deleting -1711462110.402204 [0] gc: gc 0x57079b168e30: deleting -1711462110.402208 [0] gc: gc 0x57079b188db0: deleting -1711462110.402212 [0] gc: gc 0x57079b0d83e0: deleting -1711462110.402215 [0] gc: gc 0x57079b13e610: deleting -1711462110.402219 [0] gc: gc 0x57079b1aeb00: deleting -1711462110.402225 [0] gc: gc 0x57079b103270: deleting -1711462110.402276 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:100c2) ... -1711462110.402307 [0] python3: writer_set_state(0:0:0:100c2) state transition 0 -> 3 -1711462110.402349 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:2c2) ... -1711462110.402352 [0] gc: gc 0x57079aaf1c00: not yet, shortsleep -1711462110.402363 [0] python3: writer_set_state(0:0:0:2c2) state transition 0 -> 3 -1711462110.402374 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:3c2) ... -1711462110.402383 [0] python3: writer_set_state(0:0:0:3c2) state transition 0 -> 3 -1711462110.402390 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:4c2) ... -1711462110.402397 [0] python3: writer_set_state(0:0:0:4c2) state transition 0 -> 3 -1711462110.403437 [0] gc: gc 0x57079aaf1c00: deleting -1711462110.403456 [0] gc: gc_delete_writer(0x57079aaf1c00, 0:0:0:100c2) -1711462110.403474 [0] gc: gc 0x57079b1c1ea0: deleting -1711462110.403478 [0] gc: gc_delete_writer(0x57079b1c1ea0, 0:0:0:2c2) -1711462110.403486 [0] gc: gc 0x57079b0a1580: deleting -1711462110.403490 [0] gc: gc_delete_writer(0x57079b0a1580, 0:0:0:3c2) -1711462110.403497 [0] gc: gc 0x57079b1143b0: deleting -1711462110.403501 [0] gc: gc_delete_writer(0x57079b1143b0, 0:0:0:4c2) -1711462110.403520 [0] gc: gc 0x57079b09e2f0: deleting -1711462110.404169 [0] python3: xpack_addmsg 0x7a8328002370 0x7a8338039360 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.405633 [0] python3: nn_xpack_send 96: 0x7a832800237c:20 0x7a8338039978:48 0x7a8338038af4:28 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.0.(trunc) -1711462110.405649 [0] python3: traffic-xmit (101) 96 -1711462110.405672 [0] python3: leave conn 0x57079b0e1c00 for (udp/239.255.0.1, *) interface (default): not leaving yet -1711462110.405690 [0] python3: leave conn 0x57079b0a7650 for (udp/239.255.0.1, *) interface (default): not leaving yet -1711462110.405702 [0] python3: leave conn 0x57079b0e1c00 for (udp/239.255.0.1, *) interface udp/10.101.12.71 -1711462110.405721 [0] python3: leave conn 0x57079b0a7650 for (udp/239.255.0.1, *) interface udp/10.101.12.71 -1711462110.405735 [0] python3: ddsi_udp_release_conn multicast socket 7 port 7400 -1711462110.405764 [0] python3: ddsi_udp_release_conn multicast socket 8 port 7401 -1711462110.405788 [0] python3: ddsi_udp_release_conn unicast socket 5 port 7412 -1711462110.405804 [0] python3: ddsi_udp_release_conn unicast socket 6 port 7413 -1711462110.405817 [0] python3: ddsi_udp_release_conn unicast socket 9 port 58212 -1711462110.405834 [0] python3: udp finalized -1711462110.406163 [0] python3: Finis. -hon3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4003) - no unack'ed samples -1711462110.285152 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4003) ... -1711462110.285171 [0] python3: => EVERYONE -1711462110.285201 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4003) state transition 1 -> 3 -1711462110.286018 [0] gc: gc 0x7590d8014d00: deleting -1711462110.286037 [0] gc: gc 0x556f789bf480: deleting -1711462110.286044 [0] gc: gc_delete_writer(0x556f789bf480, 110d7b4:17c5b8c5:94bd0ff4:4003) -1711462110.286081 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #76: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4003}} -1711462110.286094 [0] gc: non-timed queue now has 1 items -1711462110.286110 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4003 @ 0x556f789ea2e4) user 28 builtin 12 -1711462110.286122 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.286112 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#76/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.286173 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f00035f4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.286178 [0] tev: traffic-xmit (1) 96 -1711462110.286194 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.286220 [0] recv: INFOTS(1711462110.286072873) -1711462110.286239 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #76 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.286401 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #77: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,68,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,70,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0, -1711462110.286432 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#77/1)): niov 0 sz 0 => now niov 3 sz 600 -1711462110.286456 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 77 maxseq 46) -1711462110.286474 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.0268972 s (min-ack 46, avail-seq 77, xmit 76) -1711462110.286488 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 600 => now niov 4 sz 632 -1711462110.286553 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 632 from udp/10.101.12.71:40473 -1711462110.286562 [0] recvMC: INFOTS(1711462110.286193571) -1711462110.286585 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #77 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.286609 [0] recvMC: HEARTBEAT(F#86:77..77 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.286566 [0] python3: nn_xpack_send 632: 0x556f7833352c:20 0x556f78a27198:36 0x556f789bf480:544 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.286647 [0] python3: traffic-xmit (1) 632 -1711462110.286658 [0] python3: => EVERYONE -1711462110.286679 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:4304) ... -1711462110.286691 [0] python3: => EVERYONE -1711462110.287204 [0] gc: gc 0x7590d8014d00: deleting -1711462110.287222 [0] gc: gc 0x556f78b57230: deleting -1711462110.287229 [0] gc: gc_delete_reader(0x556f78b57230, 110d7b4:17c5b8c5:94bd0ff4:4304) -1711462110.287247 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #71: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4304}} -1711462110.287266 [0] gc: non-timed queue now has 1 items -1711462110.287281 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4304 @ 0x556f789f57a4) user 27 builtin 12 -1711462110.287289 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.287291 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#71/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.287336 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003684:28 [ udp/239.255.0.1:7400@2 ] -1711462110.287340 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4203) ... -1711462110.287357 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.287387 [0] recv: INFOTS(1711462110.287238788) -1711462110.287342 [0] tev: traffic-xmit (1) 96 -1711462110.287418 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #71 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.287362 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4203) state transition 0 -> 1 -1711462110.287492 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4203) ... -1711462110.287514 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4203) - no unack'ed samples -1711462110.287525 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4203) ... -1711462110.287547 [0] python3: => EVERYONE -1711462110.287572 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4203) state transition 1 -> 3 -1711462110.288369 [0] gc: gc 0x7590d8014d00: deleting -1711462110.288388 [0] gc: gc 0x556f78a2f640: deleting -1711462110.288394 [0] gc: gc_delete_writer(0x556f78a2f640, 110d7b4:17c5b8c5:94bd0ff4:4203) -1711462110.288417 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #77: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4203}} -1711462110.288431 [0] gc: non-timed queue now has 1 items -1711462110.288446 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4203 @ 0x556f789f28f4) user 26 builtin 12 -1711462110.288448 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#77/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.288455 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.288493 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003714:28 [ udp/239.255.0.1:7400@2 ] -1711462110.288529 [0] tev: traffic-xmit (1) 96 -1711462110.288537 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.288647 [0] recv: INFOTS(1711462110.288409621) -1711462110.288664 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #77 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.288762 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #78: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,70,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0, -1711462110.288813 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#78/1)): niov 0 sz 0 => now niov 3 sz 552 -1711462110.288829 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 78 maxseq 46) -1711462110.288849 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.0245592 s (min-ack 46, avail-seq 78, xmit 77) -1711462110.288869 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 552 => now niov 4 sz 584 -1711462110.288965 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 584 from udp/10.101.12.71:40473 -1711462110.288986 [0] recvMC: INFOTS(1711462110.288540698) -1711462110.288991 [0] python3: nn_xpack_send 584: 0x556f7833352c:20 0x556f78a27198:36 0x556f78b56b20:496 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.289037 [0] python3: traffic-xmit (1) 584 -1711462110.289016 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #78 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.289053 [0] python3: => EVERYONE -1711462110.289072 [0] recvMC: HEARTBEAT(F#87:78..78 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.289095 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:4504) ... -1711462110.289117 [0] python3: => EVERYONE -1711462110.289551 [0] gc: gc 0x7590d8014d00: deleting -1711462110.289567 [0] gc: gc 0x556f789bf480: deleting -1711462110.289574 [0] gc: gc_delete_reader(0x556f789bf480, 110d7b4:17c5b8c5:94bd0ff4:4504) -1711462110.289591 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #72: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4504}} -1711462110.289605 [0] gc: non-timed queue now has 1 items -1711462110.289619 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4504 @ 0x556f789fad74) user 25 builtin 12 -1711462110.289627 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.289639 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#72/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.289679 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4403) ... -1711462110.289690 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4403) state transition 0 -> 1 -1711462110.289702 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4403) ... -1711462110.289712 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4403) - no unack'ed samples -1711462110.289722 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4403) ... -1711462110.289691 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f00037a4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.289745 [0] tev: traffic-xmit (1) 96 -1711462110.289732 [0] python3: => EVERYONE -1711462110.289726 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.289792 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4403) state transition 1 -> 3 -1711462110.289808 [0] recv: INFOTS(1711462110.289583439) -1711462110.289862 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #72 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.290707 [0] gc: gc 0x7590d8014d00: deleting -1711462110.290725 [0] gc: gc 0x556f781e6be0: deleting -1711462110.290732 [0] gc: gc_delete_writer(0x556f781e6be0, 110d7b4:17c5b8c5:94bd0ff4:4403) -1711462110.290754 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #78: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4403}} -1711462110.290764 [0] gc: non-timed queue now has 1 items -1711462110.290784 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4403 @ 0x556f789f9e94) user 24 builtin 12 -1711462110.290803 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.290792 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#78/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.290857 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003834:28 [ udp/239.255.0.1:7400@2 ] -1711462110.290862 [0] tev: traffic-xmit (1) 96 -1711462110.290877 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.290900 [0] recv: INFOTS(1711462110.290746464) -1711462110.290921 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #78 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.291015 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #79: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0, -1711462110.291035 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#79/1)): niov 0 sz 0 => now niov 3 sz 504 -1711462110.291044 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 79 maxseq 46) -1711462110.291057 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.0222207 s (min-ack 46, avail-seq 79, xmit 78) -1711462110.291071 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 504 => now niov 4 sz 536 -1711462110.291130 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 536 from udp/10.101.12.71:40473 -1711462110.291138 [0] python3: nn_xpack_send 536: 0x556f7833352c:20 0x556f78a27198:36 0x556f789382a0:448 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.291152 [0] python3: traffic-xmit (1) 536 -1711462110.291185 [0] python3: => EVERYONE -1711462110.291208 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:4704) ... -1711462110.291221 [0] python3: => EVERYONE -1711462110.291142 [0] recvMC: INFOTS(1711462110.290874452) -1711462110.291270 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #79 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.291282 [0] recvMC: HEARTBEAT(F#88:79..79 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.291887 [0] gc: gc 0x7590d8014d00: deleting -1711462110.291904 [0] gc: gc 0x556f781e6be0: deleting -1711462110.291910 [0] gc: gc_delete_reader(0x556f781e6be0, 110d7b4:17c5b8c5:94bd0ff4:4704) -1711462110.291927 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #73: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4704}} -1711462110.291938 [0] gc: non-timed queue now has 1 items -1711462110.291951 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4704 @ 0x556f78a02a54) user 23 builtin 12 -1711462110.291959 [0] gc: gc 0x7590d8014d00: deleting -1711462110.291971 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#73/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.292031 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f00038c4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.292037 [0] tev: traffic-xmit (1) 96 -1711462110.292036 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4603) ... -1711462110.292049 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.292072 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4603) state transition 0 -> 1 -1711462110.292099 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4603) ... -1711462110.292120 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4603) - no unack'ed samples -1711462110.292134 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4603) ... -1711462110.292150 [0] python3: => EVERYONE -1711462110.292176 [0] recv: INFOTS(1711462110.291919173) -1711462110.292185 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4603) state transition 1 -> 3 -1711462110.292214 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #73 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.292264 [0] gc: gc 0x556f789bf480: deleting -1711462110.292274 [0] gc: gc_delete_writer(0x556f789bf480, 110d7b4:17c5b8c5:94bd0ff4:4603) -1711462110.292294 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #79: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4603}} -1711462110.292306 [0] gc: non-timed queue now has 1 items -1711462110.292323 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4603 @ 0x556f789ff6a4) user 22 builtin 12 -1711462110.292329 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#79/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.292331 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.292367 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003954:28 [ udp/239.255.0.1:7400@2 ] -1711462110.292374 [0] tev: traffic-xmit (1) 96 -1711462110.292380 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.292408 [0] recv: INFOTS(1711462110.292287600) -1711462110.292431 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #79 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.292604 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #80: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0, -1711462110.292641 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#80/1)): niov 0 sz 0 => now niov 3 sz 456 -1711462110.292652 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 80 maxseq 46) -1711462110.292677 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.0206795 s (min-ack 46, avail-seq 80, xmit 79) -1711462110.292724 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 456 => now niov 4 sz 488 -1711462110.292816 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 488 from udp/10.101.12.71:40473 -1711462110.292826 [0] recvMC: INFOTS(1711462110.292409838) -1711462110.292836 [0] python3: nn_xpack_send 488: 0x556f7833352c:20 0x556f78a27198:36 0x556f78b70f30:400 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.292861 [0] python3: traffic-xmit (1) 488 -1711462110.292876 [0] python3: => EVERYONE -1711462110.292842 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #80 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.292900 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:4904) ... -1711462110.292908 [0] recvMC: HEARTBEAT(F#89:80..80 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.292916 [0] python3: => EVERYONE -1711462110.293426 [0] gc: gc 0x7590d8014d00: deleting -1711462110.293444 [0] gc: gc 0x556f789ecab0: deleting -1711462110.293453 [0] gc: gc_delete_reader(0x556f789ecab0, 110d7b4:17c5b8c5:94bd0ff4:4904) -1711462110.293471 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #74: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4904}} -1711462110.293485 [0] gc: non-timed queue now has 1 items -1711462110.293503 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4904 @ 0x556f78a07a14) user 21 builtin 12 -1711462110.293513 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.293512 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#74/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.293552 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4803) ... -1711462110.293567 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4803) state transition 0 -> 1 -1711462110.293580 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4803) ... -1711462110.293587 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f00039e4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.293591 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4803) - no unack'ed samples -1711462110.293619 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4803) ... -1711462110.293647 [0] python3: => EVERYONE -1711462110.293597 [0] tev: traffic-xmit (1) 96 -1711462110.293600 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.293836 [0] recv: INFOTS(1711462110.293464039) -1711462110.293865 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #74 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.293875 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4803) state transition 1 -> 3 -1711462110.293962 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.294011 [0] recv: HEARTBEAT(#10:1..15 L(:1c1 16957.953485)110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:4c7@15(sync)) -1711462110.294072 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#4:16/0: -1711462110.294132 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.294151 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.294202 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7412@2 ] -1711462110.294211 [0] tev: traffic-xmit (1) 64 -1711462110.294597 [0] gc: gc 0x7590d8014d00: deleting -1711462110.294615 [0] gc: gc 0x556f78a2f640: deleting -1711462110.294621 [0] gc: gc_delete_writer(0x556f78a2f640, 110d7b4:17c5b8c5:94bd0ff4:4803) -1711462110.294641 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #80: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4803}} -1711462110.294670 [0] gc: non-timed queue now has 1 items -1711462110.294684 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4803 @ 0x556f78a06964) user 20 builtin 12 -1711462110.294697 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.294686 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#80/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.294771 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.294778 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003a74:28 [ udp/239.255.0.1:7400@2 ] -1711462110.294819 [0] tev: traffic-xmit (1) 96 -1711462110.294794 [0] recv: INFOTS(1711462110.294634250) -1711462110.294862 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #80 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.294976 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #81: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.295018 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#81/1)): niov 0 sz 0 => now niov 3 sz 408 -1711462110.295035 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 81 maxseq 46) -1711462110.295058 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.018305 s (min-ack 46, avail-seq 81, xmit 80) -1711462110.295073 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 408 => now niov 4 sz 440 -1711462110.295150 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 440 from udp/10.101.12.71:40473 -1711462110.295161 [0] recvMC: INFOTS(1711462110.294787650) -1711462110.295177 [0] python3: nn_xpack_send 440: 0x556f7833352c:20 0x556f78a27198:36 0x556f78b56d20:352 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.295216 [0] python3: traffic-xmit (1) 440 -1711462110.295188 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #81 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.295240 [0] python3: => EVERYONE -1711462110.295263 [0] recvMC: HEARTBEAT(F#90:81..81 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.295268 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:4b04) ... -1711462110.295303 [0] python3: => EVERYONE -1711462110.295509 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.295543 [0] recv: HEARTBEAT(#10:1..19 L(:1c1 16957.955027)110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:3c7@19(sync)) -1711462110.295581 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#4:20/0: -1711462110.295613 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.295627 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.295694 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7412@2 ] -1711462110.295715 [0] tev: traffic-xmit (1) 64 -1711462110.295765 [0] gc: gc 0x7590d8014d00: deleting -1711462110.295774 [0] gc: gc 0x556f78b57230: deleting -1711462110.295780 [0] gc: gc_delete_reader(0x556f78b57230, 110d7b4:17c5b8c5:94bd0ff4:4b04) -1711462110.295797 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #75: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4b04}} -1711462110.295815 [0] gc: non-timed queue now has 1 items -1711462110.295828 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4b04 @ 0x556f78a0e114) user 19 builtin 12 -1711462110.295835 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.295835 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#75/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.295880 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4a03) ... -1711462110.295897 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4a03) state transition 0 -> 1 -1711462110.295908 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4a03) ... -1711462110.295918 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4a03) - no unack'ed samples -1711462110.295923 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.295944 [0] recv: INFOTS(1711462110.295791082) -1711462110.295969 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #75 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.295930 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4a03) ... -1711462110.295918 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003b04:28 [ udp/239.255.0.1:7400@2 ] -1711462110.296026 [0] tev: traffic-xmit (1) 96 -1711462110.296013 [0] python3: => EVERYONE -1711462110.296070 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4a03) state transition 1 -> 3 -1711462110.296918 [0] gc: gc 0x7590d8014d00: deleting -1711462110.296948 [0] gc: gc 0x556f78b57230: deleting -1711462110.296960 [0] gc: gc_delete_writer(0x556f78b57230, 110d7b4:17c5b8c5:94bd0ff4:4a03) -1711462110.296977 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #81: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4a03}} -1711462110.296999 [0] gc: non-timed queue now has 1 items -1711462110.297013 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4a03 @ 0x556f78a0ce54) user 18 builtin 12 -1711462110.297016 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#81/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.297020 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.297066 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003b94:28 [ udp/239.255.0.1:7400@2 ] -1711462110.297072 [0] tev: traffic-xmit (1) 96 -1711462110.297078 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.297095 [0] recv: INFOTS(1711462110.296972092) -1711462110.297118 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #81 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.297222 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #82: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.297269 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#82/1)): niov 0 sz 0 => now niov 3 sz 360 -1711462110.297294 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 82 maxseq 46) -1711462110.297313 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.0160145 s (min-ack 46, avail-seq 82, xmit 81) -1711462110.297330 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 360 => now niov 4 sz 392 -1711462110.297397 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 392 from udp/10.101.12.71:40473 -1711462110.297412 [0] recvMC: INFOTS(1711462110.297082585) -1711462110.297428 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #82 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.297412 [0] python3: nn_xpack_send 392: 0x556f7833352c:20 0x556f78a27198:36 0x556f789a7af0:304 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.297463 [0] python3: traffic-xmit (1) 392 -1711462110.297443 [0] recvMC: HEARTBEAT(F#91:82..82 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.297516 [0] python3: => EVERYONE -1711462110.297546 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:4d04) ... -1711462110.297555 [0] python3: => EVERYONE -1711462110.298110 [0] gc: gc 0x7590d8014d00: deleting -1711462110.298128 [0] gc: gc 0x556f78a2f640: deleting -1711462110.298137 [0] gc: gc_delete_reader(0x556f78a2f640, 110d7b4:17c5b8c5:94bd0ff4:4d04) -1711462110.298149 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #76: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4d04}} -1711462110.298159 [0] gc: non-timed queue now has 1 items -1711462110.298170 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4d04 @ 0x556f78a13404) user 17 builtin 12 -1711462110.298176 [0] gc: gc 0x7590d8014d00: deleting -1711462110.298190 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#76/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.298229 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003c24:28 [ udp/239.255.0.1:7400@2 ] -1711462110.298233 [0] tev: traffic-xmit (1) 96 -1711462110.298240 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.298246 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4c03) ... -1711462110.298279 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4c03) state transition 0 -> 1 -1711462110.298252 [0] recv: INFOTS(1711462110.298143983) -1711462110.298297 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4c03) ... -1711462110.298311 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4c03) - no unack'ed samples -1711462110.298322 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4c03) ... -1711462110.298338 [0] python3: => EVERYONE -1711462110.298367 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4c03) state transition 1 -> 3 -1711462110.298368 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #76 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.298414 [0] gc: gc 0x556f789ecab0: deleting -1711462110.298432 [0] gc: gc_delete_writer(0x556f789ecab0, 110d7b4:17c5b8c5:94bd0ff4:4c03) -1711462110.298490 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #82: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4c03}} -1711462110.298514 [0] gc: non-timed queue now has 1 items -1711462110.298533 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#82/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.298546 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4c03 @ 0x556f78a120f4) user 16 builtin 12 -1711462110.298563 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003cb4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.298575 [0] tev: traffic-xmit (1) 96 -1711462110.298576 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.298565 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.298610 [0] recv: INFOTS(1711462110.298471822) -1711462110.298628 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #82 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.298705 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #83: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.298736 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#83/1)): niov 0 sz 0 => now niov 3 sz 312 -1711462110.298749 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 83 maxseq 46) -1711462110.298763 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.0145064 s (min-ack 46, avail-seq 83, xmit 82) -1711462110.298774 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 312 => now niov 4 sz 344 -1711462110.298829 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462110.298836 [0] recvMC: INFOTS(1711462110.298593713) -1711462110.298846 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #83 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.298860 [0] recvMC: HEARTBEAT(F#92:83..83 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.298844 [0] python3: nn_xpack_send 344: 0x556f7833352c:20 0x556f78a27198:36 0x556f78b55d60:256 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.298895 [0] python3: traffic-xmit (1) 344 -1711462110.298907 [0] python3: => EVERYONE -1711462110.298924 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:4f04) ... -1711462110.298934 [0] python3: => EVERYONE -1711462110.299693 [0] gc: gc 0x7590d8014d00: deleting -1711462110.299710 [0] gc: gc 0x556f789bf480: deleting -1711462110.299718 [0] gc: gc_delete_reader(0x556f789bf480, 110d7b4:17c5b8c5:94bd0ff4:4f04) -1711462110.299744 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #77: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4f04}} -1711462110.299764 [0] gc: non-timed queue now has 1 items -1711462110.299785 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4f04 @ 0x556f78a19de4) user 15 builtin 12 -1711462110.299792 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#77/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.299795 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.299840 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003d44:28 [ udp/239.255.0.1:7400@2 ] -1711462110.299845 [0] tev: traffic-xmit (1) 96 -1711462110.299849 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4e03) ... -1711462110.299860 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4e03) state transition 0 -> 1 -1711462110.299871 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4e03) ... -1711462110.299889 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:4e03) - no unack'ed samples -1711462110.299904 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4e03) ... -1711462110.299915 [0] python3: => EVERYONE -1711462110.299880 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.299932 [0] recv: INFOTS(1711462110.299733268) -1711462110.299940 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #77 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.299952 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4e03) state transition 1 -> 3 -1711462110.300938 [0] gc: gc 0x7590d8014d00: deleting -1711462110.300973 [0] gc: gc 0x556f781e6be0: deleting -1711462110.300986 [0] gc: gc_delete_writer(0x556f781e6be0, 110d7b4:17c5b8c5:94bd0ff4:4e03) -1711462110.301017 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #83: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4e03}} -1711462110.301039 [0] gc: non-timed queue now has 1 items -1711462110.301060 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#83/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.301071 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4e03 @ 0x556f78a18e44) user 14 builtin 12 -1711462110.301087 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.301092 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003dd4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.301121 [0] tev: traffic-xmit (1) 96 -1711462110.301135 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.301176 [0] recv: INFOTS(1711462110.301005028) -1711462110.301191 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #83 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.301246 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #84: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.301271 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#84/1)): niov 0 sz 0 => now niov 3 sz 264 -1711462110.301283 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 84 maxseq 46) -1711462110.301297 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.0119437 s (min-ack 46, avail-seq 84, xmit 83) -1711462110.301308 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 264 => now niov 4 sz 296 -1711462110.301363 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 296 from udp/10.101.12.71:40473 -1711462110.301371 [0] python3: nn_xpack_send 296: 0x556f7833352c:20 0x556f78a27198:36 0x556f78304bc0:208 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.301407 [0] python3: traffic-xmit (1) 296 -1711462110.301373 [0] recvMC: INFOTS(1711462110.301158893) -1711462110.301420 [0] python3: => EVERYONE -1711462110.301440 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #84 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.301455 [0] recvMC: HEARTBEAT(F#93:84..84 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.301440 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:5304) ... -1711462110.301472 [0] python3: => EVERYONE -1711462110.302199 [0] gc: gc 0x7590d8014d00: deleting -1711462110.302216 [0] gc: gc 0x556f781e6be0: deleting -1711462110.302223 [0] gc: gc_delete_reader(0x556f781e6be0, 110d7b4:17c5b8c5:94bd0ff4:5304) -1711462110.302245 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #78: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5304}} -1711462110.302264 [0] gc: non-timed queue now has 1 items -1711462110.302283 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:5304 @ 0x556f78a26104) user 13 builtin 12 -1711462110.302292 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#78/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.302295 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.302336 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003e64:28 [ udp/239.255.0.1:7400@2 ] -1711462110.302341 [0] tev: traffic-xmit (1) 96 -1711462110.302352 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5203) ... -1711462110.302371 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.302391 [0] recv: INFOTS(1711462110.302235006) -1711462110.302373 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5203) state transition 0 -> 1 -1711462110.302430 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #78 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.302495 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5203) ... -1711462110.302588 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5203) - no unack'ed samples -1711462110.302600 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:5203) ... -1711462110.302613 [0] python3: => EVERYONE -1711462110.302632 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5203) state transition 1 -> 3 -1711462110.303418 [0] gc: gc 0x7590d8014d00: deleting -1711462110.303439 [0] gc: gc 0x556f789bf480: deleting -1711462110.303448 [0] gc: gc_delete_writer(0x556f789bf480, 110d7b4:17c5b8c5:94bd0ff4:5203) -1711462110.303479 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #84: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5203}} -1711462110.303499 [0] gc: non-timed queue now has 1 items -1711462110.303520 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#84/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.303528 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:5203 @ 0x556f78a24f84) user 12 builtin 12 -1711462110.303543 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.303554 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003ef4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.303560 [0] tev: traffic-xmit (1) 96 -1711462110.303598 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.303614 [0] recv: INFOTS(1711462110.303466791) -1711462110.303631 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #84 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.303686 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #85: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.303724 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#85/1)): niov 0 sz 0 => now niov 3 sz 216 -1711462110.303738 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 85 maxseq 46) -1711462110.303754 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.00949309 s (min-ack 46, avail-seq 85, xmit 84) -1711462110.303771 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 216 => now niov 4 sz 248 -1711462110.303839 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 248 from udp/10.101.12.71:40473 -1711462110.303848 [0] recvMC: INFOTS(1711462110.303608296) -1711462110.303867 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #85 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.303884 [0] recvMC: HEARTBEAT(F#94:85..85 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.303851 [0] python3: nn_xpack_send 248: 0x556f7833352c:20 0x556f78a27198:36 0x556f7889eee0:160 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.303913 [0] python3: traffic-xmit (1) 248 -1711462110.303927 [0] python3: => EVERYONE -1711462110.303947 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:5504) ... -1711462110.303960 [0] python3: => EVERYONE -1711462110.304616 [0] gc: gc 0x7590d8014d00: deleting -1711462110.304641 [0] gc: gc 0x556f789ecab0: deleting -1711462110.304651 [0] gc: gc_delete_reader(0x556f789ecab0, 110d7b4:17c5b8c5:94bd0ff4:5504) -1711462110.304677 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #79: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5504}} -1711462110.304697 [0] gc: non-timed queue now has 1 items -1711462110.304719 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#79/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.304723 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:5504 @ 0x556f78a2c224) user 11 builtin 12 -1711462110.304761 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0003f84:28 [ udp/239.255.0.1:7400@2 ] -1711462110.304761 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.304767 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.304788 [0] recv: INFOTS(1711462110.304665223) -1711462110.304813 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #79 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.304761 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5403) ... -1711462110.304846 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5403) state transition 0 -> 1 -1711462110.304861 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5403) ... -1711462110.304876 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5403) - no unack'ed samples -1711462110.304897 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:5403) ... -1711462110.304926 [0] python3: => EVERYONE -1711462110.304819 [0] tev: traffic-xmit (1) 96 -1711462110.304970 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5403) state transition 1 -> 3 -1711462110.305858 [0] gc: gc 0x7590d8014d00: deleting -1711462110.305882 [0] gc: gc 0x556f78a2f640: deleting -1711462110.305892 [0] gc: gc_delete_writer(0x556f78a2f640, 110d7b4:17c5b8c5:94bd0ff4:5403) -1711462110.305958 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #85: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5403}} -1711462110.305981 [0] gc: non-timed queue now has 1 items -1711462110.306008 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#85/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.306017 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:5403 @ 0x556f78a2b164) user 10 builtin 12 -1711462110.306036 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.306054 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.306066 [0] recv: INFOTS(1711462110.305944071) -1711462110.306055 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0004014:28 [ udp/239.255.0.1:7400@2 ] -1711462110.306092 [0] tev: traffic-xmit (1) 96 -1711462110.306077 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #85 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.307111 [0] gc: gc 0x7590d8014d00: deleting -1711462110.307238 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #86: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.307273 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#86/1)): niov 0 sz 0 => now niov 3 sz 192 -1711462110.307284 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 86 maxseq 46) -1711462110.307302 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.00593483 s (min-ack 46, avail-seq 86, xmit 85) -1711462110.307313 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 192 => now niov 4 sz 224 -1711462110.307413 [0] python3: nn_xpack_send 224: 0x556f7833352c:20 0x556f78a27198:36 0x556f78b5fb20:136 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.307419 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 224 from udp/10.101.12.71:40473 -1711462110.307446 [0] recvMC: INFOTS(1711462110.307152640) -1711462110.307426 [0] python3: traffic-xmit (1) 224 -1711462110.307486 [0] python3: => EVERYONE -1711462110.307500 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5603) ... -1711462110.307510 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #86 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.307529 [0] recvMC: HEARTBEAT(F#95:86..86 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.307511 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5603) state transition 0 -> 1 -1711462110.307554 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5603) ... -1711462110.307562 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5603) - no unack'ed samples -1711462110.307569 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:5603) ... -1711462110.307582 [0] python3: => EVERYONE -1711462110.307609 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5603) state transition 1 -> 3 -1711462110.307665 [0] gc: gc 0x556f78b57230: deleting -1711462110.307677 [0] gc: gc_delete_writer(0x556f78b57230, 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462110.307721 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #86: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5603}} -1711462110.307738 [0] gc: non-timed queue now has 1 items -1711462110.307767 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#86/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.307888 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.307905 [0] recv: INFOTS(1711462110.307705837) -1711462110.307904 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f00040a4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.307936 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #86 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.307940 [0] tev: traffic-xmit (1) 96 -1711462110.308002 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:5603 @ 0x556f78a890a4) user 9 builtin 12 -1711462110.308025 [0] gc: gc 0x7590d8014d00: deleting -1711462110.308189 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #87: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.308223 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#87/1)): niov 0 sz 0 => now niov 3 sz 168 -1711462110.308234 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 87 maxseq 46) -1711462110.308252 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.00497945 s (min-ack 46, avail-seq 87, xmit 86) -1711462110.308262 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 168 => now niov 4 sz 200 -1711462110.308344 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 200 from udp/10.101.12.71:40473 -1711462110.308371 [0] recvMC: INFOTS(1711462110.308113998) -1711462110.308360 [0] python3: nn_xpack_send 200: 0x556f7833352c:20 0x556f78a27198:36 0x556f78b5a570:112 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.308402 [0] python3: traffic-xmit (1) 200 -1711462110.308412 [0] python3: => EVERYONE -1711462110.308425 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5703) ... -1711462110.308436 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5703) state transition 0 -> 1 -1711462110.308472 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5703) ... -1711462110.308393 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #87 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.308518 [0] recvMC: HEARTBEAT(F#96:87..87 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.308480 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5703) - no unack'ed samples -1711462110.308557 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:5703) ... -1711462110.308565 [0] python3: => EVERYONE -1711462110.308585 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5703) state transition 1 -> 3 -1711462110.308674 [0] gc: gc 0x556f789d3cf0: deleting -1711462110.308683 [0] gc: gc_delete_writer(0x556f789d3cf0, 110d7b4:17c5b8c5:94bd0ff4:5703) -1711462110.308707 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #87: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5703}} -1711462110.308721 [0] gc: non-timed queue now has 1 items -1711462110.308739 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:5703 @ 0x556f78b3c1d4) user 8 builtin 12 -1711462110.308752 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.308740 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#87/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.308843 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.308862 [0] recv: INFOTS(1711462110.308696854) -1711462110.308882 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #88: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.308846 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x556f78a8ab84:28 [ udp/239.255.0.1:7400@2 ] -1711462110.308955 [0] tev: traffic-xmit (1) 96 -1711462110.308888 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #87 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.308930 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#88/1)): niov 0 sz 0 => now niov 3 sz 144 -1711462110.309002 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 88 maxseq 46) -1711462110.309026 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.00429296 s (min-ack 46, avail-seq 88, xmit 87) -1711462110.309040 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 144 => now niov 4 sz 176 -1711462110.309095 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 176 from udp/10.101.12.71:40473 -1711462110.309105 [0] recvMC: INFOTS(1711462110.308810509) -1711462110.309107 [0] python3: nn_xpack_send 176: 0x556f7833352c:20 0x556f78a27198:36 0x556f7889fd10:88 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.309120 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #88 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.309139 [0] python3: traffic-xmit (1) 176 -1711462110.309160 [0] python3: => EVERYONE -1711462110.309184 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5803) ... -1711462110.309206 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5803) state transition 0 -> 1 -1711462110.309218 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5803) ... -1711462110.309228 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:5803) - no unack'ed samples -1711462110.309256 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:5803) ... -1711462110.309273 [0] python3: => EVERYONE -1711462110.309234 [0] recvMC: HEARTBEAT(F#97:88..88 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.309308 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:5803) state transition 1 -> 3 -1711462110.309825 [0] gc: gc 0x7590d8014d00: deleting -1711462110.309839 [0] gc: gc 0x556f78b57230: deleting -1711462110.309854 [0] gc: gc_delete_writer(0x556f78b57230, 110d7b4:17c5b8c5:94bd0ff4:5803) -1711462110.309886 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #88: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5803}} -1711462110.309899 [0] gc: non-timed queue now has 1 items -1711462110.309919 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#88/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.309952 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:5803 @ 0x556f78b44434) user 7 builtin 12 -1711462110.309961 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.310002 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590f0004134:28 [ udp/239.255.0.1:7400@2 ] -1711462110.310021 [0] tev: traffic-xmit (1) 96 -1711462110.310020 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.310076 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #89: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{}}}} -1711462110.310091 [0] recv: INFOTS(1711462110.309878806) -1711462110.310115 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#89/1)): niov 0 sz 0 => now niov 3 sz 120 -1711462110.310143 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 89 maxseq 46) -1711462110.310122 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #88 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.310184 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.00307106 s (min-ack 46, avail-seq 89, xmit 88) -1711462110.310218 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 120 => now niov 4 sz 152 -1711462110.310265 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 152 from udp/10.101.12.71:40473 -1711462110.310273 [0] recvMC: INFOTS(1711462110.310031935) -1711462110.310291 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #89 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.310305 [0] recvMC: HEARTBEAT(F#98:89..89 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.310274 [0] python3: nn_xpack_send 152: 0x556f7833352c:20 0x556f78a27198:36 0x556f78b576e0:64 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.310336 [0] python3: traffic-xmit (1) 152 -1711462110.310346 [0] python3: => EVERYONE -1711462110.310356 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:603) ... -1711462110.310368 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:603) state transition 0 -> 1 -1711462110.310374 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:603) ... -1711462110.310381 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:603) - no unack'ed samples -1711462110.310391 [0] python3: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:603) ... -1711462110.310400 [0] python3: => EVERYONE -1711462110.310413 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:603) state transition 1 -> 3 -1711462110.311040 [0] gc: gc 0x7590d8014d00: deleting -1711462110.311055 [0] gc: gc 0x556f78a2f640: deleting -1711462110.311061 [0] gc: gc_delete_writer(0x556f78a2f640, 110d7b4:17c5b8c5:94bd0ff4:603) -1711462110.311078 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #89: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:603}} -1711462110.311087 [0] gc: non-timed queue now has 1 items -1711462110.311114 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:603 @ 0x556f789a2324) user 6 builtin 12 -1711462110.311115 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#89/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.311123 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.311184 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.311188 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x556f78b79b14:28 [ udp/239.255.0.1:7400@2 ] -1711462110.311190 [0] recv: INFOTS(1711462110.311071865) -1711462110.311206 [0] tev: traffic-xmit (1) 96 -1711462110.311235 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #89 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.311213 [0] python3: write_sample 110d7b4:17c5b8c5:94bd0ff4:403 #90: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{}} -1711462110.311258 [0] python3: xpack_addmsg 0x556f78333520 0x556f78a2c880 0(data(110d7b4:17c5b8c5:94bd0ff4:403:#90/1)): niov 0 sz 0 => now niov 3 sz 88 -1711462110.311264 [0] python3: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 6 seq-eq-max 6 seq 90 maxseq 46) -1711462110.311271 [0] python3: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403 final) piggybacked, resched in 0.00192613 s (min-ack 46, avail-seq 90, xmit 89) -1711462110.311277 [0] python3: xpack_addmsg 0x556f78333520 0x556f78b3cc00 0(control): niov 3 sz 88 => now niov 4 sz 120 -1711462110.311317 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 120 from udp/10.101.12.71:40473 -1711462110.311336 [0] recvMC: INFOTS(1711462110.311185386) -1711462110.311317 [0] python3: nn_xpack_send 120: 0x556f7833352c:20 0x556f78a27198:36 0x556f78b58440:32 0x556f78b3ccf8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.311357 [0] python3: traffic-xmit (1) 120 -1711462110.311369 [0] python3: => EVERYONE -1711462110.311357 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #90 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.311480 [0] recvMC: HEARTBEAT(F#99:90..90 110d7b4:17c5b8c5:94bd0ff4:403? -> 0:0:0:0) -1711462110.311604 [0] python3: ddsi_unblock_throttled_writer(guid 110d7b4:17c5b8c5:94bd0ff4:403) ... -1711462110.311611 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:403) state transition 0 -> 1 -1711462110.311617 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:403) ... -1711462110.311622 [0] python3: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:403) state transition 1 -> 2 -1711462110.311631 [0] python3: delete_writer(guid 110d7b4:17c5b8c5:94bd0ff4:403) - unack'ed samples, will delete when ack'd or at t = 16948.971130 -1711462110.312219 [0] gc: gc 0x7590d8014d00: deleting -1711462110.313201 [0] tev: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403) suppressed, resched in 0.1 s (min-ack 46, avail-seq 90, xmit 90) -1711462110.314477 [0] tev: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:4c2 multicasting (rel-prd 6 seq-eq-max 6 seq 79 maxseq 40) -1711462110.314500 [0] tev: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:4c2) sent, resched in 0.1 s (min-ack 40, avail-seq 79, xmit 79) -1711462110.314516 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.314589 [0] tev: nn_xpack_send 52: 0x7590e000139c:20 0x7590e400d3a8:32 [ udp/239.255.0.1:7400@2 ] -1711462110.314601 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.314624 [0] recv: HEARTBEAT(#9:1..79 110d7b4:17c5b8c5:94bd0ff4:4c2? -> 0:0:0:0) -1711462110.314603 [0] tev: traffic-xmit (1) 52 -1711462110.314784 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462110.314800 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.314827 [0] recv: ACKNACK(F#4:80/0: L(:1c1 16957.974302) 110aba1:7b19badd:ce0adb73:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK39 RM0) -1711462110.314842 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.314850 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.314864 [0] recv: ACKNACK(F#4:80/0: L(:1c1 16957.974353) 110e21c:d0762c48:a9f0fb28:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK39 RM0) -1711462110.314878 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.314883 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.314894 [0] recv: ACKNACK(F#4:80/0: L(:1c1 16957.974386) 110443d:bb7f10a5:18901533:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK39 RM0) -1711462110.314906 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.314913 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.314925 [0] recv: ACKNACK(F#4:80/0: L(:1c1 16957.974415) 110cd0d:56a27910:57318171:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK39 RM0) -1711462110.314931 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.314942 [0] recv: ACKNACK(F#12:80/0: L(:1c1 16957.974415) 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK39 RM0) -1711462110.314948 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.314980 [0] recv: ACKNACK(F#7:80/0: L(:1c1 16957.974415) 110cd0d:79d6eeac:ea4a8907:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK39 RM39) -1711462110.315855 [0] tev: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:3c2 multicasting (rel-prd 6 seq-eq-max 6 seq 89 maxseq 45) -1711462110.315893 [0] tev: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:3c2) sent, resched in 0.1 s (min-ack 45, avail-seq 89, xmit 89) -1711462110.315909 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.315974 [0] tev: nn_xpack_send 52: 0x7590e000139c:20 0x7590e400d3a8:32 [ udp/239.255.0.1:7400@2 ] -1711462110.315983 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.315986 [0] tev: traffic-xmit (1) 52 -1711462110.316023 [0] recv: HEARTBEAT(#10:1..89 110d7b4:17c5b8c5:94bd0ff4:3c2? -> 0:0:0:0) -1711462110.316171 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.316184 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.316216 [0] recv: ACKNACK(F#5:90/0: L(:1c1 16957.975685) 110443d:bb7f10a5:18901533:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK44 RM0) -1711462110.316233 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462110.316239 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.316251 [0] recv: ACKNACK(F#5:90/0: L(:1c1 16957.975741) 110aba1:7b19badd:ce0adb73:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK44 RM0) -1711462110.316264 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.316269 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.316280 [0] recv: ACKNACK(F#5:90/0: L(:1c1 16957.975772) 110e21c:d0762c48:a9f0fb28:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK44 RM0) -1711462110.316292 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.316298 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.316310 [0] recv: ACKNACK(F#5:90/0: L(:1c1 16957.975801) 110cd0d:56a27910:57318171:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK44 RM0) -1711462110.316316 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.316328 [0] recv: ACKNACK(F#15:90/0: L(:1c1 16957.975801) 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK44 RM0) -1711462110.316334 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.316411 [0] recv: ACKNACK(F#9:90/0: L(:1c1 16957.975801) 110cd0d:79d6eeac:ea4a8907:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK44 RM44) -1711462110.376800 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.376901 [0] recvMC: HEARTBEAT(#29:20..20 L(:1c1 16958.036362)110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:504@20(sync)) -1711462110.377090 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:504 -> 110443d:bb7f10a5:18901533:403: F#6:21/0: -1711462110.377127 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.377173 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.377134 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.377252 [0] recvMC: HEARTBEAT(#35:24..24 L(:1c1 16958.036730)110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:504@24(sync)) -1711462110.377269 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7417@2 ] -1711462110.377286 [0] tev: traffic-xmit (1) 64 -1711462110.377324 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:504 -> 110aba1:7b19badd:ce0adb73:403: F#5:25/0: -1711462110.377337 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462110.377372 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.377408 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7419@2 ] -1711462110.377502 [0] tev: traffic-xmit (1) 64 -1711462110.378905 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.379008 [0] recv: INFOTS(1711462110.378453601) -1711462110.379054 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #20 L(:1c1 16958.038508)) -1711462110.379095 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.379108 [0] recv: INFOTS(1711462110.378380676) -1711462110.379128 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #24 L(:1c1 16958.038608)) -1711462110.379178 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #20: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:403}} -1711462110.379228 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:403 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:403) - deleting -1711462110.379265 [0] dq.builtin: => EVERYONE -1711462110.379320 [0] dq.builtin: delete -1711462110.379352 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #24: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:403}} -1711462110.379348 [0] gc: gc 0x7590d8046810: not yet, shortsleep -1711462110.379381 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:403 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:403) - deleting -1711462110.379441 [0] dq.builtin: => EVERYONE -1711462110.379460 [0] dq.builtin: delete -1711462110.379700 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.379773 [0] recv: HEARTBEAT(#9:20..20 L(:1c1 16958.039237)110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:3c7@20(sync)) -1711462110.379807 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.379820 [0] recv: INFOTS(1711462110.379581375) -1711462110.379846 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #14 L(:1c1 16958.039320)) -1711462110.379868 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#5:21/0: -1711462110.379897 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.379924 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.380025 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:504}} -1711462110.380043 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7416@2 ] -1711462110.380060 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:504 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:504) => EVERYONE -1711462110.380031 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.380060 [0] tev: traffic-xmit (1) 64 -1711462110.380114 [0] dq.builtin: - deleting -1711462110.380150 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#5:25/0: -1711462110.380114 [0] recv: HEARTBEAT(#8:24..24 L(:1c1 16958.039597)110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:3c7@24(sync)) -1711462110.380169 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462110.380217 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.380199 [0] dq.builtin: delete -1711462110.380277 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7418@2 ] -1711462110.380285 [0] tev: traffic-xmit (1) 64 -1711462110.380475 [0] gc: gc 0x7590d8046810: deleting -1711462110.380490 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8046810, 110443d:bb7f10a5:18901533:403) -1711462110.380533 [0] gc: gc 0x7590d8021440: deleting -1711462110.380545 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8021440, 110aba1:7b19badd:ce0adb73:403) -1711462110.380556 [0] gc: gc 0x7590d8049e20: deleting -1711462110.380585 [0] gc: gc_delete_proxy_reader (0x7590d8049e20, 110443d:bb7f10a5:18901533:504) -1711462110.380556 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7590d8046810, 110443d:bb7f10a5:18901533:403) -1711462110.380624 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7419@2 -1711462110.380630 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7590d8021440, 110aba1:7b19badd:ce0adb73:403) -1711462110.380649 [0] gc: reduced nlocs=4 -1711462110.380680 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.380696 [0] gc: rdidx 0 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.380715 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462110.380734 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.380742 [0] gc: rdidx 1 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.380753 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462110.380764 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.380771 [0] gc: rdidx 2 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.380783 [0] gc: rdidx 2 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462110.380797 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.380812 [0] gc: rdidx 3 lidx udp/10.101.12.71:7413@2 1 -> 0 -1711462110.380831 [0] gc: rdidx 3 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462110.380848 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.380895 [0] gc: rdidx 4 lidx udp/10.101.12.71:7419@2 2 -> 0 -1711462110.380846 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.380903 [0] gc: rdidx 4 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462110.380972 [0] recv: INFOTS(1711462110.380434543) -1711462110.381036 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #20 L(:1c1 16958.040471)) -1711462110.381010 [0] gc: a b c d e -1711462110.381063 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #20: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:504}} -1711462110.381084 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:504 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:504) => EVERYONE -1711462110.381082 [0] gc: loc 0 = udp/10.101.12.71:7411@2 -1 { +u +u +u .. .. } -1711462110.381097 [0] dq.builtin: - deleting -1711462110.381118 [0] gc: loc 1 = udp/10.101.12.71:7413@2 1 { .. .. .. +u .. } -1711462110.381203 [0] gc: loc 2 = udp/10.101.12.71:7419@2 1 { .. .. .. .. +u } -1711462110.381224 [0] gc: loc 3 = udp/239.255.0.1:7401@2 -2 { +1 +1 +1 +1 +1 } -1711462110.381235 [0] gc: best = 3 -1711462110.381251 [0] gc: simple udp/239.255.0.1:7401@2 -1711462110.381287 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462110.381307 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=13 -1711462110.381317 [0] dq.builtin: delete -1711462110.381324 [0] gc: gc 0x7590d8046810: deleting -1711462110.381337 [0] gc: gc_delete_proxy_writer (0x7590d8046810, 110443d:bb7f10a5:18901533:403) -1711462110.381363 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:504, 7): 1 -> 7 -1711462110.381376 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=12 -1711462110.381386 [0] gc: gc 0x7590d8021440: deleting -1711462110.381396 [0] gc: gc_delete_proxy_writer (0x7590d8021440, 110aba1:7b19badd:ce0adb73:403) -1711462110.381417 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:504, 6): 7 -> 6 -1711462110.381430 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=13 -1711462110.381453 [0] gc: gc 0x7590d8055680: deleting -1711462110.381461 [0] gc: gc_delete_proxy_reader (0x7590d8055680, 110aba1:7b19badd:ce0adb73:504) -1711462110.381481 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 udp/10.101.12.71:7413@2 -1711462110.381496 [0] gc: reduced nlocs=3 -1711462110.381505 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381521 [0] gc: rdidx 0 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.381531 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.381543 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381550 [0] gc: rdidx 1 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.381568 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.381578 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381591 [0] gc: rdidx 2 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.381603 [0] gc: rdidx 2 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.381618 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381630 [0] gc: rdidx 3 lidx udp/10.101.12.71:7413@2 1 -> 0 -1711462110.381644 [0] gc: rdidx 3 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.381659 [0] gc: a b c d -1711462110.381679 [0] gc: loc 0 = udp/10.101.12.71:7411@2 -1 { +u +u +u .. } -1711462110.381694 [0] gc: loc 1 = udp/10.101.12.71:7413@2 1 { .. .. .. +u } -1711462110.381712 [0] gc: loc 2 = udp/239.255.0.1:7401@2 -1 { +1 +1 +1 +1 } -1711462110.381723 [0] gc: best = 2 -1711462110.381752 [0] gc: simple udp/239.255.0.1:7401@2 -1711462110.381768 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462110.381778 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=12 -1711462110.385415 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.385421 [0] recv: INFOTS(1711462110.385271352) -1711462110.385433 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #2) -1711462110.385456 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110aba1:7b19badd:ce0adb73:1c1}} -1711462110.385472 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.385653 [0] recv: INFOTS(1711462110.385271352) -1711462110.385645 [0] dq.builtin: SPDP ST3 110aba1:7b19badd:ce0adb73:1c1ddsi_delete_proxy_participant_by_guid(110aba1:7b19badd:ce0adb73:1c1) - deleting -1711462110.385662 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #2) -1711462110.385679 [0] dq.builtin: => EVERYONE -1711462110.385695 [0] dq.builtin: delete_ppt(110aba1:7b19badd:ce0adb73:1c1) - deleting dependent proxy participants -1711462110.385710 [0] dq.builtin: delete_ppt(110aba1:7b19badd:ce0adb73:1c1) - deleting endpoints -1711462110.385738 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:301c4) - deleting -1711462110.385754 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:301c3) - deleting -1711462110.385766 [0] gc: gc 0x7590d8050e30: not yet, shortsleep -1711462110.385768 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:300c4) - deleting -1711462110.385788 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:300c3) - deleting -1711462110.385802 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:200c7) - deleting -1711462110.385816 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:200c2) - deleting -1711462110.385830 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:4c7) - deleting -1711462110.385846 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:4c2) - deleting -1711462110.385857 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:3c7) - deleting -1711462110.385867 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:3c2) - deleting -1711462110.385877 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:100c7) - deleting -1711462110.385886 [0] dq.builtin: delete -1711462110.385903 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110aba1:7b19badd:ce0adb73:1c1}} -1711462110.385913 [0] dq.builtin: SPDP ST3 110aba1:7b19badd:ce0adb73:1c1 unknown not allowed -1711462110.386866 [0] gc: gc 0x7590d8050e30: deleting -1711462110.386889 [0] gc: gc_delete_proxy_reader (0x7590d8050e30, 110aba1:7b19badd:ce0adb73:301c4) -1711462110.386916 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462110.386924 [0] gc: reduced nlocs=4 -1711462110.386934 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.386952 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.386960 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.386968 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.386974 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.386982 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.386990 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387001 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387012 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387024 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387034 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.387045 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387056 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387061 [0] gc: rdidx 4 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.387071 [0] gc: rdidx 4 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387084 [0] gc: a b c d e -1711462110.387097 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. } -1711462110.387108 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. } -1711462110.387114 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u } -1711462110.387123 [0] gc: loc 3 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462110.387129 [0] gc: best = 3 -1711462110.387135 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.387149 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.387156 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=11 -1711462110.387168 [0] gc: gc 0x7590d8046810: deleting -1711462110.387176 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8046810, 110aba1:7b19badd:ce0adb73:301c3) -1711462110.387201 [0] gc: gc 0x7590d8021440: deleting -1711462110.387208 [0] gc: gc_delete_proxy_reader (0x7590d8021440, 110aba1:7b19badd:ce0adb73:300c4) -1711462110.387221 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d8046810, 110aba1:7b19badd:ce0adb73:301c3) -1711462110.387227 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462110.387256 [0] gc: reduced nlocs=4 -1711462110.387267 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387275 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387294 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387303 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387313 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387324 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387333 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387340 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387347 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387352 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387361 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.387368 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387379 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387384 [0] gc: rdidx 4 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.387398 [0] gc: rdidx 4 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387410 [0] gc: a b c d e -1711462110.387426 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. } -1711462110.387437 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. } -1711462110.387446 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u } -1711462110.387459 [0] gc: loc 3 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462110.387467 [0] gc: best = 3 -1711462110.387476 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.387488 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.387501 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=10 -1711462110.387511 [0] gc: gc 0x7590d80494f0: deleting -1711462110.387515 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d80494f0, 110aba1:7b19badd:ce0adb73:300c3) -1711462110.387528 [0] gc: gc 0x7590d80544c0: deleting -1711462110.387537 [0] gc: gc_delete_proxy_reader (0x7590d80544c0, 110aba1:7b19badd:ce0adb73:200c7) -1711462110.387536 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d80494f0, 110aba1:7b19badd:ce0adb73:300c3) -1711462110.387556 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462110.387578 [0] gc: reduced nlocs=4 -1711462110.387586 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387595 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387603 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387611 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387620 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387630 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387634 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387643 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387651 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387662 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387666 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.387679 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387685 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387693 [0] gc: rdidx 4 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.387704 [0] gc: rdidx 4 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387715 [0] gc: a b c d e -1711462110.387728 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. } -1711462110.387747 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. } -1711462110.387758 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u } -1711462110.387769 [0] gc: loc 3 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462110.387777 [0] gc: best = 3 -1711462110.387785 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.387797 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.387807 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=9 -1711462110.387813 [0] gc: gc 0x7590d8052960: deleting -1711462110.387818 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8052960, 110aba1:7b19badd:ce0adb73:200c2) -1711462110.387827 [0] gc: gc 0x7590d8048bd0: deleting -1711462110.387834 [0] gc: gc_delete_proxy_reader (0x7590d8048bd0, 110aba1:7b19badd:ce0adb73:4c7) -1711462110.387835 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d8052960, 110aba1:7b19badd:ce0adb73:200c2) -1711462110.387854 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462110.387870 [0] gc: reduced nlocs=4 -1711462110.387877 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387884 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387890 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387895 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387900 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387912 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387918 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387927 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.387935 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387945 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387954 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.387963 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387973 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387980 [0] gc: rdidx 4 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.387990 [0] gc: rdidx 4 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387999 [0] gc: a b c d e -1711462110.388010 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. } -1711462110.388021 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. } -1711462110.388030 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u } -1711462110.388042 [0] gc: loc 3 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462110.388051 [0] gc: best = 3 -1711462110.388059 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.388070 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.388079 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=8 -1711462110.388086 [0] gc: gc 0x7590d804a6e0: deleting -1711462110.388121 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d804a6e0, 110aba1:7b19badd:ce0adb73:4c2) -1711462110.388129 [0] gc: gc 0x7590d8054d80: deleting -1711462110.388134 [0] gc: gc_delete_proxy_reader (0x7590d8054d80, 110aba1:7b19badd:ce0adb73:3c7) -1711462110.388137 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d804a6e0, 110aba1:7b19badd:ce0adb73:4c2) -1711462110.388147 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7416@2 -1711462110.388155 [0] gc: reduced nlocs=4 -1711462110.388162 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388168 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.388174 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388179 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388184 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.388189 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388193 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388201 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.388209 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388213 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388222 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.388228 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388232 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388237 [0] gc: rdidx 4 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.388242 [0] gc: rdidx 4 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388248 [0] gc: a b c d e -1711462110.388256 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. .. } -1711462110.388264 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u .. } -1711462110.388270 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. .. .. .. +u } -1711462110.388277 [0] gc: loc 3 = udp/239.255.0.1:7400@2 -2 { +1 +1 +1 +1 +1 } -1711462110.388281 [0] gc: best = 3 -1711462110.388285 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.388293 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.388298 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=7 -1711462110.388305 [0] gc: gc 0x7590d8049e20: deleting -1711462110.388309 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8049e20, 110aba1:7b19badd:ce0adb73:3c2) -1711462110.388319 [0] gc: gc 0x7590d8055680: deleting -1711462110.388324 [0] gc: gc_delete_proxy_reader (0x7590d8055680, 110aba1:7b19badd:ce0adb73:100c7) -1711462110.388328 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=6 -1711462110.388336 [0] gc: gc 0x7590d803a600: deleting -1711462110.388343 [0] gc: gc_delete_proxy_participant(0x7590d803a600, 110aba1:7b19badd:ce0adb73:1c1) -1711462110.388389 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d8049e20, 110aba1:7b19badd:ce0adb73:3c2) -1711462110.388413 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=5 -1711462110.388439 [0] gc: gc 0x7590d8046810: deleting -1711462110.388451 [0] gc: gc_delete_proxy_writer (0x7590d8046810, 110aba1:7b19badd:ce0adb73:301c3) -1711462110.388471 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:301c4, 3): 1 -> 3 -1711462110.388483 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=4 -1711462110.388522 [0] gc: gc 0x7590d80494f0: deleting -1711462110.388529 [0] gc: gc_delete_proxy_writer (0x7590d80494f0, 110aba1:7b19badd:ce0adb73:300c3) -1711462110.388543 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:300c4, 3): 1 -> 3 -1711462110.388555 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=3 -1711462110.388565 [0] gc: gc 0x7590d8052960: deleting -1711462110.388575 [0] gc: gc_delete_proxy_writer (0x7590d8052960, 110aba1:7b19badd:ce0adb73:200c2) -1711462110.388600 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:200c7, 6): 1 -> 6 -1711462110.388615 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=2 -1711462110.388628 [0] gc: gc 0x7590d804a6e0: deleting -1711462110.388639 [0] gc: gc_delete_proxy_writer (0x7590d804a6e0, 110aba1:7b19badd:ce0adb73:4c2) -1711462110.388653 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:4c7, 5): 1 -> 5 -1711462110.388662 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=1 -1711462110.388673 [0] gc: gc 0x7590d8049e20: deleting -1711462110.388680 [0] gc: gc_delete_proxy_writer (0x7590d8049e20, 110aba1:7b19badd:ce0adb73:3c2) -1711462110.388692 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:3c7, 6): 1 -> 6 -1711462110.388704 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=0, freeing -1711462110.388717 [0] gc: lease_unregister(l 0x7590d804c550 guid 110aba1:7b19badd:ce0adb73:1c1) -1711462110.388731 [0] gc: lease_free(l 0x7590d804c550 guid 110aba1:7b19badd:ce0adb73:1c1) -1711462110.388741 [0] gc: lease_free(l 0x7590d804c4d0 guid 110aba1:7b19badd:ce0adb73:1c1) -1711462110.388753 [0] gc: ddsi_remove_deleted_participant_guid(110aba1:7b19badd:ce0adb73:1c1 for_what=3) -1711462110.388765 [0] gc: gc 0x7590d8014d00: deleting -1711462110.391681 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.391709 [0] recvMC: HEARTBEAT(#30:22..22 L(:1c1 16958.051198)110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:504@22(sync)) -1711462110.391744 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:504 -> 110e21c:d0762c48:a9f0fb28:403: F#5:23/0: -1711462110.391764 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462110.391771 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.391800 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7413@2 ] -1711462110.391804 [0] tev: traffic-xmit (1) 64 -1711462110.392134 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.392139 [0] recv: INFOTS(1711462110.392032273) -1711462110.392147 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #20 L(:1c1 16958.051642)) -1711462110.392174 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #20: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:403}} -1711462110.392188 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:403 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:403) - deleting -1711462110.392206 [0] dq.builtin: => EVERYONE -1711462110.392225 [0] dq.builtin: delete -1711462110.392238 [0] gc: gc 0x7590d8050e30: deleting -1711462110.392250 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8050e30, 110e21c:d0762c48:a9f0fb28:403) -1711462110.392269 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7590d8050e30, 110e21c:d0762c48:a9f0fb28:403) -1711462110.392285 [0] gc: gc 0x7590d8050e30: deleting -1711462110.392295 [0] gc: gc_delete_proxy_writer (0x7590d8050e30, 110e21c:d0762c48:a9f0fb28:403) -1711462110.392312 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:504, 6): 7 -> 6 -1711462110.392324 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=13 -1711462110.392414 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.392419 [0] recv: INFOTS(1711462110.384844509) -1711462110.392426 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #2) -1711462110.392438 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110443d:bb7f10a5:18901533:1c1}} -1711462110.392450 [0] dq.builtin: SPDP ST3 110443d:bb7f10a5:18901533:1c1ddsi_delete_proxy_participant_by_guid(110443d:bb7f10a5:18901533:1c1) - deleting -1711462110.392457 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.392470 [0] dq.builtin: => EVERYONE -1711462110.392483 [0] recv: INFOTS(1711462110.384844509) -1711462110.392491 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #2) -1711462110.392483 [0] dq.builtin: delete_ppt(110443d:bb7f10a5:18901533:1c1) - deleting dependent proxy participants -1711462110.392525 [0] dq.builtin: delete_ppt(110443d:bb7f10a5:18901533:1c1) - deleting endpoints -1711462110.392537 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:301c4) - deleting -1711462110.392578 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:301c3) - deleting -1711462110.392580 [0] gc: gc 0x7590d801b140: not yet, shortsleep -1711462110.392605 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:300c4) - deleting -1711462110.392617 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:300c3) - deleting -1711462110.392628 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:200c7) - deleting -1711462110.392639 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:200c2) - deleting -1711462110.392652 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:4c7) - deleting -1711462110.392663 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:4c2) - deleting -1711462110.392674 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:3c7) - deleting -1711462110.392686 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:3c2) - deleting -1711462110.392697 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:100c7) - deleting -1711462110.392707 [0] dq.builtin: delete -1711462110.392724 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110443d:bb7f10a5:18901533:1c1}} -1711462110.392734 [0] dq.builtin: SPDP ST3 110443d:bb7f10a5:18901533:1c1 unknown not allowed -1711462110.393358 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.393365 [0] recv: INFOTS(1711462110.393273698) -1711462110.393401 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #16 L(:1c1 16958.052868)) -1711462110.393461 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #16: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:504}} -1711462110.393475 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:504 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:504) => EVERYONE -1711462110.393487 [0] dq.builtin: - deleting -1711462110.393497 [0] dq.builtin: delete -1711462110.393659 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.393662 [0] gc: gc 0x7590d801b140: deleting -1711462110.393704 [0] gc: gc_delete_proxy_reader (0x7590d801b140, 110443d:bb7f10a5:18901533:301c4) -1711462110.393704 [0] recv: HEARTBEAT(#11:16..16 L(:1c1 16958.053195)110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:4c7@16(sync)) -1711462110.393729 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 -1711462110.393730 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#5:17/0: -1711462110.393748 [0] gc: reduced nlocs=3 -1711462110.393763 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.393770 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393774 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.393784 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393803 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393812 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393831 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7412@2 ] -1711462110.393841 [0] tev: traffic-xmit (1) 64 -1711462110.393833 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393864 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393875 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393883 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.393891 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393899 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393907 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.393914 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393926 [0] gc: a b c d -1711462110.393937 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.393949 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u } -1711462110.393961 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.393966 [0] gc: best = 2 -1711462110.393979 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.393991 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394000 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=11 -1711462110.394009 [0] gc: gc 0x7590d8050e30: deleting -1711462110.394020 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8050e30, 110443d:bb7f10a5:18901533:301c3) -1711462110.394028 [0] gc: gc 0x7590d8046810: deleting -1711462110.394033 [0] gc: gc_delete_proxy_reader (0x7590d8046810, 110443d:bb7f10a5:18901533:300c4) -1711462110.394038 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d8050e30, 110443d:bb7f10a5:18901533:301c3) -1711462110.394054 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 -1711462110.394063 [0] gc: reduced nlocs=3 -1711462110.394070 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394078 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394085 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394093 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394100 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394108 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394117 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394124 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394132 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394140 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394148 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.394155 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394164 [0] gc: a b c d -1711462110.394174 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.394185 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u } -1711462110.394196 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.394201 [0] gc: best = 2 -1711462110.394206 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394224 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394232 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=10 -1711462110.394250 [0] gc: gc 0x7590d804ffa0: deleting -1711462110.394259 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d804ffa0, 110443d:bb7f10a5:18901533:300c3) -1711462110.394269 [0] gc: gc 0x7590d804f540: deleting -1711462110.394275 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d804ffa0, 110443d:bb7f10a5:18901533:300c3) -1711462110.394278 [0] gc: gc_delete_proxy_reader (0x7590d804f540, 110443d:bb7f10a5:18901533:200c7) -1711462110.394309 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 -1711462110.394319 [0] gc: reduced nlocs=3 -1711462110.394327 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394335 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394345 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394355 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394362 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394373 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394381 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394389 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394397 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394404 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394412 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.394419 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394426 [0] gc: a b c d -1711462110.394436 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.394447 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u } -1711462110.394458 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.394464 [0] gc: best = 2 -1711462110.394471 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394482 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394491 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=9 -1711462110.394501 [0] gc: gc 0x7590d804eae0: deleting -1711462110.394508 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d804eae0, 110443d:bb7f10a5:18901533:200c2) -1711462110.394518 [0] gc: gc 0x7590d804e080: deleting -1711462110.394527 [0] gc: gc_delete_proxy_reader (0x7590d804e080, 110443d:bb7f10a5:18901533:4c7) -1711462110.394527 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d804eae0, 110443d:bb7f10a5:18901533:200c2) -1711462110.394544 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 -1711462110.394555 [0] gc: reduced nlocs=3 -1711462110.394563 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394577 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394587 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394595 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394608 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394615 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394623 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394630 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394638 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394644 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394655 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.394666 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394673 [0] gc: a b c d -1711462110.394689 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.394700 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u } -1711462110.394710 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.394716 [0] gc: best = 2 -1711462110.394723 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394735 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394744 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=8 -1711462110.394753 [0] gc: gc 0x7590d804d470: deleting -1711462110.394759 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d804d470, 110443d:bb7f10a5:18901533:4c2) -1711462110.394767 [0] gc: gc 0x7590d804cb90: deleting -1711462110.394775 [0] gc: gc_delete_proxy_reader (0x7590d804cb90, 110443d:bb7f10a5:18901533:3c7) -1711462110.394776 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d804d470, 110443d:bb7f10a5:18901533:4c2) -1711462110.394792 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 udp/10.101.12.71:7412@2 -1711462110.394803 [0] gc: reduced nlocs=3 -1711462110.394811 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394820 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394827 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394835 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394843 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394852 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394859 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394867 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.394876 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394885 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394892 [0] gc: rdidx 3 lidx udp/10.101.12.71:7412@2 1 -> 0 -1711462110.394904 [0] gc: rdidx 3 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394914 [0] gc: a b c d -1711462110.394924 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u .. } -1711462110.394933 [0] gc: loc 1 = udp/10.101.12.71:7412@2 1 { .. .. .. +u } -1711462110.394943 [0] gc: loc 2 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462110.394949 [0] gc: best = 2 -1711462110.394955 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394967 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394976 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=7 -1711462110.394984 [0] gc: gc 0x7590d8021440: deleting -1711462110.394989 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8021440, 110443d:bb7f10a5:18901533:3c2) -1711462110.394997 [0] gc: gc 0x7590d804fb10: deleting -1711462110.395003 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d8021440, 110443d:bb7f10a5:18901533:3c2) -1711462110.395004 [0] gc: gc_delete_proxy_reader (0x7590d804fb10, 110443d:bb7f10a5:18901533:100c7) -1711462110.395023 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=6 -1711462110.395031 [0] gc: gc 0x7590d804f0b0: deleting -1711462110.395038 [0] gc: gc_delete_proxy_participant(0x7590d804f0b0, 110443d:bb7f10a5:18901533:1c1) -1711462110.395044 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=5 -1711462110.395050 [0] gc: gc 0x7590d804e650: deleting -1711462110.395063 [0] gc: gc_delete_proxy_reader (0x7590d804e650, 110e21c:d0762c48:a9f0fb28:504) -1711462110.395078 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7411@2 -1711462110.395085 [0] gc: reduced nlocs=2 -1711462110.395092 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395100 [0] gc: rdidx 0 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.395109 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 1 -> 8 -1711462110.395115 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395123 [0] gc: rdidx 1 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.395130 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 1 -> 8 -1711462110.395138 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395145 [0] gc: rdidx 2 lidx udp/10.101.12.71:7411@2 0 -> 0 -1711462110.395152 [0] gc: rdidx 2 lidx udp/239.255.0.1:7401@2 1 -> 8 -1711462110.395160 [0] gc: a b c -1711462110.395170 [0] gc: loc 0 = udp/10.101.12.71:7411@2 -1 { +u +u +u } -1711462110.395179 [0] gc: loc 1 = udp/239.255.0.1:7401@2 0 { +1 +1 +1 } -1711462110.395190 [0] gc: best = 0 -1711462110.395197 [0] gc: simple udp/10.101.12.71:7411@2 -1711462110.395209 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:403): udp/10.101.12.71:7411@2 (burst size 4294901760 rexmit 1048576) -1711462110.395218 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=12 -1711462110.395227 [0] gc: gc 0x7590d8050e30: deleting -1711462110.395233 [0] gc: gc_delete_proxy_writer (0x7590d8050e30, 110443d:bb7f10a5:18901533:301c3) -1711462110.395242 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:301c4, 3): 3 -> 3 -1711462110.395251 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=4 -1711462110.395258 [0] gc: gc 0x7590d804ffa0: deleting -1711462110.395266 [0] gc: gc_delete_proxy_writer (0x7590d804ffa0, 110443d:bb7f10a5:18901533:300c3) -1711462110.395274 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:300c4, 3): 3 -> 3 -1711462110.395283 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=3 -1711462110.395291 [0] gc: gc 0x7590d804eae0: deleting -1711462110.395298 [0] gc: gc_delete_proxy_writer (0x7590d804eae0, 110443d:bb7f10a5:18901533:200c2) -1711462110.395307 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:200c7, 6): 6 -> 6 -1711462110.395317 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=2 -1711462110.395324 [0] gc: gc 0x7590d804d470: deleting -1711462110.395332 [0] gc: gc_delete_proxy_writer (0x7590d804d470, 110443d:bb7f10a5:18901533:4c2) -1711462110.395339 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:4c7, 5): 5 -> 5 -1711462110.395349 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=1 -1711462110.395357 [0] gc: gc 0x7590d8021440: deleting -1711462110.395363 [0] gc: gc_delete_proxy_writer (0x7590d8021440, 110443d:bb7f10a5:18901533:3c2) -1711462110.395371 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:3c7, 6): 6 -> 6 -1711462110.395381 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=0, freeing -1711462110.395387 [0] gc: lease_unregister(l 0x7590d80151b0 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.395395 [0] gc: lease_free(l 0x7590d80151b0 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.395402 [0] gc: lease_free(l 0x7590d8015130 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.395412 [0] gc: ddsi_remove_deleted_participant_guid(110443d:bb7f10a5:18901533:1c1 for_what=3) -1711462110.395423 [0] gc: gc 0x7590d8014d00: deleting -1711462110.395619 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.395644 [0] recv: HEARTBEAT(#11:20..20 L(:1c1 16958.055136)110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0: 110d7b4:17c5b8c5:94bd0ff4:3c7@20(sync)) -1711462110.395658 [0] tev: acknack 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#5:21/0: -1711462110.395673 [0] tev: send acknack(rd 110d7b4:17c5b8c5:94bd0ff4:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.395684 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.395720 [0] tev: nn_xpack_send 64: 0x7590e000139c:20 0x7590e400d398:44 [ udp/10.101.12.71:7412@2 ] -1711462110.395727 [0] tev: traffic-xmit (1) 64 -1711462110.404301 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.404312 [0] recv: INFOTS(1711462110.397918357) -1711462110.404336 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #2) -1711462110.404352 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.404356 [0] recv: INFOTS(1711462110.397918357) -1711462110.404377 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #2) -1711462110.404408 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110e21c:d0762c48:a9f0fb28:1c1}} -1711462110.404434 [0] dq.builtin: SPDP ST3 110e21c:d0762c48:a9f0fb28:1c1ddsi_delete_proxy_participant_by_guid(110e21c:d0762c48:a9f0fb28:1c1) - deleting -1711462110.404452 [0] dq.builtin: => EVERYONE -1711462110.404481 [0] dq.builtin: delete_ppt(110e21c:d0762c48:a9f0fb28:1c1) - deleting dependent proxy participants -1711462110.404493 [0] dq.builtin: delete_ppt(110e21c:d0762c48:a9f0fb28:1c1) - deleting endpoints -1711462110.404531 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:301c4) - deleting -1711462110.404557 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:301c3) - deleting -1711462110.404571 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:300c4) - deleting -1711462110.404573 [0] gc: gc 0x7590d801b140: not yet, shortsleep -1711462110.404585 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:300c3) - deleting -1711462110.404616 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:200c7) - deleting -1711462110.404633 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:200c2) - deleting -1711462110.404646 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:4c7) - deleting -1711462110.404659 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:4c2) - deleting -1711462110.404683 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:3c7) - deleting -1711462110.404696 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:3c2) - deleting -1711462110.404707 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:100c7) - deleting -1711462110.404714 [0] dq.builtin: delete -1711462110.404730 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110e21c:d0762c48:a9f0fb28:1c1}} -1711462110.404739 [0] dq.builtin: SPDP ST3 110e21c:d0762c48:a9f0fb28:1c1 unknown not allowed -1711462110.405657 [0] gc: gc 0x7590d801b140: deleting -1711462110.405666 [0] gc: gc_delete_proxy_reader (0x7590d801b140, 110e21c:d0762c48:a9f0fb28:301c4) -1711462110.405690 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 -1711462110.405703 [0] gc: reduced nlocs=2 -1711462110.405715 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.405726 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.405735 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.405746 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.405754 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.405779 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.405788 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.405798 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.405806 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.405818 [0] gc: a b c -1711462110.405832 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u } -1711462110.405842 [0] gc: loc 1 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.405850 [0] gc: best = 0 -1711462110.405858 [0] gc: simple udp/10.101.12.71:7410@2 -1711462110.405870 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:301c3): udp/10.101.12.71:7410@2 (burst size 4294901760 rexmit 1048576) -1711462110.405881 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=11 -1711462110.405891 [0] gc: gc 0x7590d8020ea0: deleting -1711462110.405900 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8020ea0, 110e21c:d0762c48:a9f0fb28:301c3) -1711462110.405909 [0] gc: gc 0x7590d8050e30: deleting -1711462110.405916 [0] gc: gc_delete_proxy_reader (0x7590d8050e30, 110e21c:d0762c48:a9f0fb28:300c4) -1711462110.405917 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d8020ea0, 110e21c:d0762c48:a9f0fb28:301c3) -1711462110.405933 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 -1711462110.405955 [0] gc: reduced nlocs=2 -1711462110.405964 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.405973 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.405982 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.405991 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.405999 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406023 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406031 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406039 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406049 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406058 [0] gc: a b c -1711462110.406070 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u } -1711462110.406081 [0] gc: loc 1 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.406087 [0] gc: best = 0 -1711462110.406094 [0] gc: simple udp/10.101.12.71:7410@2 -1711462110.406106 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:300c3): udp/10.101.12.71:7410@2 (burst size 4294901760 rexmit 1048576) -1711462110.406116 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=10 -1711462110.406126 [0] gc: gc 0x7590d8046810: deleting -1711462110.406134 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8046810, 110e21c:d0762c48:a9f0fb28:300c3) -1711462110.406143 [0] gc: gc 0x7590d8018b90: deleting -1711462110.406150 [0] gc: gc_delete_proxy_reader (0x7590d8018b90, 110e21c:d0762c48:a9f0fb28:200c7) -1711462110.406150 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d8046810, 110e21c:d0762c48:a9f0fb28:300c3) -1711462110.406167 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 -1711462110.406181 [0] gc: reduced nlocs=2 -1711462110.406190 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406201 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406210 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406218 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406227 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406244 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406252 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406261 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406269 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406280 [0] gc: a b c -1711462110.406293 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u } -1711462110.406303 [0] gc: loc 1 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.406309 [0] gc: best = 0 -1711462110.406316 [0] gc: simple udp/10.101.12.71:7410@2 -1711462110.406328 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:200c2): udp/10.101.12.71:7410@2 (burst size 4294901760 rexmit 1048576) -1711462110.406338 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=9 -1711462110.406353 [0] gc: gc 0x7590d8018130: deleting -1711462110.406360 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d8018130, 110e21c:d0762c48:a9f0fb28:200c2) -1711462110.406368 [0] gc: gc 0x7590d804f540: deleting -1711462110.406374 [0] gc: gc_delete_proxy_reader (0x7590d804f540, 110e21c:d0762c48:a9f0fb28:4c7) -1711462110.406373 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d8018130, 110e21c:d0762c48:a9f0fb28:200c2) -1711462110.406402 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 -1711462110.406412 [0] gc: reduced nlocs=2 -1711462110.406422 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406431 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406439 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406445 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406455 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406463 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406472 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406480 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406488 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406496 [0] gc: a b c -1711462110.406507 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u } -1711462110.406517 [0] gc: loc 1 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.406524 [0] gc: best = 0 -1711462110.406530 [0] gc: simple udp/10.101.12.71:7410@2 -1711462110.406541 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:4c2): udp/10.101.12.71:7410@2 (burst size 4294901760 rexmit 1048576) -1711462110.406550 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=8 -1711462110.406558 [0] gc: gc 0x7590d80176d0: deleting -1711462110.406565 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d80176d0, 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.406573 [0] gc: gc 0x7590d804e080: deleting -1711462110.406579 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d80176d0, 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.406582 [0] gc: gc_delete_proxy_reader (0x7590d804e080, 110e21c:d0762c48:a9f0fb28:3c7) -1711462110.406611 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7410@2 -1711462110.406618 [0] gc: reduced nlocs=2 -1711462110.406627 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406640 [0] gc: rdidx 0 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406649 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406656 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406665 [0] gc: rdidx 1 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406679 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406688 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406697 [0] gc: rdidx 2 lidx udp/10.101.12.71:7410@2 0 -> 0 -1711462110.406705 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406716 [0] gc: a b c -1711462110.406725 [0] gc: loc 0 = udp/10.101.12.71:7410@2 -1 { +u +u +u } -1711462110.406735 [0] gc: loc 1 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.406740 [0] gc: best = 0 -1711462110.406748 [0] gc: simple udp/10.101.12.71:7410@2 -1711462110.406759 [0] gc: ddsi_rebuild_writer_addrset(110d7b4:17c5b8c5:94bd0ff4:3c2): udp/10.101.12.71:7410@2 (burst size 4294901760 rexmit 1048576) -1711462110.406768 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=7 -1711462110.406777 [0] gc: gc 0x7590d804cb90: deleting -1711462110.406783 [0] gc: gc_delete_proxy_writer_dqueue(0x7590d804cb90, 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.406790 [0] gc: gc 0x7590d8016090: deleting -1711462110.406796 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7590d804cb90, 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.406798 [0] gc: gc_delete_proxy_reader (0x7590d8016090, 110e21c:d0762c48:a9f0fb28:100c7) -1711462110.406822 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=6 -1711462110.406830 [0] gc: gc 0x7590d80157d0: deleting -1711462110.406837 [0] gc: gc_delete_proxy_participant(0x7590d80157d0, 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.406843 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=5 -1711462110.406850 [0] gc: gc 0x7590d8020ea0: deleting -1711462110.406857 [0] gc: gc_delete_proxy_writer (0x7590d8020ea0, 110e21c:d0762c48:a9f0fb28:301c3) -1711462110.406867 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:301c4, 3): 3 -> 3 -1711462110.406878 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=4 -1711462110.406886 [0] gc: gc 0x7590d8046810: deleting -1711462110.406893 [0] gc: gc_delete_proxy_writer (0x7590d8046810, 110e21c:d0762c48:a9f0fb28:300c3) -1711462110.406903 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:300c4, 3): 3 -> 3 -1711462110.406915 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=3 -1711462110.406924 [0] gc: gc 0x7590d8018130: deleting -1711462110.406930 [0] gc: gc_delete_proxy_writer (0x7590d8018130, 110e21c:d0762c48:a9f0fb28:200c2) -1711462110.406939 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:200c7, 5): 6 -> 5 -1711462110.406949 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=2 -1711462110.406957 [0] gc: gc 0x7590d80176d0: deleting -1711462110.406964 [0] gc: gc_delete_proxy_writer (0x7590d80176d0, 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.406973 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:4c7, 6): 5 -> 6 -1711462110.406984 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=1 -1711462110.406993 [0] gc: gc 0x7590d804cb90: deleting -1711462110.406999 [0] gc: gc_delete_proxy_writer (0x7590d804cb90, 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.407008 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:3c7, 6): 6 -> 6 -1711462110.407019 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=0, freeing -1711462110.407027 [0] gc: lease_unregister(l 0x7590d8001c90 guid 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.407035 [0] gc: lease_free(l 0x7590d8001c90 guid 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.407042 [0] gc: lease_free(l 0x7590d8001c10 guid 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.407059 [0] gc: ddsi_remove_deleted_participant_guid(110e21c:d0762c48:a9f0fb28:1c1 for_what=3) -1711462110.407069 [0] gc: gc 0x7590d8014d00: deleting -1711462110.413296 [0] tev: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:403 multicasting (rel-prd 3 seq-eq-max 3 seq 90 maxseq 46) -1711462110.413378 [0] tev: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:403) sent, resched in 0.1 s (min-ack 46, avail-seq 90, xmit 90) -1711462110.413396 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.413497 [0] tev: nn_xpack_send 52: 0x7590e000139c:20 0x7590e400d3a8:32 [ udp/10.101.12.71:7411@2 ] -1711462110.413505 [0] tev: traffic-xmit (1) 52 -1711462110.413823 [0] recvUC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.413838 [0] recvUC: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.413861 [0] recvUC: ACKNACK(F#7:91/0: L(:1c1 16958.073337) 110cd0d:56a27910:57318171:504 -> 110d7b4:17c5b8c5:94bd0ff4:403 ACK44 RM0) -1711462110.413868 [0] recvUC: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.413880 [0] recvUC: ACKNACK(F#23:91/0: L(:1c1 16958.073337) 110cd0d:e3b81b8d:1ccb65b1:504 -> 110d7b4:17c5b8c5:94bd0ff4:403 ACK44 RM0) -1711462110.413887 [0] recvUC: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.413900 [0] recvUC: ACKNACK(F#14:91/0: L(:1c1 16958.073337) 110cd0d:79d6eeac:ea4a8907:504 -> 110d7b4:17c5b8c5:94bd0ff4:403remove_acked_messages: deleting lingering writer 110d7b4:17c5b8c5:94bd0ff4:403 -1711462110.413907 [0] recvUC: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:403) ... -1711462110.413921 [0] recvUC: => EVERYONE -1711462110.413943 [0] recvUC: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:403) state transition 2 -> 3 -1711462110.413954 [0] recvUC: ACK44 RM0) -1711462110.413975 [0] gc: gc 0x7590d4002ab0: deleting -1711462110.413995 [0] gc: gc_delete_writer(0x7590d4002ab0, 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.414041 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:3c2 #90: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:403}} -1711462110.414058 [0] gc: non-timed queue now has 1 items -1711462110.414084 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:3c2:#90/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.414109 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:403 @ 0x556f7899f434) user 5 builtin 12 -1711462110.414146 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590d8018674:28 [ udp/10.101.12.71:7410@2 ] -1711462110.414150 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:504) ... -1711462110.414170 [0] python3: => EVERYONE -1711462110.414155 [0] tev: traffic-xmit (1) 96 -1711462110.414213 [0] gc: gc 0x556f781e6be0: deleting -1711462110.414237 [0] gc: gc_delete_reader(0x556f781e6be0, 110d7b4:17c5b8c5:94bd0ff4:504) -1711462110.414257 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:504, 3): reader no longer exists -1711462110.414272 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:504, 4): reader no longer exists -1711462110.414286 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:504, 3): reader no longer exists -1711462110.414321 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:4c2 #80: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:504}} -1711462110.414343 [0] gc: non-timed queue now has 1 items -1711462110.414370 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(data(110d7b4:17c5b8c5:94bd0ff4:4c2:#80/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.414371 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:504 @ 0x556f78809dd4) user 4 builtin 12 -1711462110.414409 [0] tev: nn_xpack_send 96: 0x7590e000139c:20 0x7590e400d3a8:48 0x7590d8028224:28 [ udp/10.101.12.71:7410@2 ] -1711462110.414438 [0] tev: traffic-xmit (1) 96 -1711462110.414474 [0] tev: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:4c2 multicasting (rel-prd 3 seq-eq-max 3 seq 80 maxseq 79) -1711462110.414475 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:107) ... -1711462110.414498 [0] tev: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:4c2) sent, resched in 0.1 s (min-ack 79, avail-seq 80, xmit 80) -1711462110.414528 [0] python3: => EVERYONE -1711462110.414537 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.414566 [0] gc: gc 0x556f7899e420: not yet, shortsleep -1711462110.414572 [0] tev: nn_xpack_send 52: 0x7590e000139c:20 0x7590e400d3a8:32 [ udp/10.101.12.71:7410@2 ] -1711462110.414588 [0] tev: traffic-xmit (1) 52 -1711462110.414813 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.414826 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.414847 [0] recv: ACKNACK(F#5:81/0: L(:1c1 16958.074326) 110cd0d:56a27910:57318171:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK1 RM0) -1711462110.414855 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.414864 [0] recv: ACKNACK(F#13:81/0: L(:1c1 16958.074326) 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK1 RM0) -1711462110.414869 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.414879 [0] recv: ACKNACK(F#8:81/0: L(:1c1 16958.074326) 110cd0d:79d6eeac:ea4a8907:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2 ACK1 RM1) -1711462110.415652 [0] gc: gc 0x556f7899e420: deleting -1711462110.415673 [0] gc: gc_delete_reader(0x556f7899e420, 110d7b4:17c5b8c5:94bd0ff4:107) -1711462110.415696 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:107 @ 0x556f7877a504) user 3 builtin 12 -1711462110.415767 [0] gc: gc 0x556f781e6be0: deleting -1711462110.415779 [0] gc: gc 0x556f78a2f640: deleting -1711462110.415790 [0] gc: gc 0x556f78b57230: deleting -1711462110.415780 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:207) ... -1711462110.415824 [0] python3: => EVERYONE -1711462110.415851 [0] gc: gc 0x556f789d3cf0: deleting -1711462110.415859 [0] gc: gc_delete_reader(0x556f789d3cf0, 110d7b4:17c5b8c5:94bd0ff4:207) -1711462110.415880 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:207 @ 0x556f789485f4) user 2 builtin 12 -1711462110.415908 [0] gc: gc 0x556f789d3cf0: not yet, shortsleep -1711462110.415920 [0] tev: writer_hbcontrol: wr 110d7b4:17c5b8c5:94bd0ff4:3c2 multicasting (rel-prd 3 seq-eq-max 3 seq 90 maxseq 89) -1711462110.415932 [0] python3: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:307) ... -1711462110.415949 [0] tev: heartbeat(wr 110d7b4:17c5b8c5:94bd0ff4:3c2) sent, resched in 0.1 s (min-ack 89, avail-seq 90, xmit 90) -1711462110.415960 [0] python3: => EVERYONE -1711462110.415977 [0] tev: xpack_addmsg 0x7590e0001390 0x7590e400d2b0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.416018 [0] tev: nn_xpack_send 52: 0x7590e000139c:20 0x7590e400d3a8:32 [ udp/10.101.12.71:7410@2 ] -1711462110.416024 [0] tev: traffic-xmit (1) 52 -1711462110.416215 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 168 from udp/10.101.12.71:50807 -1711462110.416223 [0] recv: INFODST(110d7b4:17c5b8c5:94bd0ff4) -1711462110.416231 [0] recv: ACKNACK(F#6:91/0: L(:1c1 16958.075726) 110cd0d:56a27910:57318171:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK1 RM0) -1711462110.416236 [0] recv: INFOSRC(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) -1711462110.416243 [0] recv: ACKNACK(F#16:91/0: L(:1c1 16958.075726) 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK1 RM0) -1711462110.416261 [0] recv: INFOSRC(110cd0d:79d6eeac:ea4a8907 vendor 1.16) -1711462110.416270 [0] recv: ACKNACK(F#10:91/0: L(:1c1 16958.075726) 110cd0d:79d6eeac:ea4a8907:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2 ACK1 RM1) -1711462110.416975 [0] gc: gc 0x556f789d3cf0: deleting -1711462110.416992 [0] gc: gc 0x556f78b57230: deleting -1711462110.417000 [0] gc: gc 0x556f78a2f640: deleting -1711462110.417007 [0] gc: gc 0x556f781e6be0: deleting -1711462110.417013 [0] gc: gc 0x556f789a7a40: deleting -1711462110.417019 [0] gc: gc 0x556f78a1a470: deleting -1711462110.417029 [0] gc: gc 0x556f789e4e40: deleting -1711462110.417035 [0] gc: gc_delete_reader(0x556f789e4e40, 110d7b4:17c5b8c5:94bd0ff4:307) -1711462110.417053 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:307 @ 0x556f78949de4) user 1 builtin 12 -1711462110.417063 [0] gc: gc 0x7590d8014d00: not yet, shortsleep -1711462110.417107 [0] python3: ddsi_delete_participant (110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.417127 [0] python3: => EVERYONE -1711462110.417152 [0] python3: trigger_recv_threads: 0 many 0x556f78782ff0 -1711462110.417176 [0] python3: trigger_recv_threads: 1 single udp/239.255.0.1:7401 -1711462110.417207 [0] recv: done -1711462110.417246 [0] recvMC: done -1711462110.417266 [0] python3: trigger_recv_threads: 2 single udp/10.101.12.71:7415 -1711462110.417332 [0] recvUC: done -1711462110.417579 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:56a27910:57318171:1c1) - deleting -1711462110.417593 [0] python3: => EVERYONE -1711462110.417603 [0] python3: delete_ppt(110cd0d:56a27910:57318171:1c1) - deleting dependent proxy participants -1711462110.417614 [0] python3: delete_ppt(110cd0d:56a27910:57318171:1c1) - deleting endpoints -1711462110.417621 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1604) => EVERYONE -1711462110.417628 [0] python3: - deleting -1711462110.417635 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1504) => EVERYONE -1711462110.417641 [0] python3: - deleting -1711462110.417654 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1404) => EVERYONE -1711462110.417659 [0] python3: - deleting -1711462110.417665 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1304) => EVERYONE -1711462110.417670 [0] python3: - deleting -1711462110.417676 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:1104) => EVERYONE -1711462110.417681 [0] python3: - deleting -1711462110.417687 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:f04) => EVERYONE -1711462110.417692 [0] python3: - deleting -1711462110.417698 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:d04) => EVERYONE -1711462110.417703 [0] python3: - deleting -1711462110.417709 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:b04) => EVERYONE -1711462110.417714 [0] python3: - deleting -1711462110.417720 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:904) => EVERYONE -1711462110.417726 [0] python3: - deleting -1711462110.417732 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:504) => EVERYONE -1711462110.417737 [0] python3: - deleting -1711462110.417743 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:1203) - deleting -1711462110.417748 [0] python3: => EVERYONE -1711462110.417756 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:1003) - deleting -1711462110.417760 [0] python3: => EVERYONE -1711462110.417766 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:e03) - deleting -1711462110.417770 [0] python3: => EVERYONE -1711462110.417777 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:c03) - deleting -1711462110.417781 [0] python3: => EVERYONE -1711462110.417788 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:a03) - deleting -1711462110.417793 [0] python3: => EVERYONE -1711462110.417811 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:803) - deleting -1711462110.417816 [0] python3: => EVERYONE -1711462110.417822 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:703) - deleting -1711462110.417826 [0] python3: => EVERYONE -1711462110.417833 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:403) - deleting -1711462110.417837 [0] python3: => EVERYONE -1711462110.417844 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:301c4) - deleting -1711462110.417852 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:301c3) - deleting -1711462110.417860 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:300c4) - deleting -1711462110.417866 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:300c3) - deleting -1711462110.417875 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:200c7) - deleting -1711462110.417882 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:200c2) - deleting -1711462110.417889 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:4c7) - deleting -1711462110.417896 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:4c2) - deleting -1711462110.417902 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:3c7) - deleting -1711462110.417908 [0] python3: ddsi_delete_proxy_writer (110cd0d:56a27910:57318171:3c2) - deleting -1711462110.417916 [0] python3: ddsi_delete_proxy_reader (110cd0d:56a27910:57318171:100c7) - deleting -1711462110.417922 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting -1711462110.417926 [0] python3: => EVERYONE -1711462110.417932 [0] python3: delete_ppt(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting dependent proxy participants -1711462110.417937 [0] python3: delete_ppt(110cd0d:e3b81b8d:1ccb65b1:1c1) - deleting endpoints -1711462110.417943 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1604) => EVERYONE -1711462110.417947 [0] python3: - deleting -1711462110.417953 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1504) => EVERYONE -1711462110.417957 [0] python3: - deleting -1711462110.417962 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1404) => EVERYONE -1711462110.417967 [0] python3: - deleting -1711462110.417972 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1304) => EVERYONE -1711462110.417977 [0] python3: - deleting -1711462110.417982 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:1104) => EVERYONE -1711462110.417987 [0] python3: - deleting -1711462110.417992 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:f04) => EVERYONE -1711462110.417997 [0] python3: - deleting -1711462110.418002 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:d04) => EVERYONE -1711462110.418007 [0] python3: - deleting -1711462110.418012 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:b04) => EVERYONE -1711462110.418016 [0] python3: - deleting -1711462110.418022 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:904) => EVERYONE -1711462110.418026 [0] python3: - deleting -1711462110.418031 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:504) => EVERYONE -1711462110.418035 [0] python3: - deleting -1711462110.418040 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:1203) - deleting -1711462110.418045 [0] python3: => EVERYONE -1711462110.418051 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:1003) - deleting -1711462110.418055 [0] python3: => EVERYONE -1711462110.418061 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:e03) - deleting -1711462110.418065 [0] python3: => EVERYONE -1711462110.418071 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:c03) - deleting -1711462110.418075 [0] python3: => EVERYONE -1711462110.418085 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:a03) - deleting -1711462110.418089 [0] python3: => EVERYONE -1711462110.418095 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:803) - deleting -1711462110.418099 [0] python3: => EVERYONE -1711462110.418105 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:703) - deleting -1711462110.418109 [0] python3: => EVERYONE -1711462110.418114 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:403) - deleting -1711462110.418118 [0] python3: => EVERYONE -1711462110.418125 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:301c4) - deleting -1711462110.418131 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:301c3) - deleting -1711462110.418137 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:300c4) - deleting -1711462110.418143 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:300c3) - deleting -1711462110.418149 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:200c7) - deleting -1711462110.418155 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:200c2) - deleting -1711462110.418162 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:4c7) - deleting -1711462110.418165 [0] gc: gc 0x7590d8014d00: deleting -1711462110.418168 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:4c2) - deleting -1711462110.418189 [0] gc: gc 0x556f781e6be0: deleting -1711462110.418206 [0] gc: gc 0x556f78a2f640: deleting -1711462110.418197 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:3c7) - deleting -1711462110.418218 [0] gc: gc 0x556f78b57230: deleting -1711462110.418230 [0] python3: ddsi_delete_proxy_writer (110cd0d:e3b81b8d:1ccb65b1:3c2) - deleting -1711462110.418239 [0] python3: ddsi_delete_proxy_reader (110cd0d:e3b81b8d:1ccb65b1:100c7) - deleting -1711462110.418247 [0] python3: ddsi_delete_proxy_participant_by_guid(110cd0d:79d6eeac:ea4a8907:1c1) - deleting -1711462110.418251 [0] python3: => EVERYONE -1711462110.418259 [0] python3: delete_ppt(110cd0d:79d6eeac:ea4a8907:1c1) - deleting dependent proxy participants -1711462110.418264 [0] python3: delete_ppt(110cd0d:79d6eeac:ea4a8907:1c1) - deleting endpoints -1711462110.418271 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1604) => EVERYONE -1711462110.418277 [0] python3: - deleting -1711462110.418272 [0] gc: gc 0x556f789d3cf0: deleting -1711462110.418286 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1504) => EVERYONE -1711462110.418301 [0] python3: - deleting -1711462110.418295 [0] gc: gc 0x556f7899de30: deleting -1711462110.418307 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1404) => EVERYONE -1711462110.418326 [0] python3: - deleting -1711462110.418319 [0] gc: gc_delete_participant (0x556f7899de30, 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.418331 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1304) => EVERYONE -1711462110.418345 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 0:0:0:0 @ (nil)) user 0 builtin 12 -1711462110.418348 [0] python3: - deleting -1711462110.418365 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:1104) => EVERYONE -1711462110.418370 [0] python3: - deleting -1711462110.418376 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:f04) => EVERYONE -1711462110.418380 [0] python3: - deleting -1711462110.418386 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:d04) => EVERYONE -1711462110.418387 [0] gc: write_sample 110d7b4:17c5b8c5:94bd0ff4:100c2 #2: ST3 DCPSParticipant/ParticipantBuiltinTopicData:{participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1}} -1711462110.418391 [0] python3: - deleting -1711462110.418410 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:b04) => EVERYONE -1711462110.418411 [0] gc: non-timed queue now has 1 items -1711462110.418414 [0] python3: - deleting -1711462110.418452 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:904) => EVERYONE -1711462110.418440 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:100c2) ... -1711462110.418457 [0] python3: - deleting -1711462110.418477 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:504) => EVERYONE -1711462110.418483 [0] python3: - deleting -1711462110.418469 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:100c2) ... -1711462110.418489 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:1203) - deleting -1711462110.418506 [0] python3: => EVERYONE -1711462110.418515 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:1003) - deleting -1711462110.418520 [0] python3: => EVERYONE -1711462110.418506 [0] gc: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:100c2) state transition 0 -> 3 -1711462110.418527 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:e03) - deleting -1711462110.418539 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:3c2) ... -1711462110.418549 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:3c2) ... -1711462110.418542 [0] python3: => EVERYONE -1711462110.418570 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:c03) - deleting -1711462110.418576 [0] python3: => EVERYONE -1711462110.418562 [0] gc: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:3c2) state transition 0 -> 3 -1711462110.418582 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:a03) - deleting -1711462110.418598 [0] python3: => EVERYONE -1711462110.418592 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4c2) ... -1711462110.418605 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:803) - deleting -1711462110.418613 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:4c2) ... -1711462110.418622 [0] python3: => EVERYONE -1711462110.418629 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:703) - deleting -1711462110.418633 [0] python3: => EVERYONE -1711462110.418633 [0] gc: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:4c2) state transition 0 -> 3 -1711462110.418642 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:403) - deleting -1711462110.418666 [0] python3: => EVERYONE -1711462110.418658 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:200c2) ... -1711462110.418673 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:301c4) - deleting -1711462110.418683 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:200c2) ... -1711462110.418695 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:301c3) - deleting -1711462110.418704 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:300c4) - deleting -1711462110.418712 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:300c3) - deleting -1711462110.418720 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:200c7) - deleting -1711462110.418727 [0] gc: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:200c2) state transition 0 -> 3 -1711462110.418741 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:100c7) ... -1711462110.418746 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:200c2) - deleting -1711462110.418753 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:2c2) - unknown guid -1711462110.418755 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:4c7) - deleting -1711462110.418763 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:2c7) - unknown guid -1711462110.418775 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:4c2) - deleting -1711462110.418783 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:3c7) - deleting -1711462110.418789 [0] python3: ddsi_delete_proxy_writer (110cd0d:79d6eeac:ea4a8907:3c2) - deleting -1711462110.418802 [0] python3: ddsi_delete_proxy_reader (110cd0d:79d6eeac:ea4a8907:100c7) - deleting -1711462110.418792 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:3c7) ... -1711462110.418825 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:4c7) ... -1711462110.418835 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:200c7) ... -1711462110.418845 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:300c3) ... -1711462110.418852 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:300c3) ... -1711462110.418860 [0] gc: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:300c3) state transition 0 -> 3 -1711462110.418867 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:301c3) ... -1711462110.418873 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:301c3) ... -1711462110.418881 [0] gc: writer_set_state(110d7b4:17c5b8c5:94bd0ff4:301c3) state transition 0 -> 3 -1711462110.418888 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:300c4) ... -1711462110.418896 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:301c4) ... -1711462110.418905 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:ff0003c2) - unknown guid -1711462110.418911 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:ff0003c7) - unknown guid -1711462110.418917 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:ff0004c2) - unknown guid -1711462110.418923 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:ff0004c7) - unknown guid -1711462110.418929 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:ff0200c2) - unknown guid -1711462110.418934 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:ff0200c7) - unknown guid -1711462110.418940 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:201c3) - unknown guid -1711462110.418946 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:201c4) - unknown guid -1711462110.418952 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:ff0101c2) - unknown guid -1711462110.418957 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:ff0101c7) - unknown guid -1711462110.418963 [0] gc: ddsi_delete_writer_nolinger(guid 110d7b4:17c5b8c5:94bd0ff4:ff0202c3) - unknown guid -1711462110.418968 [0] gc: delete_reader_guid(guid 110d7b4:17c5b8c5:94bd0ff4:ff0202c4) - unknown guid -1711462110.418976 [0] gc: gc 0x556f7899dc00: deleting -1711462110.418982 [0] gc: gc_delete_proxy_reader (0x556f7899dc00, 110cd0d:56a27910:57318171:1604) -1711462110.418990 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=29 -1711462110.419005 [0] gc: gc 0x556f789c67e0: deleting -1711462110.419011 [0] gc: gc_delete_proxy_reader (0x556f789c67e0, 110cd0d:56a27910:57318171:1504) -1711462110.419017 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=28 -1711462110.419028 [0] gc: gc 0x556f789e2a90: deleting -1711462110.419034 [0] gc: gc_delete_proxy_reader (0x556f789e2a90, 110cd0d:56a27910:57318171:1404) -1711462110.419040 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=27 -1711462110.419050 [0] gc: gc 0x556f78a3b790: deleting -1711462110.419057 [0] gc: gc_delete_proxy_reader (0x556f78a3b790, 110cd0d:56a27910:57318171:1304) -1711462110.419063 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=26 -1711462110.419074 [0] gc: gc 0x556f7883cee0: deleting -1711462110.419080 [0] gc: gc_delete_proxy_reader (0x556f7883cee0, 110cd0d:56a27910:57318171:1104) -1711462110.419086 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=25 -1711462110.419099 [0] gc: gc 0x556f7899fac0: deleting -1711462110.419106 [0] gc: gc_delete_proxy_reader (0x556f7899fac0, 110cd0d:56a27910:57318171:f04) -1711462110.419122 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=24 -1711462110.419134 [0] gc: gc 0x556f7888fca0: deleting -1711462110.419140 [0] gc: gc_delete_proxy_reader (0x556f7888fca0, 110cd0d:56a27910:57318171:d04) -1711462110.419146 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=23 -1711462110.419157 [0] gc: gc 0x556f78805850: deleting -1711462110.419163 [0] gc: gc_delete_proxy_reader (0x556f78805850, 110cd0d:56a27910:57318171:b04) -1711462110.419170 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=22 -1711462110.419182 [0] gc: gc 0x556f788bcb70: deleting -1711462110.419188 [0] gc: gc_delete_proxy_reader (0x556f788bcb70, 110cd0d:56a27910:57318171:904) -1711462110.419194 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=21 -1711462110.419205 [0] gc: gc 0x556f7888f150: deleting -1711462110.419211 [0] gc: gc_delete_proxy_reader (0x556f7888f150, 110cd0d:56a27910:57318171:504) -1711462110.419218 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=20 -1711462110.419229 [0] gc: gc 0x556f788bad20: deleting -1711462110.419236 [0] gc: gc_delete_proxy_writer_dqueue(0x556f788bad20, 110cd0d:56a27910:57318171:1203) -1711462110.419245 [0] gc: gc 0x556f789295a0: deleting -1711462110.419252 [0] gc: gc_delete_proxy_writer_dqueue(0x556f789295a0, 110cd0d:56a27910:57318171:1003) -1711462110.419258 [0] gc: gc 0x556f781b6bc0: deleting -1711462110.419264 [0] gc: gc_delete_proxy_writer_dqueue(0x556f781b6bc0, 110cd0d:56a27910:57318171:e03) -1711462110.419270 [0] gc: gc 0x556f78209640: deleting -1711462110.419280 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78209640, 110cd0d:56a27910:57318171:c03) -1711462110.419287 [0] gc: gc 0x556f78315b80: deleting -1711462110.419295 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78315b80, 110cd0d:56a27910:57318171:a03) -1711462110.419269 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f788bad20, 110cd0d:56a27910:57318171:1203) -1711462110.419304 [0] gc: gc 0x556f78301d90: deleting -1711462110.419315 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f789295a0, 110cd0d:56a27910:57318171:1003) -1711462110.419323 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f781b6bc0, 110cd0d:56a27910:57318171:e03) -1711462110.419328 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f78209640, 110cd0d:56a27910:57318171:c03) -1711462110.419330 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78301d90, 110cd0d:56a27910:57318171:803) -1711462110.419333 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f78315b80, 110cd0d:56a27910:57318171:a03) -1711462110.419342 [0] gc: gc 0x556f782d7be0: deleting -1711462110.419353 [0] gc: gc_delete_proxy_writer_dqueue(0x556f782d7be0, 110cd0d:56a27910:57318171:703) -1711462110.419361 [0] gc: gc 0x556f782e6d60: deleting -1711462110.419370 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f78301d90, 110cd0d:56a27910:57318171:803) -1711462110.419377 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f782d7be0, 110cd0d:56a27910:57318171:703) -1711462110.419384 [0] gc: gc_delete_proxy_writer_dqueue(0x556f782e6d60, 110cd0d:56a27910:57318171:403) -1711462110.419396 [0] gc: gc 0x556f78328970: deleting -1711462110.419400 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f782e6d60, 110cd0d:56a27910:57318171:403) -1711462110.419403 [0] gc: gc_delete_proxy_reader (0x556f78328970, 110cd0d:56a27910:57318171:301c4) -1711462110.419414 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=19 -1711462110.419425 [0] gc: gc 0x556f782e5800: deleting -1711462110.419431 [0] gc: gc_delete_proxy_writer_dqueue(0x556f782e5800, 110cd0d:56a27910:57318171:301c3) -1711462110.419439 [0] gc: gc 0x556f782a7020: deleting -1711462110.419446 [0] gc: gc_delete_proxy_reader (0x556f782a7020, 110cd0d:56a27910:57318171:300c4) -1711462110.419452 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f782e5800, 110cd0d:56a27910:57318171:301c3) -1711462110.419462 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=18 -1711462110.419474 [0] gc: gc 0x556f7831cf50: deleting -1711462110.419482 [0] gc: gc_delete_proxy_writer_dqueue(0x556f7831cf50, 110cd0d:56a27910:57318171:300c3) -1711462110.419491 [0] gc: gc 0x556f781c6da0: deleting -1711462110.419497 [0] gc: gc_delete_proxy_reader (0x556f781c6da0, 110cd0d:56a27910:57318171:200c7) -1711462110.419497 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f7831cf50, 110cd0d:56a27910:57318171:300c3) -1711462110.419510 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=17 -1711462110.419521 [0] gc: gc 0x556f78653850: deleting -1711462110.419529 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78653850, 110cd0d:56a27910:57318171:200c2) -1711462110.419537 [0] gc: gc 0x556f786eda90: deleting -1711462110.419543 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f78653850, 110cd0d:56a27910:57318171:200c2) -1711462110.419544 [0] gc: gc_delete_proxy_reader (0x556f786eda90, 110cd0d:56a27910:57318171:4c7) -1711462110.419572 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=16 -1711462110.419582 [0] gc: gc 0x556f78b3cd80: deleting -1711462110.419588 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78b3cd80, 110cd0d:56a27910:57318171:4c2) -1711462110.419597 [0] gc: gc 0x556f78391120: deleting -1711462110.419604 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f78b3cd80, 110cd0d:56a27910:57318171:4c2) -1711462110.419605 [0] gc: gc_delete_proxy_reader (0x556f78391120, 110cd0d:56a27910:57318171:3c7) -1711462110.419628 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=15 -1711462110.419638 [0] gc: gc 0x556f78739460: deleting -1711462110.419644 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78739460, 110cd0d:56a27910:57318171:3c2) -1711462110.419652 [0] gc: gc 0x556f78795cb0: deleting -1711462110.419658 [0] gc: gc_delete_proxy_reader (0x556f78795cb0, 110cd0d:56a27910:57318171:100c7) -1711462110.419667 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=14 -1711462110.419659 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f78739460, 110cd0d:56a27910:57318171:3c2) -1711462110.419677 [0] gc: gc 0x556f78769270: deleting -1711462110.419696 [0] gc: gc_delete_proxy_participant(0x556f78769270, 110cd0d:56a27910:57318171:1c1) -1711462110.419706 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=13 -1711462110.419711 [0] gc: gc 0x556f78731d20: deleting -1711462110.419717 [0] gc: gc_delete_proxy_reader (0x556f78731d20, 110cd0d:e3b81b8d:1ccb65b1:1604) -1711462110.419725 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=29 -1711462110.419738 [0] gc: gc 0x556f787a7cf0: deleting -1711462110.419745 [0] gc: gc_delete_proxy_reader (0x556f787a7cf0, 110cd0d:e3b81b8d:1ccb65b1:1504) -1711462110.419751 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=28 -1711462110.419762 [0] gc: gc 0x556f78794970: deleting -1711462110.419768 [0] gc: gc_delete_proxy_reader (0x556f78794970, 110cd0d:e3b81b8d:1ccb65b1:1404) -1711462110.419774 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=27 -1711462110.419785 [0] gc: gc 0x556f787a2f90: deleting -1711462110.419791 [0] gc: gc_delete_proxy_reader (0x556f787a2f90, 110cd0d:e3b81b8d:1ccb65b1:1304) -1711462110.419797 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=26 -1711462110.419809 [0] gc: gc 0x556f78768f60: deleting -1711462110.419815 [0] gc: gc_delete_proxy_reader (0x556f78768f60, 110cd0d:e3b81b8d:1ccb65b1:1104) -1711462110.419822 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=25 -1711462110.419840 [0] gc: gc 0x556f78746930: deleting -1711462110.419847 [0] gc: gc_delete_proxy_reader (0x556f78746930, 110cd0d:e3b81b8d:1ccb65b1:f04) -1711462110.419854 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=24 -1711462110.419866 [0] gc: gc 0x556f7877dd30: deleting -1711462110.419872 [0] gc: gc_delete_proxy_reader (0x556f7877dd30, 110cd0d:e3b81b8d:1ccb65b1:d04) -1711462110.419879 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=23 -1711462110.419889 [0] gc: gc 0x556f787b7c40: deleting -1711462110.419895 [0] gc: gc_delete_proxy_reader (0x556f787b7c40, 110cd0d:e3b81b8d:1ccb65b1:b04) -1711462110.419902 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=22 -1711462110.419912 [0] gc: gc 0x556f787bcfe0: deleting -1711462110.419918 [0] gc: gc_delete_proxy_reader (0x556f787bcfe0, 110cd0d:e3b81b8d:1ccb65b1:904) -1711462110.419925 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=21 -1711462110.419936 [0] gc: gc 0x556f787b7990: deleting -1711462110.419942 [0] gc: gc_delete_proxy_reader (0x556f787b7990, 110cd0d:e3b81b8d:1ccb65b1:504) -1711462110.419948 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=20 -1711462110.419958 [0] gc: gc 0x556f787a68a0: deleting -1711462110.419964 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787a68a0, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.419973 [0] gc: gc 0x556f787c61d0: deleting -1711462110.419979 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787c61d0, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.419987 [0] gc: gc 0x556f78759ef0: deleting -1711462110.419991 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f787a68a0, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.419993 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78759ef0, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.420006 [0] gc: gc 0x556f787bf2f0: deleting -1711462110.420012 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787bf2f0, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.420015 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f787c61d0, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.420025 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f78759ef0, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.420019 [0] gc: gc 0x556f787c5f40: deleting -1711462110.420031 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f787bf2f0, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.420041 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787c5f40, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.420062 [0] gc: gc 0x556f787c18d0: deleting -1711462110.420072 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787c18d0, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.420066 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f787c5f40, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.420084 [0] gc: gc 0x556f78741fe0: deleting -1711462110.420088 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f787c18d0, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.420091 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78741fe0, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.420112 [0] gc: gc 0x556f7876c720: deleting -1711462110.420116 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f78741fe0, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.420119 [0] gc: gc_delete_proxy_writer_dqueue(0x556f7876c720, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.420132 [0] gc: gc 0x556f787d3830: deleting -1711462110.420135 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f7876c720, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.420138 [0] gc: gc_delete_proxy_reader (0x556f787d3830, 110cd0d:e3b81b8d:1ccb65b1:301c4) -1711462110.420150 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=19 -1711462110.420158 [0] gc: gc 0x556f787ebaf0: deleting -1711462110.420172 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787ebaf0, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.420181 [0] gc: gc 0x556f78788fd0: deleting -1711462110.420188 [0] gc: gc_delete_proxy_reader (0x556f78788fd0, 110cd0d:e3b81b8d:1ccb65b1:300c4) -1711462110.420188 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f787ebaf0, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.420198 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=18 -1711462110.420222 [0] gc: gc 0x556f787fa080: deleting -1711462110.420229 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787fa080, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.420237 [0] gc: gc 0x556f787b0d90: deleting -1711462110.420244 [0] gc: gc_delete_proxy_reader (0x556f787b0d90, 110cd0d:e3b81b8d:1ccb65b1:200c7) -1711462110.420243 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f787fa080, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.420256 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=17 -1711462110.420266 [0] gc: gc 0x556f787c05b0: deleting -1711462110.420272 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787c05b0, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.420280 [0] gc: gc 0x556f787669a0: deleting -1711462110.420285 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f787c05b0, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.420286 [0] gc: gc_delete_proxy_reader (0x556f787669a0, 110cd0d:e3b81b8d:1ccb65b1:4c7) -1711462110.420309 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=16 -1711462110.420318 [0] gc: gc 0x556f78788b50: deleting -1711462110.420324 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78788b50, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.420332 [0] gc: gc 0x556f78a2f640: deleting -1711462110.420338 [0] gc: gc_delete_proxy_reader (0x556f78a2f640, 110cd0d:e3b81b8d:1ccb65b1:3c7) -1711462110.420339 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f78788b50, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.420348 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=15 -1711462110.420371 [0] gc: gc 0x556f781e6be0: deleting -1711462110.420379 [0] gc: gc_delete_proxy_writer_dqueue(0x556f781e6be0, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.420387 [0] gc: gc 0x556f787c55e0: deleting -1711462110.420394 [0] gc: gc_delete_proxy_reader (0x556f787c55e0, 110cd0d:e3b81b8d:1ccb65b1:100c7) -1711462110.420393 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f781e6be0, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.420402 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=14 -1711462110.420413 [0] gc: gc 0x556f787af560: deleting -1711462110.420420 [0] gc: gc_delete_proxy_participant(0x556f787af560, 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.420426 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=13 -1711462110.420432 [0] gc: gc 0x556f78b57230: deleting -1711462110.420437 [0] gc: gc_delete_proxy_reader (0x556f78b57230, 110cd0d:79d6eeac:ea4a8907:1604) -1711462110.420444 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=29 -1711462110.420455 [0] gc: gc 0x556f789d3cf0: deleting -1711462110.420462 [0] gc: gc_delete_proxy_reader (0x556f789d3cf0, 110cd0d:79d6eeac:ea4a8907:1504) -1711462110.420468 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=28 -1711462110.420478 [0] gc: gc 0x556f787cb400: deleting -1711462110.420484 [0] gc: gc_delete_proxy_reader (0x556f787cb400, 110cd0d:79d6eeac:ea4a8907:1404) -1711462110.420491 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=27 -1711462110.420523 [0] gc: gc 0x556f787ef290: deleting -1711462110.420540 [0] gc: gc_delete_proxy_reader (0x556f787ef290, 110cd0d:79d6eeac:ea4a8907:1304) -1711462110.420548 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=26 -1711462110.420565 [0] gc: gc 0x556f78792df0: deleting -1711462110.420572 [0] gc: gc_delete_proxy_reader (0x556f78792df0, 110cd0d:79d6eeac:ea4a8907:1104) -1711462110.420579 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=25 -1711462110.420590 [0] gc: gc 0x556f78792570: deleting -1711462110.420649 [0] gc: gc_delete_proxy_reader (0x556f78792570, 110cd0d:79d6eeac:ea4a8907:f04) -1711462110.420665 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=24 -1711462110.420681 [0] gc: gc 0x556f787f2800: deleting -1711462110.420687 [0] gc: gc_delete_proxy_reader (0x556f787f2800, 110cd0d:79d6eeac:ea4a8907:d04) -1711462110.420691 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=23 -1711462110.420698 [0] gc: gc 0x556f787cde60: deleting -1711462110.420702 [0] gc: gc_delete_proxy_reader (0x556f787cde60, 110cd0d:79d6eeac:ea4a8907:b04) -1711462110.420707 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=22 -1711462110.420713 [0] gc: gc 0x556f7887cfe0: deleting -1711462110.420717 [0] gc: gc_delete_proxy_reader (0x556f7887cfe0, 110cd0d:79d6eeac:ea4a8907:904) -1711462110.420721 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=21 -1711462110.420728 [0] gc: gc 0x556f784f75f0: deleting -1711462110.420732 [0] gc: gc_delete_proxy_reader (0x556f784f75f0, 110cd0d:79d6eeac:ea4a8907:504) -1711462110.420736 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=20 -1711462110.420743 [0] gc: gc 0x556f787ea650: deleting -1711462110.420748 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787ea650, 110cd0d:79d6eeac:ea4a8907:1203) -1711462110.420755 [0] gc: gc 0x556f7883c120: deleting -1711462110.420760 [0] gc: gc_delete_proxy_writer_dqueue(0x556f7883c120, 110cd0d:79d6eeac:ea4a8907:1003) -1711462110.420762 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f787ea650, 110cd0d:79d6eeac:ea4a8907:1203) -1711462110.420772 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f7883c120, 110cd0d:79d6eeac:ea4a8907:1003) -1711462110.420764 [0] gc: gc 0x7590d8014d00: deleting -1711462110.420781 [0] gc: gc_delete_writer(0x7590d8014d00, 110d7b4:17c5b8c5:94bd0ff4:100c2) -1711462110.420802 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:100c2 @ 0x556f78728c14) user 0 builtin 11 -1711462110.420808 [0] gc: gc 0x556f788497e0: deleting -1711462110.420812 [0] gc: gc_delete_proxy_writer_dqueue(0x556f788497e0, 110cd0d:79d6eeac:ea4a8907:e03) -1711462110.420818 [0] gc: gc 0x556f78a8afc0: deleting -1711462110.420823 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78a8afc0, 110cd0d:79d6eeac:ea4a8907:c03) -1711462110.420825 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f788497e0, 110cd0d:79d6eeac:ea4a8907:e03) -1711462110.420834 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f78a8afc0, 110cd0d:79d6eeac:ea4a8907:c03) -1711462110.420827 [0] gc: gc 0x7590d8012530: deleting -1711462110.420848 [0] gc: gc_delete_writer(0x7590d8012530, 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.420861 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:3c2 @ 0x556f78722e74) user 0 builtin 10 -1711462110.420866 [0] gc: gc 0x556f78840730: deleting -1711462110.420870 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78840730, 110cd0d:79d6eeac:ea4a8907:a03) -1711462110.420876 [0] gc: gc 0x556f788c3190: deleting -1711462110.420881 [0] gc: gc_delete_proxy_writer_dqueue(0x556f788c3190, 110cd0d:79d6eeac:ea4a8907:803) -1711462110.420883 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f78840730, 110cd0d:79d6eeac:ea4a8907:a03) -1711462110.420892 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f788c3190, 110cd0d:79d6eeac:ea4a8907:803) -1711462110.420885 [0] gc: gc 0x556f787e1070: deleting -1711462110.420913 [0] gc: gc_delete_proxy_writer_dqueue(0x556f787e1070, 110cd0d:79d6eeac:ea4a8907:703) -1711462110.420920 [0] gc: gc 0x7590d800fc90: deleting -1711462110.420924 [0] gc: gc_delete_writer(0x7590d800fc90, 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.420925 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f787e1070, 110cd0d:79d6eeac:ea4a8907:703) -1711462110.420939 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4c2 @ 0x556f787109e4) user 0 builtin 9 -1711462110.420945 [0] gc: gc 0x556f78814a00: deleting -1711462110.420950 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78814a00, 110cd0d:79d6eeac:ea4a8907:403) -1711462110.420956 [0] gc: gc 0x556f789ac8e0: deleting -1711462110.420961 [0] gc: gc_delete_proxy_reader (0x556f789ac8e0, 110cd0d:79d6eeac:ea4a8907:301c4) -1711462110.420963 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x556f78814a00, 110cd0d:79d6eeac:ea4a8907:403) -1711462110.420966 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=19 -1711462110.420973 [0] gc: gc 0x556f782d10f0: deleting -1711462110.420977 [0] gc: gc_delete_proxy_writer_dqueue(0x556f782d10f0, 110cd0d:79d6eeac:ea4a8907:301c3) -1711462110.420984 [0] gc: gc 0x556f789b18b0: deleting -1711462110.420988 [0] gc: gc_delete_proxy_reader (0x556f789b18b0, 110cd0d:79d6eeac:ea4a8907:300c4) -1711462110.420993 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=18 -1711462110.420998 [0] gc: gc 0x556f789c17b0: deleting -1711462110.421003 [0] gc: gc_delete_proxy_writer_dqueue(0x556f789c17b0, 110cd0d:79d6eeac:ea4a8907:300c3) -1711462110.421007 [0] gc: gc 0x7590d800b4e0: deleting -1711462110.421009 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f782d10f0, 110cd0d:79d6eeac:ea4a8907:301c3) -1711462110.421012 [0] gc: gc_delete_writer(0x7590d800b4e0, 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462110.421038 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f789c17b0, 110cd0d:79d6eeac:ea4a8907:300c3) -1711462110.421048 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:200c2 @ 0x556f7899a0d4) user 0 builtin 8 -1711462110.421054 [0] gc: gc 0x556f789f64f0: deleting -1711462110.421058 [0] gc: gc_delete_proxy_reader (0x556f789f64f0, 110cd0d:79d6eeac:ea4a8907:200c7) -1711462110.421063 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=17 -1711462110.421068 [0] gc: gc 0x7590d8006ae0: deleting -1711462110.421073 [0] gc: gc_delete_reader(0x7590d8006ae0, 110d7b4:17c5b8c5:94bd0ff4:100c7) -1711462110.421080 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:100c7 @ 0x556f78743644) user 0 builtin 7 -1711462110.421084 [0] gc: gc 0x556f78a03780: deleting -1711462110.421088 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78a03780, 110cd0d:79d6eeac:ea4a8907:200c2) -1711462110.421094 [0] gc: gc 0x556f78a0edf0: deleting -1711462110.421099 [0] gc: gc_delete_proxy_reader (0x556f78a0edf0, 110cd0d:79d6eeac:ea4a8907:4c7) -1711462110.421102 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f78a03780, 110cd0d:79d6eeac:ea4a8907:200c2) -1711462110.421104 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=16 -1711462110.421123 [0] gc: gc 0x556f78a26ec0: deleting -1711462110.421127 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78a26ec0, 110cd0d:79d6eeac:ea4a8907:4c2) -1711462110.421133 [0] gc: gc 0x556f78a2d150: deleting -1711462110.421137 [0] gc: gc_delete_proxy_reader (0x556f78a2d150, 110cd0d:79d6eeac:ea4a8907:3c7) -1711462110.421140 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f78a26ec0, 110cd0d:79d6eeac:ea4a8907:4c2) -1711462110.421142 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=15 -1711462110.421165 [0] gc: gc 0x556f78a47730: deleting -1711462110.421170 [0] gc: gc_delete_proxy_writer_dqueue(0x556f78a47730, 110cd0d:79d6eeac:ea4a8907:3c2) -1711462110.421176 [0] gc: gc 0x556f789ea750: deleting -1711462110.421180 [0] gc: gc_delete_proxy_reader (0x556f789ea750, 110cd0d:79d6eeac:ea4a8907:100c7) -1711462110.421184 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x556f78a47730, 110cd0d:79d6eeac:ea4a8907:3c2) -1711462110.421185 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=14 -1711462110.421206 [0] gc: gc 0x556f78996810: deleting -1711462110.421211 [0] gc: gc_delete_proxy_participant(0x556f78996810, 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.421215 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=13 -1711462110.421220 [0] gc: gc 0x7590d8005430: deleting -1711462110.421224 [0] gc: gc_delete_reader(0x7590d8005430, 110d7b4:17c5b8c5:94bd0ff4:3c7) -1711462110.421232 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:3c7 @ 0x556f78777634) user 0 builtin 6 -1711462110.421236 [0] gc: gc 0x556f78999480: deleting -1711462110.421241 [0] gc: gc_delete_reader(0x556f78999480, 110d7b4:17c5b8c5:94bd0ff4:4c7) -1711462110.421248 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:4c7 @ 0x556f78768244) user 0 builtin 5 -1711462110.421253 [0] gc: gc 0x7590f0001a90: deleting -1711462110.421257 [0] gc: gc_delete_reader(0x7590f0001a90, 110d7b4:17c5b8c5:94bd0ff4:200c7) -1711462110.421265 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:200c7 @ 0x556f784892c4) user 0 builtin 4 -1711462110.421269 [0] gc: gc 0x7590f0001cc0: deleting -1711462110.421274 [0] gc: gc_delete_writer(0x7590f0001cc0, 110d7b4:17c5b8c5:94bd0ff4:300c3) -1711462110.421284 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:300c3 @ 0x556f7899c394) user 0 builtin 3 -1711462110.421288 [0] gc: gc 0x7590f0001ef0: deleting -1711462110.421292 [0] gc: gc_delete_writer(0x7590f0001ef0, 110d7b4:17c5b8c5:94bd0ff4:301c3) -1711462110.421322 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:301c3 @ 0x556f7899c914) user 0 builtin 2 -1711462110.421326 [0] gc: gc 0x7590f0002120: deleting -1711462110.421331 [0] gc: gc_delete_reader(0x7590f0002120, 110d7b4:17c5b8c5:94bd0ff4:300c4) -1711462110.421337 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:300c4 @ 0x556f78489174) user 0 builtin 1 -1711462110.421342 [0] gc: gc 0x7590f0002350: deleting -1711462110.421347 [0] gc: gc_delete_reader(0x7590f0002350, 110d7b4:17c5b8c5:94bd0ff4:301c4) -1711462110.421353 [0] gc: ddsi_unref_participant(110d7b4:17c5b8c5:94bd0ff4:1c1 @ 0x556f78729120 <- 110d7b4:17c5b8c5:94bd0ff4:301c4 @ 0x556f78200c94) user 0 builtin 0 -1711462110.421366 [0] gc: ddsi_remove_deleted_participant_guid(110d7b4:17c5b8c5:94bd0ff4:1c1 for_what=1) -1711462110.421373 [0] gc: gc 0x556f7899dc00: deleting -1711462110.421379 [0] gc: gc 0x7590f0002580: deleting -1711462110.421387 [0] gc: gc 0x7590f00027b0: deleting -1711462110.421392 [0] gc: gc 0x7590f00029e0: deleting -1711462110.421396 [0] gc: gc 0x7590f0002c10: deleting -1711462110.421401 [0] gc: gc 0x7590f0002e40: deleting -1711462110.421406 [0] gc: gc 0x7590f0003070: deleting -1711462110.421411 [0] gc: gc 0x556f78805850: deleting -1711462110.421416 [0] gc: gc 0x556f788bcb70: deleting -1711462110.421421 [0] gc: gc 0x556f7888f150: deleting -1711462110.421426 [0] gc: gc 0x556f788bad20: deleting -1711462110.421430 [0] gc: gc_delete_proxy_writer (0x556f788bad20, 110cd0d:56a27910:57318171:1203) -1711462110.421436 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=12 -1711462110.421450 [0] gc: gc 0x556f789295a0: deleting -1711462110.421455 [0] gc: gc_delete_proxy_writer (0x556f789295a0, 110cd0d:56a27910:57318171:1003) -1711462110.421460 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=11 -1711462110.421465 [0] gc: gc 0x556f781b6bc0: deleting -1711462110.421470 [0] gc: gc_delete_proxy_writer (0x556f781b6bc0, 110cd0d:56a27910:57318171:e03) -1711462110.421474 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=10 -1711462110.421480 [0] gc: gc 0x556f78209640: deleting -1711462110.421485 [0] gc: gc_delete_proxy_writer (0x556f78209640, 110cd0d:56a27910:57318171:c03) -1711462110.421490 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=9 -1711462110.421496 [0] gc: gc 0x556f78315b80: deleting -1711462110.421501 [0] gc: gc_delete_proxy_writer (0x556f78315b80, 110cd0d:56a27910:57318171:a03) -1711462110.421505 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=8 -1711462110.421512 [0] gc: gc 0x556f78301d90: deleting -1711462110.421516 [0] gc: gc_delete_proxy_writer (0x556f78301d90, 110cd0d:56a27910:57318171:803) -1711462110.421521 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=7 -1711462110.421527 [0] gc: gc 0x556f782d7be0: deleting -1711462110.421531 [0] gc: gc_delete_proxy_writer (0x556f782d7be0, 110cd0d:56a27910:57318171:703) -1711462110.421536 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=6 -1711462110.421542 [0] gc: gc 0x556f782e6d60: deleting -1711462110.421547 [0] gc: gc_delete_proxy_writer (0x556f782e6d60, 110cd0d:56a27910:57318171:403) -1711462110.421551 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=5 -1711462110.421557 [0] gc: gc 0x556f782e5800: deleting -1711462110.421562 [0] gc: gc_delete_proxy_writer (0x556f782e5800, 110cd0d:56a27910:57318171:301c3) -1711462110.421567 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:301c4, 2): reader no longer exists -1711462110.421572 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=4 -1711462110.421577 [0] gc: gc 0x556f7831cf50: deleting -1711462110.421582 [0] gc: gc_delete_proxy_writer (0x556f7831cf50, 110cd0d:56a27910:57318171:300c3) -1711462110.421587 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:300c4, 2): reader no longer exists -1711462110.421591 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=3 -1711462110.421596 [0] gc: gc 0x556f78653850: deleting -1711462110.421601 [0] gc: gc_delete_proxy_writer (0x556f78653850, 110cd0d:56a27910:57318171:200c2) -1711462110.421606 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:200c7, 5): reader no longer exists -1711462110.421610 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=2 -1711462110.421615 [0] gc: gc 0x556f78b3cd80: deleting -1711462110.421619 [0] gc: gc_delete_proxy_writer (0x556f78b3cd80, 110cd0d:56a27910:57318171:4c2) -1711462110.421624 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:4c7, 3): reader no longer exists -1711462110.421631 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=1 -1711462110.421636 [0] gc: gc 0x556f78739460: deleting -1711462110.421640 [0] gc: gc_delete_proxy_writer (0x556f78739460, 110cd0d:56a27910:57318171:3c2) -1711462110.421645 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:3c7, 4): reader no longer exists -1711462110.421650 [0] gc: ddsi_unref_proxy_participant(110cd0d:56a27910:57318171:1c1): refc=0, freeing -1711462110.421656 [0] gc: lease_unregister(l 0x7590d80071e0 guid 110cd0d:56a27910:57318171:1c1) -1711462110.421664 [0] gc: lease_free(l 0x7590d80071e0 guid 110cd0d:56a27910:57318171:1c1) -1711462110.421669 [0] gc: lease_free(l 0x7590d8007160 guid 110cd0d:56a27910:57318171:1c1) -1711462110.421677 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:56a27910:57318171:1c1 for_what=3) -1711462110.421683 [0] gc: gc 0x556f78769270: deleting -1711462110.421688 [0] gc: gc 0x556f787a7cf0: deleting -1711462110.421693 [0] gc: gc 0x556f78794970: deleting -1711462110.421698 [0] gc: gc 0x556f787a2f90: deleting -1711462110.421703 [0] gc: gc 0x556f78768f60: deleting -1711462110.421708 [0] gc: gc 0x556f78746930: deleting -1711462110.421712 [0] gc: gc 0x556f7877dd30: deleting -1711462110.421717 [0] gc: gc 0x556f787b7c40: deleting -1711462110.421721 [0] gc: gc 0x556f787bcfe0: deleting -1711462110.421726 [0] gc: gc 0x556f787b7990: deleting -1711462110.421731 [0] gc: gc 0x556f787a68a0: deleting -1711462110.421735 [0] gc: gc_delete_proxy_writer (0x556f787a68a0, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.421740 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=12 -1711462110.421747 [0] gc: gc 0x556f787c61d0: deleting -1711462110.421751 [0] gc: gc_delete_proxy_writer (0x556f787c61d0, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.421756 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=11 -1711462110.421762 [0] gc: gc 0x556f78759ef0: deleting -1711462110.421766 [0] gc: gc_delete_proxy_writer (0x556f78759ef0, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.421771 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=10 -1711462110.421778 [0] gc: gc 0x556f787bf2f0: deleting -1711462110.421783 [0] gc: gc_delete_proxy_writer (0x556f787bf2f0, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.421788 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=9 -1711462110.421794 [0] gc: gc 0x556f787c5f40: deleting -1711462110.421798 [0] gc: gc_delete_proxy_writer (0x556f787c5f40, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.421803 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=8 -1711462110.421810 [0] gc: gc 0x556f787c18d0: deleting -1711462110.421814 [0] gc: gc_delete_proxy_writer (0x556f787c18d0, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.421819 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=7 -1711462110.421824 [0] gc: gc 0x556f78741fe0: deleting -1711462110.421829 [0] gc: gc_delete_proxy_writer (0x556f78741fe0, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.421833 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=6 -1711462110.421840 [0] gc: gc 0x556f7876c720: deleting -1711462110.421844 [0] gc: gc_delete_proxy_writer (0x556f7876c720, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.421849 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=5 -1711462110.421854 [0] gc: gc 0x556f787ebaf0: deleting -1711462110.421859 [0] gc: gc_delete_proxy_writer (0x556f787ebaf0, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.421863 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:301c4, 2): reader no longer exists -1711462110.421868 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=4 -1711462110.421873 [0] gc: gc 0x556f787fa080: deleting -1711462110.421877 [0] gc: gc_delete_proxy_writer (0x556f787fa080, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.421882 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:300c4, 2): reader no longer exists -1711462110.421887 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=3 -1711462110.421892 [0] gc: gc 0x556f787c05b0: deleting -1711462110.421896 [0] gc: gc_delete_proxy_writer (0x556f787c05b0, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.421901 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:200c7, 6): reader no longer exists -1711462110.421909 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=2 -1711462110.421914 [0] gc: gc 0x556f78788b50: deleting -1711462110.421918 [0] gc: gc_delete_proxy_writer (0x556f78788b50, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.421924 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:4c7, 3): reader no longer exists -1711462110.421928 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=1 -1711462110.421933 [0] gc: gc 0x556f781e6be0: deleting -1711462110.421937 [0] gc: gc_delete_proxy_writer (0x556f781e6be0, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.421942 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:3c7, 3): reader no longer exists -1711462110.421947 [0] gc: ddsi_unref_proxy_participant(110cd0d:e3b81b8d:1ccb65b1:1c1): refc=0, freeing -1711462110.421951 [0] gc: lease_unregister(l 0x7590d800b990 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.421956 [0] gc: lease_free(l 0x7590d800b990 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.421960 [0] gc: lease_free(l 0x7590d800b910 guid 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.421967 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:e3b81b8d:1ccb65b1:1c1 for_what=3) -1711462110.421972 [0] gc: gc 0x556f787d3830: deleting -1711462110.421977 [0] gc: gc 0x556f789d3cf0: deleting -1711462110.421981 [0] gc: gc 0x556f787cb400: deleting -1711462110.421986 [0] gc: gc 0x556f787ef290: deleting -1711462110.421990 [0] gc: gc 0x556f78792df0: deleting -1711462110.421995 [0] gc: gc 0x556f78792570: deleting -1711462110.421999 [0] gc: gc 0x556f787f2800: deleting -1711462110.422003 [0] gc: gc 0x556f787cde60: deleting -1711462110.422007 [0] gc: gc 0x556f7887cfe0: deleting -1711462110.422012 [0] gc: gc 0x556f784f75f0: deleting -1711462110.422016 [0] gc: gc 0x556f787ea650: deleting -1711462110.422020 [0] gc: gc_delete_proxy_writer (0x556f787ea650, 110cd0d:79d6eeac:ea4a8907:1203) -1711462110.422025 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=12 -1711462110.422031 [0] gc: gc 0x556f7883c120: deleting -1711462110.422036 [0] gc: gc_delete_proxy_writer (0x556f7883c120, 110cd0d:79d6eeac:ea4a8907:1003) -1711462110.422040 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=11 -1711462110.422047 [0] gc: gc 0x556f788497e0: deleting -1711462110.422051 [0] gc: gc_delete_proxy_writer (0x556f788497e0, 110cd0d:79d6eeac:ea4a8907:e03) -1711462110.422056 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=10 -1711462110.422062 [0] gc: gc 0x556f78a8afc0: deleting -1711462110.422067 [0] gc: gc_delete_proxy_writer (0x556f78a8afc0, 110cd0d:79d6eeac:ea4a8907:c03) -1711462110.422071 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=9 -1711462110.422078 [0] gc: gc 0x556f78840730: deleting -1711462110.422082 [0] gc: gc_delete_proxy_writer (0x556f78840730, 110cd0d:79d6eeac:ea4a8907:a03) -1711462110.422087 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=8 -1711462110.422093 [0] gc: gc 0x556f788c3190: deleting -1711462110.422097 [0] gc: gc_delete_proxy_writer (0x556f788c3190, 110cd0d:79d6eeac:ea4a8907:803) -1711462110.422102 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=7 -1711462110.422108 [0] gc: gc 0x556f787e1070: deleting -1711462110.422112 [0] gc: gc_delete_proxy_writer (0x556f787e1070, 110cd0d:79d6eeac:ea4a8907:703) -1711462110.422117 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=6 -1711462110.422122 [0] gc: gc 0x556f78814a00: deleting -1711462110.422126 [0] gc: gc_delete_proxy_writer (0x556f78814a00, 110cd0d:79d6eeac:ea4a8907:403) -1711462110.422134 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=5 -1711462110.422140 [0] gc: gc 0x556f782d10f0: deleting -1711462110.422144 [0] gc: gc_delete_proxy_writer (0x556f782d10f0, 110cd0d:79d6eeac:ea4a8907:301c3) -1711462110.422149 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:301c4, 2): reader no longer exists -1711462110.422153 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=4 -1711462110.422158 [0] gc: gc 0x556f789c17b0: deleting -1711462110.422163 [0] gc: gc_delete_proxy_writer (0x556f789c17b0, 110cd0d:79d6eeac:ea4a8907:300c3) -1711462110.422168 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:300c4, 2): reader no longer exists -1711462110.422173 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=3 -1711462110.422178 [0] gc: gc 0x556f78a03780: deleting -1711462110.422182 [0] gc: gc_delete_proxy_writer (0x556f78a03780, 110cd0d:79d6eeac:ea4a8907:200c2) -1711462110.422187 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:200c7, 6): reader no longer exists -1711462110.422192 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=2 -1711462110.422196 [0] gc: gc 0x556f78a26ec0: deleting -1711462110.422200 [0] gc: gc_delete_proxy_writer (0x556f78a26ec0, 110cd0d:79d6eeac:ea4a8907:4c2) -1711462110.422205 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:4c7, 3): reader no longer exists -1711462110.422210 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=1 -1711462110.422215 [0] gc: gc 0x556f78a47730: deleting -1711462110.422219 [0] gc: gc_delete_proxy_writer (0x556f78a47730, 110cd0d:79d6eeac:ea4a8907:3c2) -1711462110.422224 [0] gc: ddsi_update_reader_init_acknack_count (110d7b4:17c5b8c5:94bd0ff4:3c7, 3): reader no longer exists -1711462110.422228 [0] gc: ddsi_unref_proxy_participant(110cd0d:79d6eeac:ea4a8907:1c1): refc=0, freeing -1711462110.422233 [0] gc: lease_unregister(l 0x7590d8010140 guid 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.422237 [0] gc: lease_free(l 0x7590d8010140 guid 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.422241 [0] gc: lease_free(l 0x7590d80100c0 guid 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.422247 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:79d6eeac:ea4a8907:1c1 for_what=3) -1711462110.422253 [0] gc: gc 0x7590d8014d00: deleting -1711462110.422257 [0] gc: gc 0x556f7899dc00: deleting -1711462110.422262 [0] gc: gc 0x556f789295a0: deleting -1711462110.422266 [0] gc: gc 0x556f781b6bc0: deleting -1711462110.422270 [0] gc: gc 0x556f78209640: deleting -1711462110.422274 [0] gc: gc 0x556f78315b80: deleting -1711462110.422279 [0] gc: gc 0x556f78301d90: deleting -1711462110.422283 [0] gc: gc 0x556f782d7be0: deleting -1711462110.422287 [0] gc: gc 0x556f782e6d60: deleting -1711462110.422292 [0] gc: gc 0x556f782e5800: deleting -1711462110.422297 [0] gc: gc 0x556f78795cb0: deleting -1711462110.422302 [0] gc: gc 0x556f787a7cf0: deleting -1711462110.422306 [0] gc: gc 0x556f787c61d0: deleting -1711462110.422310 [0] gc: gc 0x556f78759ef0: deleting -1711462110.422315 [0] gc: gc 0x556f787bf2f0: deleting -1711462110.422319 [0] gc: gc 0x556f787c5f40: deleting -1711462110.422323 [0] gc: gc 0x556f787c18d0: deleting -1711462110.422328 [0] gc: gc 0x556f78741fe0: deleting -1711462110.422332 [0] gc: gc 0x556f7876c720: deleting -1711462110.422336 [0] gc: gc 0x556f787ebaf0: deleting -1711462110.422340 [0] gc: gc 0x556f78769270: deleting -1711462110.422344 [0] gc: gc 0x556f789d3cf0: deleting -1711462110.422348 [0] gc: gc 0x556f7883c120: deleting -1711462110.422353 [0] gc: gc 0x556f788497e0: deleting -1711462110.422357 [0] gc: gc 0x556f78a8afc0: deleting -1711462110.422365 [0] gc: gc 0x556f78840730: deleting -1711462110.422369 [0] gc: gc 0x556f788c3190: deleting -1711462110.422373 [0] gc: gc 0x556f787e1070: deleting -1711462110.422378 [0] gc: gc 0x556f78814a00: deleting -1711462110.422382 [0] gc: gc 0x556f782d10f0: deleting -1711462110.422390 [0] gc: gc 0x556f787d3830: deleting -1711462110.422417 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:100c2) ... -1711462110.422438 [0] python3: writer_set_state(0:0:0:100c2) state transition 0 -> 3 -1711462110.422485 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:2c2) ... -1711462110.422491 [0] python3: writer_set_state(0:0:0:2c2) state transition 0 -> 3 -1711462110.422498 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:3c2) ... -1711462110.422492 [0] gc: gc 0x556f789d3cf0: not yet, shortsleep -1711462110.422503 [0] python3: writer_set_state(0:0:0:3c2) state transition 0 -> 3 -1711462110.422518 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:4c2) ... -1711462110.422523 [0] python3: writer_set_state(0:0:0:4c2) state transition 0 -> 3 -1711462110.423588 [0] gc: gc 0x556f789d3cf0: deleting -1711462110.423609 [0] gc: gc_delete_writer(0x556f789d3cf0, 0:0:0:100c2) -1711462110.423638 [0] gc: gc 0x556f781e6be0: deleting -1711462110.423644 [0] gc: gc_delete_writer(0x556f781e6be0, 0:0:0:2c2) -1711462110.423656 [0] gc: gc 0x556f78a0f590: deleting -1711462110.423660 [0] gc: gc_delete_writer(0x556f78a0f590, 0:0:0:3c2) -1711462110.423669 [0] gc: gc 0x556f78999480: deleting -1711462110.423673 [0] gc: gc_delete_writer(0x556f78999480, 0:0:0:4c2) -1711462110.423706 [0] gc: gc 0x556f78b57230: deleting -1711462110.424084 [0] python3: xpack_addmsg 0x556f78833260 0x7590e400d2b0 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.425157 [0] python3: nn_xpack_send 96: 0x556f7883326c:20 0x7590e400d3a8:48 0x7590f0001a64:28 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.0.(trunc) -1711462110.425166 [0] python3: traffic-xmit (101) 96 -1711462110.425186 [0] python3: leave conn 0x556f7872aec0 for (udp/239.255.0.1, *) interface (default): not leaving yet -1711462110.425203 [0] python3: leave conn 0x556f7875adb0 for (udp/239.255.0.1, *) interface (default): not leaving yet -1711462110.425212 [0] python3: leave conn 0x556f7872aec0 for (udp/239.255.0.1, *) interface udp/10.101.12.71 -1711462110.425228 [0] python3: leave conn 0x556f7875adb0 for (udp/239.255.0.1, *) interface udp/10.101.12.71 -1711462110.425237 [0] python3: ddsi_udp_release_conn multicast socket 7 port 7400 -1711462110.425260 [0] python3: ddsi_udp_release_conn multicast socket 8 port 7401 -1711462110.425268 [0] python3: ddsi_udp_release_conn unicast socket 5 port 7414 -1711462110.425275 [0] python3: ddsi_udp_release_conn unicast socket 6 port 7415 -1711462110.425282 [0] python3: ddsi_udp_release_conn unicast socket 9 port 40473 -1711462110.425292 [0] python3: udp finalized -1711462110.425564 [0] python3: Finis. -1711462090.567655 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 208 from udp/10.101.12.71:44991 -1711462090.567674 [0] recvMC: INFOTS(1711462090.567591438) -1711462090.567659 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 224 from udp/10.101.12.71:44991 -1711462090.567700 [0] recv: INFOTS(1711462090.567575352) -1711462090.567688 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #3 L(:1c1 16938.227177) 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: no heartbeat seen yet) -1711462090.567704 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #3 L(:1c1 16938.227203)) -1711462090.567717 [0] recvMC: HEARTBEAT(#5:3..3 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: end-of-tl-seq(rd 110cd0d:56a27910:57318171:504 #3) end-of-tl-seq(rd 110cd0d:e3b81b8d:1ccb65b1:504 #3) end-of-tl-seq(rd 110cd0d:79d6eeac:ea4a8907:504 #3) 110cd0d:56a27910:57318171:504@2 110cd0d:e3b81b8d:1ccb65b1:504@2 110cd0d:79d6eeac:ea4a8907:504@2) -1711462090.567723 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110443d:bb7f10a5:18901533:403: F#1:3/1:1 -1711462090.567729 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.567736 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007e50 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.567740 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110443d:bb7f10a5:18901533:403: F#8:3/1:1 -1711462090.567743 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.567746 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007fd0 0(control): niov 2 sz 68 => now niov 4 sz 124 -1711462090.567750 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110443d:bb7f10a5:18901533:403: F#17:3/1:1 -1711462090.567753 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.567731 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.567756 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007cd0 0(control): niov 4 sz 124 => now niov 6 sz 180 -1711462090.567776 [0] tev: nn_xpack_send 180: 0x7408c000139c:20 0x7408bc007f38:48 0x7408bc0080a0:24 0x7408bc0080c8:32 0x7408bc007da0:24 0x7408bc007dc8:32 [ udp/10.101.12.71:7417@2 ] -1711462090.567778 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.567778 [0] tev: traffic-xmit (1) 180 -1711462090.567794 [0] dq.builtin: => EVERYONE -1711462090.567808 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:703) scanning all rds of topic rt/parameter_events -1711462090.567817 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 192 from udp/10.101.12.71:44991 -1711462090.567830 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.567834 [0] recvUC: INFOTS(1711462090.567591438) -1711462090.567853 [0] recvUC: DATA(110443d:bb7f10a5:18901533:403 -> 110cd0d:56a27910:57318171:504 #3 L(:1c1 16938.227333) msr_in_sync(110cd0d:56a27910:57318171:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #3: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}}}}} =>110cd0d:56a27910:57318171:504 -1711462090.567870 [0] recvUC: msr_in_sync(110cd0d:e3b81b8d:1ccb65b1:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #3: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}}}}} =>110cd0d:e3b81b8d:1ccb65b1:504 -1711462090.567886 [0] recvUC: msr_in_sync(110cd0d:79d6eeac:ea4a8907:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #3: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}}}}} =>110cd0d:79d6eeac:ea4a8907:504 -1711462090.567893 [0] recvUC: ) -1711462090.567899 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 240 from udp/10.101.12.71:44991 -1711462090.567903 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.567906 [0] recvUC: INFOTS(1711462090.567591438) -1711462090.567910 [0] recvUC: DATA(110443d:bb7f10a5:18901533:403 -> 110cd0d:79d6eeac:ea4a8907:504 #3 L(:1c1 16938.227406)) -1711462090.567913 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.567919 [0] recvUC: HEARTBEAT(#6:3..3 110443d:bb7f10a5:18901533:403 -> 110cd0d:56a27910:57318171:504: 110cd0d:56a27910:57318171:504@3(sync)) -1711462090.567921 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110443d:bb7f10a5:18901533:403: F#2:4/0: -1711462090.567924 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.567924 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 288 from udp/10.101.12.71:44991 -1711462090.567925 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007e50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.567929 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.567933 [0] recvUC: INFOTS(1711462090.567591438) -1711462090.567938 [0] tev: nn_xpack_send 64: 0x7408c000139c:20 0x7408bc007f38:44 [ udp/10.101.12.71:7417@2 ] -1711462090.567940 [0] tev: traffic-xmit (1) 64 -1711462090.567938 [0] recvUC: DATA(110443d:bb7f10a5:18901533:403 -> 110cd0d:e3b81b8d:1ccb65b1:504 #3 L(:1c1 16938.227433)) -1711462090.567947 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.567953 [0] recvUC: HEARTBEAT(#7:3..3 110443d:bb7f10a5:18901533:403 -> 110cd0d:79d6eeac:ea4a8907:504: 110cd0d:79d6eeac:ea4a8907:504@3(sync)) -1711462090.567954 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110443d:bb7f10a5:18901533:403: F#9:4/0: -1711462090.567959 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.567956 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.567961 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007e50 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.567966 [0] recvUC: HEARTBEAT(#8:3..3 110443d:bb7f10a5:18901533:403 -> 110cd0d:e3b81b8d:1ccb65b1:504: 110cd0d:e3b81b8d:1ccb65b1:504@3(sync)) -1711462090.567967 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110443d:bb7f10a5:18901533:403: F#18:4/0: -1711462090.567976 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.567977 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007fd0 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.567981 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc007f38:44 0x7408bc0080a0:24 0x7408bc0080c8:28 [ udp/10.101.12.71:7417@2 ] -1711462090.567983 [0] tev: traffic-xmit (1) 116 -1711462090.568280 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 328 from udp/10.101.12.71:44991 -1711462090.568293 [0] recv: INFOTS(1711462090.568216939) -1711462090.568296 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #4 L(:1c1 16938.227797)) -1711462090.568300 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 324 from udp/10.101.12.71:44991 -1711462090.568308 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.1;">,topic_name="rr/static_transformer/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.568310 [0] recv: INFOTS(1711462090.568255806) -1711462090.568318 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #2 L(:1c1 16938.227814)) -1711462090.568326 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:803 reliable volatile writer unnamed: (default).rr/static_transformer/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.1;">,topic_name="rr/static_transformer/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.568334 [0] dq.builtin: => EVERYONE -1711462090.568336 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 256 from udp/10.101.12.71:44991 -1711462090.568340 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:803) scanning all rds of topic rr/static_transformer/describe_parametersReply -1711462090.568341 [0] recvMC: INFOTS(1711462090.568269284) -1711462090.568352 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.1;">,topic_name="rq/static_transformer/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:904},adlink_entity_factory=1} -1711462090.568355 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #4 L(:1c1 16938.227845) => EVERYONE -1711462090.568363 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:904 reliable volatile reader unnamed: (default).rq/static_transformer/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.1;">,topic_name="rq/static_transformer/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.568372 [0] dq.builtin: => EVERYONE -1711462090.568377 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:904) scanning all wrs of topic rq/static_transformer/describe_parametersRequest -1711462090.568378 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #4: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462090.568385 [0] recvMC: HEARTBEAT(F#9:4..4 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@4(sync) 110cd0d:e3b81b8d:1ccb65b1:504@4(sync) 110cd0d:79d6eeac:ea4a8907:504@4(sync)) -1711462090.568541 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 320 from udp/10.101.12.71:44991 -1711462090.568543 [0] recv: INFOTS(1711462090.568522519) -1711462090.568545 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #5 L(:1c1 16938.228047)) -1711462090.568558 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.2;">,topic_name="rr/static_transformer/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.568566 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:a03 reliable volatile writer unnamed: (default).rr/static_transformer/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.2;">,topic_name="rr/static_transformer/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.568570 [0] dq.builtin: => EVERYONE -1711462090.568574 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:a03) scanning all rds of topic rr/static_transformer/get_parametersReply -1711462090.568581 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 312 from udp/10.101.12.71:44991 -1711462090.568584 [0] recv: INFOTS(1711462090.568565213) -1711462090.568586 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #3 L(:1c1 16938.228088)) -1711462090.568593 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.2;">,topic_name="rq/static_transformer/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:b04},adlink_entity_factory=1} -1711462090.568603 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:b04 reliable volatile reader unnamed: (default).rq/static_transformer/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.2;">,topic_name="rq/static_transformer/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.568608 [0] dq.builtin: => EVERYONE -1711462090.568615 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:b04) scanning all wrs of topic rq/static_transformer/get_parametersRequest -1711462090.568628 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 304 from udp/10.101.12.71:44991 -1711462090.568631 [0] recvMC: INFOTS(1711462090.568575899) -1711462090.568636 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #5 L(:1c1 16938.228135) => EVERYONE -1711462090.568654 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #5: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462090.568658 [0] recvMC: HEARTBEAT(F#10:5..5 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@5(sync) 110cd0d:e3b81b8d:1ccb65b1:504@5(sync) 110cd0d:79d6eeac:ea4a8907:504@5(sync)) -1711462090.568867 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 328 from udp/10.101.12.71:44991 -1711462090.568873 [0] recv: INFOTS(1711462090.568841289) -1711462090.568878 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #6 L(:1c1 16938.228377)) -1711462090.568889 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.3;">,topic_name="rr/static_transformer/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.568901 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:c03 reliable volatile writer unnamed: (default).rr/static_transformer/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.3;">,topic_name="rr/static_transformer/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.568912 [0] dq.builtin: => EVERYONE -1711462090.568915 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 324 from udp/10.101.12.71:44991 -1711462090.568918 [0] recv: INFOTS(1711462090.568893800) -1711462090.568919 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:c03) scanning all rds of topic rr/static_transformer/get_parameter_typesReply -1711462090.568923 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #4 L(:1c1 16938.228421)) -1711462090.568929 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.3;">,topic_name="rq/static_transformer/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:d04},adlink_entity_factory=1} -1711462090.568942 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:d04 reliable volatile reader unnamed: (default).rq/static_transformer/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.3;">,topic_name="rq/static_transformer/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.568950 [0] dq.builtin: => EVERYONE -1711462090.568955 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:d04) scanning all wrs of topic rq/static_transformer/get_parameter_typesRequest -1711462090.568968 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 352 from udp/10.101.12.71:44991 -1711462090.568970 [0] recvMC: INFOTS(1711462090.568905973) -1711462090.568972 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #6 L(:1c1 16938.228474) => EVERYONE -1711462090.568991 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #6: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462090.568995 [0] recvMC: HEARTBEAT(F#11:6..6 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@6(sync) 110cd0d:e3b81b8d:1ccb65b1:504@6(sync) 110cd0d:79d6eeac:ea4a8907:504@6(sync)) -1711462090.569162 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 320 from udp/10.101.12.71:44991 -1711462090.569169 [0] recv: INFOTS(1711462090.569135598) -1711462090.569177 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #7 L(:1c1 16938.228673)) -1711462090.569188 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.4;">,topic_name="rr/static_transformer/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.569196 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:e03 reliable volatile writer unnamed: (default).rr/static_transformer/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.4;">,topic_name="rr/static_transformer/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.569201 [0] dq.builtin: => EVERYONE -1711462090.569206 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:e03) scanning all rds of topic rr/static_transformer/list_parametersReply -1711462090.569216 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 316 from udp/10.101.12.71:44991 -1711462090.569219 [0] recv: INFOTS(1711462090.569197394) -1711462090.569223 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #5 L(:1c1 16938.228723)) -1711462090.569230 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.4;">,topic_name="rq/static_transformer/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:f04},adlink_entity_factory=1} -1711462090.569238 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:f04 reliable volatile reader unnamed: (default).rq/static_transformer/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.4;">,topic_name="rq/static_transformer/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.569242 [0] dq.builtin: => EVERYONE -1711462090.569247 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:f04) scanning all wrs of topic rq/static_transformer/list_parametersRequest -1711462090.569281 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 400 from udp/10.101.12.71:44991 -1711462090.569288 [0] recvMC: INFOTS(1711462090.569209337) -1711462090.569294 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #7 L(:1c1 16938.228791) => EVERYONE -1711462090.569331 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #7: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462090.569341 [0] recvMC: HEARTBEAT(F#12:7..7 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@7(sync) 110cd0d:e3b81b8d:1ccb65b1:504@7(sync) 110cd0d:79d6eeac:ea4a8907:504@7(sync)) -1711462090.569463 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 320 from udp/10.101.12.71:44991 -1711462090.569468 [0] recv: INFOTS(1711462090.569441604) -1711462090.569473 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #8 L(:1c1 16938.228972)) -1711462090.569480 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.5;">,topic_name="rr/static_transformer/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.569489 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1003 reliable volatile writer unnamed: (default).rr/static_transformer/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.5;">,topic_name="rr/static_transformer/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.569494 [0] dq.builtin: => EVERYONE -1711462090.569498 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:1003) scanning all rds of topic rr/static_transformer/set_parametersReply -1711462090.569507 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 312 from udp/10.101.12.71:44991 -1711462090.569509 [0] recv: INFOTS(1711462090.569489785) -1711462090.569520 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #6 L(:1c1 16938.229013)) -1711462090.569527 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.5;">,topic_name="rq/static_transformer/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1104},adlink_entity_factory=1} -1711462090.569534 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1104 reliable volatile reader unnamed: (default).rq/static_transformer/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.5;">,topic_name="rq/static_transformer/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.569541 [0] dq.builtin: => EVERYONE -1711462090.569546 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:1104) scanning all wrs of topic rq/static_transformer/set_parametersRequest -1711462090.569564 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 448 from udp/10.101.12.71:44991 -1711462090.569570 [0] recvMC: INFOTS(1711462090.569500644) -1711462090.569576 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #8 L(:1c1 16938.229074) => EVERYONE -1711462090.569617 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #8: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,17,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,16,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462090.569623 [0] recvMC: HEARTBEAT(F#13:8..8 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@8(sync) 110cd0d:e3b81b8d:1ccb65b1:504@8(sync) 110cd0d:79d6eeac:ea4a8907:504@8(sync)) -1711462090.569732 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 344 from udp/10.101.12.71:44991 -1711462090.569736 [0] recv: INFOTS(1711462090.569710842) -1711462090.569743 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #9 L(:1c1 16938.229240)) -1711462090.569759 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.6;">,topic_name="rr/static_transformer/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.569773 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1203 reliable volatile writer unnamed: (default).rr/static_transformer/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.6;">,topic_name="rr/static_transformer/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.569782 [0] dq.builtin: => EVERYONE -1711462090.569788 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:1203) scanning all rds of topic rr/static_transformer/set_parameters_atomicallyReply -1711462090.569801 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 332 from udp/10.101.12.71:44991 -1711462090.569804 [0] recv: INFOTS(1711462090.569761244) -1711462090.569809 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #7 L(:1c1 16938.229308)) -1711462090.569817 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.6;">,topic_name="rq/static_transformer/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1304},adlink_entity_factory=1} -1711462090.569827 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1304 reliable volatile reader unnamed: (default).rq/static_transformer/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.44.3d.bb.7f.10.a5.18.90.15.33.0.0.0.6;">,topic_name="rq/static_transformer/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.569833 [0] dq.builtin: => EVERYONE -1711462090.569837 [0] dq.builtin: match_proxy_reader_with_writers(prd 110443d:bb7f10a5:18901533:1304) scanning all wrs of topic rq/static_transformer/set_parameters_atomicallyRequest -1711462090.569857 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 496 from udp/10.101.12.71:44991 -1711462090.569865 [0] recvMC: INFOTS(1711462090.569771719) -1711462090.569871 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #9 L(:1c1 16938.229368) => EVERYONE -1711462090.569916 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #9: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,19,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,16,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,16) -1711462090.569923 [0] recvMC: HEARTBEAT(F#14:9..9 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@9(sync) 110cd0d:e3b81b8d:1ccb65b1:504@9(sync) 110cd0d:79d6eeac:ea4a8907:504@9(sync)) -1711462090.570486 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462090.570494 [0] recv: INFOTS(1711462090.570393375) -1711462090.570500 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462090.570507 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462090.570510 [0] recv: INFOTS(1711462090.570393375) -1711462090.570515 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462090.570511 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.570552 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 bes fc3f NEW (0x00000000-0x0000002c-0x00000000-0x00000000-0x00000000 op-Alienware-x16-R1/0.10.4/Linux/Linux) (data udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 meta udp/239.255.0.1:7400@2 udp/10.101.12.71:7418@2) QOS={user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{}} -1711462090.570561 [0] dq.builtin: lease_new(tdur 10000000000 guid 110aba1:7b19badd:ce0adb73:1c1) @ 0x7408c800c690 -1711462090.570565 [0] dq.builtin: lease_new(tdur 10000000000 guid 110aba1:7b19badd:ce0adb73:1c1) @ 0x7408c80256c0 -1711462090.570582 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:100c7) scanning participants tgt=0 -1711462090.570591 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:3c2) scanning participants tgt=3c7 -1711462090.570598 [0] dq.builtin: reader 110cd0d:56a27910:57318171:3c7 init_acknack_count = 1 -1711462090.570601 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:3c2 rd 110cd0d:56a27910:57318171:3c7) -1711462090.570608 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:3c2 rd 110cd0d:56a27910:57318171:3c7) - out-of-sync -1711462090.570613 [0] dq.builtin: reader 110cd0d:e3b81b8d:1ccb65b1:3c7 init_acknack_count = 12 -1711462090.570617 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:3c2 rd 110cd0d:e3b81b8d:1ccb65b1:3c7) -1711462090.570622 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:3c2 rd 110cd0d:e3b81b8d:1ccb65b1:3c7) -1711462090.570627 [0] dq.builtin: reader 110cd0d:79d6eeac:ea4a8907:3c7 init_acknack_count = 6 -1711462090.570630 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:3c2 rd 110cd0d:79d6eeac:ea4a8907:3c7) -1711462090.570636 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:3c2 rd 110cd0d:79d6eeac:ea4a8907:3c7) -1711462090.570645 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:3c7) scanning participants tgt=3c2 -1711462090.570650 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:56a27910:57318171:3c2 prd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.570655 [0] dq.builtin: writer_add_connection(wr 110cd0d:56a27910:57318171:3c2 prd 110aba1:7b19badd:ce0adb73:3c7) - ack seq 9 -1711462090.570664 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.570669 [0] dq.builtin: reduced nlocs=5 -1711462090.570674 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570684 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.570689 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570693 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570696 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.570700 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570704 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570709 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.570712 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570715 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570719 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.570723 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570727 [0] dq.builtin: a b c d -1711462090.570732 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.570737 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.570741 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.570744 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.570749 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.570753 [0] dq.builtin: best = 4 -1711462090.570756 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.570762 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.570767 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:3c2 prd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.570771 [0] dq.builtin: writer_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:3c2 prd 110aba1:7b19badd:ce0adb73:3c7) - ack seq 10 -1711462090.570778 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.570782 [0] dq.builtin: reduced nlocs=5 -1711462090.570786 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570790 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.570794 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570798 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570801 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.570805 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570810 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570814 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.570819 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570823 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570827 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.570832 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570836 [0] dq.builtin: a b c d -1711462090.570841 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.570845 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.570849 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.570853 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.570857 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.570860 [0] dq.builtin: best = 4 -1711462090.570864 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.570873 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.570877 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:79d6eeac:ea4a8907:3c2 prd 110aba1:7b19badd:ce0adb73:3c7) -1711462090.570881 [0] dq.builtin: writer_add_connection(wr 110cd0d:79d6eeac:ea4a8907:3c2 prd 110aba1:7b19badd:ce0adb73:3c7) - ack seq 10 -1711462090.570889 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.570893 [0] dq.builtin: reduced nlocs=5 -1711462090.570897 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570901 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.570905 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570909 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570913 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.570917 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570920 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570925 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.570930 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570933 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.570937 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.570940 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.570945 [0] dq.builtin: a b c d -1711462090.570950 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.570954 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.570958 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.570961 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.570966 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.570969 [0] dq.builtin: best = 4 -1711462090.570974 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.570979 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.570987 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:4c2) scanning participants tgt=4c7 -1711462090.570992 [0] dq.builtin: reader 110cd0d:56a27910:57318171:4c7 init_acknack_count = 1 -1711462090.570996 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:4c2 rd 110cd0d:56a27910:57318171:4c7) -1711462090.571000 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:4c2 rd 110cd0d:56a27910:57318171:4c7) - out-of-sync -1711462090.571005 [0] dq.builtin: reader 110cd0d:e3b81b8d:1ccb65b1:4c7 init_acknack_count = 10 -1711462090.571009 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:4c2 rd 110cd0d:e3b81b8d:1ccb65b1:4c7) -1711462090.571013 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:4c2 rd 110cd0d:e3b81b8d:1ccb65b1:4c7) -1711462090.571017 [0] dq.builtin: reader 110cd0d:79d6eeac:ea4a8907:4c7 init_acknack_count = 5 -1711462090.571020 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:4c2 rd 110cd0d:79d6eeac:ea4a8907:4c7) -1711462090.571025 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:4c2 rd 110cd0d:79d6eeac:ea4a8907:4c7) -1711462090.571034 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:4c7) scanning participants tgt=4c2 -1711462090.571038 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:56a27910:57318171:4c2 prd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.571043 [0] dq.builtin: writer_add_connection(wr 110cd0d:56a27910:57318171:4c2 prd 110aba1:7b19badd:ce0adb73:4c7) - ack seq 10 -1711462090.571053 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571057 [0] dq.builtin: reduced nlocs=5 -1711462090.571062 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571067 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571072 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571075 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571080 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571083 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571088 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571092 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.571095 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571099 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571102 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571105 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571109 [0] dq.builtin: a b c d -1711462090.571113 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.571117 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.571121 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.571126 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.571130 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.571133 [0] dq.builtin: best = 4 -1711462090.571137 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571142 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571147 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:4c2 prd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.571151 [0] dq.builtin: writer_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:4c2 prd 110aba1:7b19badd:ce0adb73:4c7) - ack seq 10 -1711462090.571157 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571161 [0] dq.builtin: reduced nlocs=5 -1711462090.571165 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571170 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571173 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571177 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571181 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571185 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571189 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571193 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.571202 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571205 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571209 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571213 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571217 [0] dq.builtin: a b c d -1711462090.571221 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.571225 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.571229 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.571235 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.571239 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.571243 [0] dq.builtin: best = 4 -1711462090.571246 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571255 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571260 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:79d6eeac:ea4a8907:4c2 prd 110aba1:7b19badd:ce0adb73:4c7) -1711462090.571264 [0] dq.builtin: writer_add_connection(wr 110cd0d:79d6eeac:ea4a8907:4c2 prd 110aba1:7b19badd:ce0adb73:4c7) - ack seq 10 -1711462090.571270 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571274 [0] dq.builtin: reduced nlocs=5 -1711462090.571278 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571281 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571285 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571289 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571292 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571296 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571299 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571303 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.571306 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571310 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571313 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571318 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571323 [0] dq.builtin: a b c d -1711462090.571327 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.571330 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.571335 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.571340 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.571344 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.571347 [0] dq.builtin: best = 4 -1711462090.571351 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571355 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571364 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:200c2) scanning participants tgt=200c7 -1711462090.571368 [0] dq.builtin: reader 110cd0d:56a27910:57318171:200c7 init_acknack_count = 1 -1711462090.571372 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:200c2 rd 110cd0d:56a27910:57318171:200c7) -1711462090.571377 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:200c2 rd 110cd0d:56a27910:57318171:200c7) - out-of-sync -1711462090.571382 [0] dq.builtin: reader 110cd0d:e3b81b8d:1ccb65b1:200c7 init_acknack_count = 7 -1711462090.571386 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:200c2 rd 110cd0d:e3b81b8d:1ccb65b1:200c7) -1711462090.571390 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:200c2 rd 110cd0d:e3b81b8d:1ccb65b1:200c7) -1711462090.571394 [0] dq.builtin: reader 110cd0d:79d6eeac:ea4a8907:200c7 init_acknack_count = 5 -1711462090.571399 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:200c2 rd 110cd0d:79d6eeac:ea4a8907:200c7) -1711462090.571406 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:200c2 rd 110cd0d:79d6eeac:ea4a8907:200c7) -1711462090.571413 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:200c7) scanning participants tgt=200c2 -1711462090.571417 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:56a27910:57318171:200c2 prd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.571421 [0] dq.builtin: writer_add_connection(wr 110cd0d:56a27910:57318171:200c2 prd 110aba1:7b19badd:ce0adb73:200c7) - ack seq 1 -1711462090.571428 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571433 [0] dq.builtin: reduced nlocs=5 -1711462090.571437 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571441 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571446 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571449 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571453 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571456 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571460 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571463 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.571466 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571470 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571474 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571477 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571483 [0] dq.builtin: a b c d -1711462090.571487 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.571490 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.571494 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.571499 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.571503 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.571505 [0] dq.builtin: best = 4 -1711462090.571509 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571513 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571518 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:200c2 prd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.571522 [0] dq.builtin: writer_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:200c2 prd 110aba1:7b19badd:ce0adb73:200c7) - ack seq 3 -1711462090.571529 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571533 [0] dq.builtin: reduced nlocs=5 -1711462090.571537 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571541 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571544 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571548 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571551 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571555 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571558 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571562 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.571565 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571568 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571572 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571578 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571582 [0] dq.builtin: a b c d -1711462090.571587 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.571591 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.571594 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.571598 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.571602 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.571605 [0] dq.builtin: best = 4 -1711462090.571608 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571612 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571617 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:79d6eeac:ea4a8907:200c2 prd 110aba1:7b19badd:ce0adb73:200c7) -1711462090.571620 [0] dq.builtin: writer_add_connection(wr 110cd0d:79d6eeac:ea4a8907:200c2 prd 110aba1:7b19badd:ce0adb73:200c7) - ack seq 3 -1711462090.571627 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571631 [0] dq.builtin: reduced nlocs=5 -1711462090.571634 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571638 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571642 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571645 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571648 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571652 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571655 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571658 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.571663 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571667 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571671 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571674 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571679 [0] dq.builtin: a b c d -1711462090.571683 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.571686 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.571690 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.571694 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.571698 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.571701 [0] dq.builtin: best = 4 -1711462090.571704 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571709 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571718 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:300c3) scanning participants tgt=300c4 -1711462090.571722 [0] dq.builtin: reader 110cd0d:56a27910:57318171:300c4 init_acknack_count = 1 -1711462090.571725 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:300c3 rd 110cd0d:56a27910:57318171:300c4) -1711462090.571729 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:300c3 rd 110cd0d:56a27910:57318171:300c4) - out-of-sync -1711462090.571734 [0] dq.builtin: reader 110cd0d:e3b81b8d:1ccb65b1:300c4 init_acknack_count = 5 -1711462090.571739 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:300c3 rd 110cd0d:e3b81b8d:1ccb65b1:300c4) -1711462090.571744 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:300c3 rd 110cd0d:e3b81b8d:1ccb65b1:300c4) -1711462090.571748 [0] dq.builtin: reader 110cd0d:79d6eeac:ea4a8907:300c4 init_acknack_count = 3 -1711462090.571751 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:300c3 rd 110cd0d:79d6eeac:ea4a8907:300c4) -1711462090.571755 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:300c3 rd 110cd0d:79d6eeac:ea4a8907:300c4) -1711462090.571763 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:300c4) scanning participants tgt=300c3 -1711462090.571767 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:56a27910:57318171:300c3 prd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.571771 [0] dq.builtin: writer_add_connection(wr 110cd0d:56a27910:57318171:300c3 prd 110aba1:7b19badd:ce0adb73:300c4) - ack seq 0 -1711462090.571779 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571782 [0] dq.builtin: reduced nlocs=5 -1711462090.571787 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571790 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571793 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571797 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571801 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571804 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571808 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571811 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.571814 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571817 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571821 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571824 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571825 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:3c2) suppressed, resched in 0.1 s (min-ack 9!, avail-seq 9, xmit 9) -1711462090.571828 [0] dq.builtin: a b c d -1711462090.571841 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.571845 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.571848 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.571852 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.571856 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.571859 [0] dq.builtin: best = 4 -1711462090.571862 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571867 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571872 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:300c3 prd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.571876 [0] dq.builtin: writer_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:300c3 prd 110aba1:7b19badd:ce0adb73:300c4) - ack seq 0 -1711462090.571883 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571886 [0] dq.builtin: reduced nlocs=5 -1711462090.571890 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571893 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571896 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571899 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571906 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.571909 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571912 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571915 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.571919 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571922 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571926 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.571929 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.571932 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:3c2) suppressed, resched in 0.1 s (min-ack 10!, avail-seq 9, xmit 10) -1711462090.571932 [0] dq.builtin: a b c d -1711462090.571941 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.571945 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.571949 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.571952 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.571957 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.571959 [0] dq.builtin: best = 4 -1711462090.571963 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.571967 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.571972 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:79d6eeac:ea4a8907:300c3 prd 110aba1:7b19badd:ce0adb73:300c4) -1711462090.571976 [0] dq.builtin: writer_add_connection(wr 110cd0d:79d6eeac:ea4a8907:300c3 prd 110aba1:7b19badd:ce0adb73:300c4) - ack seq 0 -1711462090.571982 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.571986 [0] dq.builtin: reduced nlocs=5 -1711462090.571990 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.571994 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.571997 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572001 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572004 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.572008 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572014 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572008 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 204 from udp/10.101.12.71:44991 -1711462090.572018 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.572027 [0] recv: INFOTS(1711462090.571973111) -1711462090.572033 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572033 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #10 L(:1c1 16938.231530)) -1711462090.572042 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:3c2) suppressed, resched in 0.1 s (min-ack 10!, avail-seq 9, xmit 10) -1711462090.572038 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572055 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.572059 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572063 [0] dq.builtin: a b c d -1711462090.572066 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.572070 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.572075 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.572078 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 520 from udp/10.101.12.71:44991 -1711462090.572087 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.572094 [0] recvMC: INFOTS(1711462090.571985769) -1711462090.572099 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.572116 [0] dq.builtin: best = 4 -1711462090.572120 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.572106 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #10 L(:1c1 16938.231597) => EVERYONE -1711462090.572125 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.572138 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:301c3) scanning participants tgt=301c4 -1711462090.572142 [0] dq.builtin: reader 110cd0d:56a27910:57318171:301c4 init_acknack_count = 1 -1711462090.572146 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:301c3 rd 110cd0d:56a27910:57318171:301c4) -1711462090.572150 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:301c3 rd 110cd0d:56a27910:57318171:301c4) - out-of-sync -1711462090.572154 [0] dq.builtin: reader 110cd0d:e3b81b8d:1ccb65b1:301c4 init_acknack_count = 5 -1711462090.572157 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:301c3 rd 110cd0d:e3b81b8d:1ccb65b1:301c4) -1711462090.572160 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:301c3 rd 110cd0d:e3b81b8d:1ccb65b1:301c4) -1711462090.572165 [0] dq.builtin: reader 110cd0d:79d6eeac:ea4a8907:301c4 init_acknack_count = 3 -1711462090.572168 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:301c3 rd 110cd0d:79d6eeac:ea4a8907:301c4) -1711462090.572174 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:301c3 rd 110cd0d:79d6eeac:ea4a8907:301c4) -1711462090.572182 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:301c4) scanning participants tgt=301c3 -1711462090.572187 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:56a27910:57318171:301c3 prd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.572182 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #10: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,19,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,16,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,16) -1711462090.572192 [0] dq.builtin: writer_add_connection(wr 110cd0d:56a27910:57318171:301c3 prd 110aba1:7b19badd:ce0adb73:301c4) - ack seq 0 -1711462090.572200 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.572203 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:4c2) suppressed, resched in 0.1 s (min-ack 10!, avail-seq 10, xmit 10) -1711462090.572205 [0] dq.builtin: reduced nlocs=5 -1711462090.572201 [0] recvMC: HEARTBEAT(F#15:10..10 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@10(sync) 110cd0d:e3b81b8d:1ccb65b1:504@10(sync) 110cd0d:79d6eeac:ea4a8907:504@10(sync)) -1711462090.572216 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572234 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.572237 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572241 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572244 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.572247 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572251 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572254 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.572257 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572260 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572264 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.572268 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572272 [0] dq.builtin: a b c d -1711462090.572276 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.572280 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.572284 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.572288 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.572292 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.572294 [0] dq.builtin: best = 4 -1711462090.572297 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.572301 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.572306 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:301c3 prd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.572310 [0] dq.builtin: writer_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:301c3 prd 110aba1:7b19badd:ce0adb73:301c4) - ack seq 0 -1711462090.572314 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:4c2) suppressed, resched in 0.1 s (min-ack 10!, avail-seq 10, xmit 10) -1711462090.572316 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.572324 [0] dq.builtin: reduced nlocs=5 -1711462090.572328 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572331 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.572334 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572338 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572341 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.572344 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572347 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572350 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.572353 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572356 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572359 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.572362 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572365 [0] dq.builtin: a b c d -1711462090.572368 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.572372 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.572375 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.572381 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.572385 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.572388 [0] dq.builtin: best = 4 -1711462090.572391 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.572396 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.572400 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:79d6eeac:ea4a8907:301c3 prd 110aba1:7b19badd:ce0adb73:301c4) -1711462090.572403 [0] dq.builtin: writer_add_connection(wr 110cd0d:79d6eeac:ea4a8907:301c3 prd 110aba1:7b19badd:ce0adb73:301c4) - ack seq 0 -1711462090.572410 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 udp/10.101.12.71:7418@2 -1711462090.572413 [0] dq.builtin: reduced nlocs=5 -1711462090.572417 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572421 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462090.572425 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572428 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572433 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462090.572430 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 84 from udp/10.101.12.71:52025 -1711462090.572437 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572442 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572413 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:4c2) suppressed, resched in 0.1 s (min-ack 10!, avail-seq 10, xmit 10) -1711462090.572447 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7418@2 3 -> 0 -1711462090.572447 [0] recv: HEARTBEAT(#1:1..2 L(:1c1 16938.231947)110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0: end-of-tl-seq(rd 110cd0d:56a27910:57318171:3c7 #2) 110cd0d:56a27910:57318171:3c7@0 110cd0d:e3b81b8d:1ccb65b1:3c7@2(sync) 110cd0d:79d6eeac:ea4a8907:3c7@2(sync)) -1711462090.572458 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572465 [0] recv: HEARTBEAT(#1:1..1 110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0: end-of-tl-seq(rd 110cd0d:56a27910:57318171:4c7 #1) 110cd0d:56a27910:57318171:4c7@0 110cd0d:e3b81b8d:1ccb65b1:4c7@1(sync) 110cd0d:79d6eeac:ea4a8907:4c7@1(sync)) -1711462090.572470 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.572473 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462090.572455 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#1:1/2:11 -1711462090.572474 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462090.572490 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462090.572494 [0] recv: HEARTBEAT(#1:1..1 L(:1c1 16938.231996)110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0: end-of-tl-seq(rd 110cd0d:56a27910:57318171:200c7 #1) 110cd0d:56a27910:57318171:200c7@0 110cd0d:e3b81b8d:1ccb65b1:200c7@1(sync) 110cd0d:79d6eeac:ea4a8907:200c7@1(sync)) -1711462090.572501 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007e50 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.572508 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#12:3/0: -1711462090.572512 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462090.572516 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007fd0 0(control): niov 2 sz 68 => now niov 4 sz 120 -1711462090.572520 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#6:3/0: -1711462090.572522 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462090.572528 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007cd0 0(control): niov 4 sz 120 => now niov 6 sz 172 -1711462090.572478 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7400@2 4 -> 8 -1711462090.572536 [0] dq.builtin: a b c d -1711462090.572541 [0] dq.builtin: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. .. } -1711462090.572545 [0] dq.builtin: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. .. +u } -1711462090.572531 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#1:1/1:1 -1711462090.572551 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462090.572554 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0079d0 0(control): niov 6 sz 172 => now niov 8 sz 228 -1711462090.572555 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.572549 [0] dq.builtin: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. .. } -1711462090.572562 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.572557 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#10:2/0: -1711462090.572570 [0] recvUC: ACKNACK(#0:1/0: L(:1c1 16938.232065) 110443d:bb7f10a5:18901533:504 -> 110cd0d:56a27910:57318171:403 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:403 -> 110443d:bb7f10a5:18901533:504 - queue for transmit -1711462090.572572 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462090.572583 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007b50 0(control): niov 8 sz 228 => now niov 10 sz 280 -1711462090.572580 [0] dq.builtin: loc 3 = udp/10.101.12.71:7418@2 1 { .. .. +u .. } -1711462090.572579 [0] recvUC: non-timed queue now has 1 items -1711462090.572592 [0] dq.builtin: loc 4 = udp/239.255.0.1:7400@2 -1 { +1 +1 +1 +1 } -1711462090.572602 [0] dq.builtin: best = 4 -1711462090.572599 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 84 from udp/10.101.12.71:52025 -1711462090.572606 [0] dq.builtin: simple udp/239.255.0.1:7400@2 -1711462090.572603 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#5:2/0: -1711462090.572616 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462090.572598 [0] recvUC: ) -1711462090.572618 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007850 0(control): niov 10 sz 280 => now niov 12 sz 332 -1711462090.572613 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462090.572625 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#1:1/1:1 -1711462090.572638 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462090.572641 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0076d0 0(control): niov 12 sz 332 => now niov 14 sz 388 -1711462090.572638 [0] recv: HEARTBEAT(#1:1..0 L(:1c1 16938.232139)110aba1:7b19badd:ce0adb73:300c3 -> 0:0:0:0: end-of-tl-seq(rd 110cd0d:56a27910:57318171:300c4 #0) msr_in_sync(110cd0d:56a27910:57318171:300c4 out-of-sync to tlcatchup) 110cd0d:56a27910:57318171:300c4@0(sync) 110cd0d:e3b81b8d:1ccb65b1:300c4@0(sync) 110cd0d:79d6eeac:ea4a8907:300c4@0(sync)) -1711462090.572644 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#7:2/0: -1711462090.572650 [0] recv: HEARTBEAT(#1:1..0 110aba1:7b19badd:ce0adb73:301c3 -> 0:0:0:0: end-of-tl-seq(rd 110cd0d:56a27910:57318171:301c4 #0) msr_in_sync(110cd0d:56a27910:57318171:301c4 out-of-sync to tlcatchup) 110cd0d:56a27910:57318171:301c4@0(sync) 110cd0d:e3b81b8d:1ccb65b1:301c4@0(sync) 110cd0d:79d6eeac:ea4a8907:301c4@0(sync)) -1711462090.572650 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462090.572650 [0] dq.builtin: => EVERYONE -1711462090.572662 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007550 0(control): niov 14 sz 388 => now niov 16 sz 440 -1711462090.572671 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#5:2/0: -1711462090.572673 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462090.572674 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0073d0 0(control): niov 16 sz 440 => now niov 18 sz 492 -1711462090.572675 [0] dq.builtin: lease_register(l 0x7408c80256c0 guid 110aba1:7b19badd:ce0adb73:1c1) -1711462090.572678 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:200c2) suppressed, resched in 0.1 s (min-ack 1!, avail-seq 1, xmit 1) -1711462090.572683 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:200c2) suppressed, resched in 0.1 s (min-ack 3!, avail-seq 3, xmit 3) -1711462090.572686 [0] dq.builtin: broadcasted SPDP packet -> answering 0 0 0 -1711462090.572686 [0] tev: acknack 110cd0d:56a27910:57318171:300c4 -> 110aba1:7b19badd:ce0adb73:300c3: F#1:1/0: -1711462090.572689 [0] gc: gc 0x7408c80640a0: not yet, shortsleep -1711462090.572698 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:300c4 -> pwr 110aba1:7b19badd:ce0adb73:300c3) -1711462090.572700 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4009af0 0(control): niov 18 sz 492 => now niov 20 sz 544 -1711462090.572702 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:300c4 -> 110aba1:7b19badd:ce0adb73:300c3: F#5:1/0: -1711462090.572704 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:300c4 -> pwr 110aba1:7b19badd:ce0adb73:300c3) -1711462090.572705 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.572706 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4008bf0 0(control): niov 20 sz 544 => now niov 22 sz 596 -1711462090.572713 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:300c4 -> 110aba1:7b19badd:ce0adb73:300c3: F#3:1/0: -1711462090.572713 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (known) L(:1c1 16938.232216) -1711462090.572715 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:300c4 -> pwr 110aba1:7b19badd:ce0adb73:300c3) -1711462090.572720 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4008d70 0(control): niov 22 sz 596 => now niov 24 sz 648 -1711462090.572723 [0] tev: acknack 110cd0d:56a27910:57318171:301c4 -> 110aba1:7b19badd:ce0adb73:301c3: F#1:1/0: -1711462090.572724 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #10: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/tf_static",type_name="tf2_msgs::msg::dds_::TFMessage_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110443d:bb7f10a5:18901533:1403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.572725 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:301c4 -> pwr 110aba1:7b19badd:ce0adb73:301c3) -1711462090.572733 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4008ef0 0(control): niov 24 sz 648 => now niov 26 sz 700 -1711462090.572735 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:301c4 -> 110aba1:7b19badd:ce0adb73:301c3: F#5:1/0: -1711462090.572737 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:301c4 -> pwr 110aba1:7b19badd:ce0adb73:301c3) -1711462090.572741 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4008470 0(control): niov 26 sz 700 => now niov 28 sz 752 -1711462090.572744 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:301c4 -> 110aba1:7b19badd:ce0adb73:301c3: F#3:1/0: -1711462090.572747 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:301c4 -> pwr 110aba1:7b19badd:ce0adb73:301c3) -1711462090.572744 [0] dq.builtin: SEDP ST0 110443d:bb7f10a5:18901533:1403 reliable transient-local writer unnamed: (default).rt/tf_static/tf2_msgs::msg::dds_::TFMessage_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7417@2 ssm=0) QOS={user_data=0<>,topic_name="rt/tf_static",type_name="tf2_msgs::msg::dds_::TFMessage_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.572749 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40085f0 0(control): niov 28 sz 752 => now niov 30 sz 804 -1711462090.572754 [0] tev: non-timed queue now has 2 items -1711462090.572757 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462090.572760 [0] dq.builtin: => EVERYONE -1711462090.572762 [0] tev: non-timed queue now has 3 items -1711462090.572768 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462090.572770 [0] tev: non-timed queue now has 4 items -1711462090.572771 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110443d:bb7f10a5:18901533:1403) scanning all rds of topic rt/tf_static -1711462090.572772 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462090.572778 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:200c2) suppressed, resched in 0.1 s (min-ack 3!, avail-seq 3, xmit 3) -1711462090.572793 [0] tev: nn_xpack_send 804: 0x7408c000139c:20 0x7408bc007f38:48 0x7408bc0080a0:24 0x7408bc0080c8:28 0x7408bc007da0:24 0x7408bc007dc8:28 0x7408bc007aa0:24 0x7408bc007ac8:32 0x7408bc007c20:24 0x7408bc007c48:28 0x7408bc007920:24 0x7408bc007948:28 0x7408bc0077a0:24 0x7408bc0077c8:32 0x7408bc007620:24 0x7408bc007648:28 0x7408bc0074a0:24 0x7408bc0074c8:28 0x7408c4009bc0:24 0x7408c4009be8:28 0x7408c4008cc0:24 0x7408c4008ce8:28 0x7408c4008e40:24 0x7408c4008e68:28 0x7408c4008fc0:24 0x7408c4008fe8:28 0x7408c4008540:24 0x7408c4008568:28 0x7408c40086c0:24 0x7408c40086e8:28 [ udp/10.101.12.71:7418@2 ] -1711462090.572795 [0] tev: traffic-xmit (1) 804 -1711462090.572797 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc008330 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.572802 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc008418:48 [ udp/10.101.12.71:7417@2 ] -1711462090.572804 [0] tev: traffic-xmit (1) 68 -1711462090.572806 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4008770 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462090.572809 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4008a70 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462090.572811 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40079f0 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462090.572817 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408c4008858:52 0x7408ac009c94:388 0x7408c4008b40:24 0x7408c4008b68:36 0x7408d401eea4:388 0x7408c4007ac0:24 0x7408c4007ae8:36 0x7408e000a504:388 [ udp/10.101.12.71:7418@2 ] -1711462090.572818 [0] tev: traffic-xmit (1) 1356 -1711462090.572851 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 84 from udp/10.101.12.71:52025 -1711462090.572856 [0] recv: INFOTS(1711462090.570429633) -1711462090.572862 [0] recv: DATA(110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0 #1 L(:1c1 16938.232359) msr_in_sync(110cd0d:56a27910:57318171:200c7 out-of-sync to tlcatchup)) -1711462090.572868 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:200c2 #1: ST0 DCPSParticipantMessage/ParticipantMessageData:{110aba1:7b19badd:ce0adb73:1}:1<0> -1711462090.572873 [0] dq.builtin: PMD ST0 pp 110aba1:7b19badd:ce0adb73 kind 1 data 1 -1711462090.572881 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 68 from udp/10.101.12.71:44991 -1711462090.572889 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.572900 [0] recvUC: ACKNACK(F#1:12/1:1 L(:1c1 16938.232392) 110443d:bb7f10a5:18901533:504 -> 110cd0d:56a27910:57318171:403 complying RX12non-timed queue now has 1 items -1711462090.572906 [0] recvUC: rexmit#1 maxseq:12<12<=12defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:403 -> 110443d:bb7f10a5:18901533:504 - queue for transmit -1711462090.572909 [0] recvUC: ) -1711462090.572915 [0] recvUC: send_deferred_heartbeat: 790fc2d10bf8844f -> 7bd5090527eafdf7 - queue for transmit -1711462090.572918 [0] recvUC: non-timed queue now has 2 items -1711462090.572912 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:300c3) suppressed, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.572930 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0084b0 0(rexmit(110cd0d:56a27910:57318171:403:#12/1)): niov 0 sz 0 => now niov 3 sz 544 -1711462090.572937 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc008630 0(control): niov 3 sz 544 => now niov 4 sz 576 -1711462090.572953 [0] tev: nn_xpack_send 576: 0x7408c000139c:20 0x7408bc008598:52 0x7408ac01b0d0:472 0x7408bc008728:32 [ udp/10.101.12.71:7417@2 ] -1711462090.572957 [0] tev: traffic-xmit (1) 576 -1711462090.573026 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:300c3) suppressed, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.573073 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.573078 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.573084 [0] recvUC: ACKNACK(F#2:13/0: L(:1c1 16938.232581) 110443d:bb7f10a5:18901533:504 -> 110cd0d:56a27910:57318171:403 setting-has-replied-to-hb happy-now) -1711462090.573187 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:300c3) suppressed, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.573359 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:301c3) suppressed, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.573453 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:301c3) suppressed, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.573633 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.573643 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.573649 [0] recvUC: ACKNACK(#0:1/0: L(:1c1 16938.233146) 110443d:bb7f10a5:18901533:504 -> 110cd0d:e3b81b8d:1ccb65b1:403 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:403 -> 110443d:bb7f10a5:18901533:504 - queue for transmit -1711462090.573656 [0] recvUC: non-timed queue now has 1 items -1711462090.573659 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 612 from udp/10.101.12.71:40473 -1711462090.573664 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:301c3) suppressed, resched in 0.1 s (min-ack 0!, avail-seq 0, xmit 0) -1711462090.573665 [0] recv: INFOTS(1711462090.573532356) -1711462090.573670 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc008800 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.573672 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 536 from udp/10.101.12.71:40473 -1711462090.573678 [0] recvMC: INFOTS(1711462090.573586880) -1711462090.573682 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc0088e8:48 [ udp/10.101.12.71:7417@2 ] -1711462090.573686 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #10 L(:1c1 16938.233181) => EVERYONE -1711462090.573688 [0] tev: traffic-xmit (1) 68 -1711462090.573661 [0] recvUC: ) -1711462090.573681 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #10 L(:1c1 16938.233169)) -1711462090.573715 [0] recv: INFOTS(1711462090.573575071) -1711462090.573722 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #8) -1711462090.573738 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #10: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1) -1711462090.573739 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #10: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.7;">,topic_name="rr/topological_map_manager2/get_topological_mapReply",type_name="std_srvs::srv::dds_::Trigger_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.573748 [0] recvMC: HEARTBEAT(F#16:10..10 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@10(sync) 110cd0d:e3b81b8d:1ccb65b1:504@10(sync) 110cd0d:79d6eeac:ea4a8907:504@10(sync)) -1711462090.573754 [0] gc: gc 0x7408c80640a0: deleting -1711462090.573761 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 68 from udp/10.101.12.71:44991 -1711462090.573771 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_topological_mapReply/std_srvs::srv::dds_::Trigger_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.7;">,topic_name="rr/topological_map_manager2/get_topological_mapReply",type_name="std_srvs::srv::dds_::Trigger_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.573775 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.573785 [0] dq.builtin: => EVERYONE -1711462090.573790 [0] recvUC: ACKNACK(F#1:13/1:1 L(:1c1 16938.233278) 110443d:bb7f10a5:18901533:504 -> 110cd0d:e3b81b8d:1ccb65b1:403 complying RX13non-timed queue now has 1 items -1711462090.573791 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1403) scanning all rds of topic rr/topological_map_manager2/get_topological_mapReply -1711462090.573796 [0] recvUC: rexmit#1 maxseq:13<13<=13defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:403 -> 110443d:bb7f10a5:18901533:504 - queue for transmit -1711462090.573804 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.7;">,topic_name="rq/topological_map_manager2/get_topological_mapRequest",type_name="std_srvs::srv::dds_::Trigger_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1504},adlink_entity_factory=1} -1711462090.573800 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc008980 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:403:#13/1)): niov 0 sz 0 => now niov 3 sz 520 -1711462090.573804 [0] recvUC: ) -1711462090.573817 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_topological_mapRequest/std_srvs::srv::dds_::Trigger_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.7;">,topic_name="rq/topological_map_manager2/get_topological_mapRequest",type_name="std_srvs::srv::dds_::Trigger_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.573820 [0] recvUC: send_deferred_heartbeat: 18ed9dbea84bb67e -> 7bd5090527eafdf7 - queue for transmit -1711462090.573826 [0] dq.builtin: => EVERYONE -1711462090.573827 [0] recvUC: non-timed queue now has 1 items -1711462090.573830 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1504) scanning all wrs of topic rq/topological_map_manager2/get_topological_mapRequest -1711462090.573825 [0] tev: nn_xpack_send 520: 0x7408c000139c:20 0x7408bc008a68:52 0x7408d406c3b0:448 [ udp/10.101.12.71:7417@2 ] -1711462090.573837 [0] tev: traffic-xmit (1) 520 -1711462090.573841 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc008b00 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.573850 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc008be8:48 [ udp/10.101.12.71:7417@2 ] -1711462090.573853 [0] tev: traffic-xmit (1) 68 -1711462090.573925 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.573929 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.573935 [0] recvUC: ACKNACK(F#2:14/0: L(:1c1 16938.233433) 110443d:bb7f10a5:18901533:504 -> 110cd0d:e3b81b8d:1ccb65b1:403 setting-has-replied-to-hb happy-now) -1711462090.574490 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.574494 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.574499 [0] recvUC: ACKNACK(#0:1/0: L(:1c1 16938.233998) 110443d:bb7f10a5:18901533:504 -> 110cd0d:79d6eeac:ea4a8907:403 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:403 -> 110443d:bb7f10a5:18901533:504 - queue for transmit -1711462090.574503 [0] recvUC: non-timed queue now has 1 items -1711462090.574507 [0] recvUC: ) -1711462090.574509 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc008c80 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.574518 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc008d68:48 [ udp/10.101.12.71:7417@2 ] -1711462090.574521 [0] tev: traffic-xmit (1) 68 -1711462090.574550 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 68 from udp/10.101.12.71:44991 -1711462090.574554 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.574560 [0] recvUC: ACKNACK(F#1:13/1:1 L(:1c1 16938.234058) 110443d:bb7f10a5:18901533:504 -> 110cd0d:79d6eeac:ea4a8907:403 complying RX13non-timed queue now has 1 items -1711462090.574564 [0] recvUC: rexmit#1 maxseq:13<13<=13defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:403 -> 110443d:bb7f10a5:18901533:504 - queue for transmit -1711462090.574566 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc008e00 0(rexmit(110cd0d:79d6eeac:ea4a8907:403:#13/1)): niov 0 sz 0 => now niov 3 sz 520 -1711462090.574573 [0] recvUC: ) -1711462090.574579 [0] recvUC: send_deferred_heartbeat: abaf4210bc473a65 -> 7bd5090527eafdf7 - queue for transmit -1711462090.574583 [0] recvUC: non-timed queue now has 1 items -1711462090.574583 [0] tev: nn_xpack_send 520: 0x7408c000139c:20 0x7408bc008ee8:52 0x7408e003d100:448 [ udp/10.101.12.71:7417@2 ] -1711462090.574591 [0] tev: traffic-xmit (1) 520 -1711462090.574595 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc008f80 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.574603 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc009068:48 [ udp/10.101.12.71:7417@2 ] -1711462090.574606 [0] tev: traffic-xmit (1) 68 -1711462090.574684 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462090.574688 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.574692 [0] recvUC: ACKNACK(F#2:14/0: L(:1c1 16938.234192) 110443d:bb7f10a5:18901533:504 -> 110cd0d:79d6eeac:ea4a8907:403 setting-has-replied-to-hb happy-now) -1711462090.575578 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 656 from udp/10.101.12.71:40473 -1711462090.575603 [0] recv: INFOTS(1711462090.575442461) -1711462090.575610 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #11 L(:1c1 16938.235107)) -1711462090.575613 [0] recv: INFOTS(1711462090.575483556) -1711462090.575617 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #9) -1711462090.575602 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 584 from udp/10.101.12.71:40473 -1711462090.575626 [0] recvMC: INFOTS(1711462090.575495060) -1711462090.575632 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #11 L(:1c1 16938.235130) => EVERYONE -1711462090.575634 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #11: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.8;">,topic_name="rr/topological_map_manager2/get_tagged_nodesReply",type_name="topological_navigation_msgs::srv::dds_::GetTaggedNodes_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.575657 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1603 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_tagged_nodesReply/topological_navigation_msgs::srv::dds_::GetTaggedNodes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.8;">,topic_name="rr/topological_map_manager2/get_tagged_nodesReply",type_name="topological_navigation_msgs::srv::dds_::GetTaggedNodes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.575668 [0] dq.builtin: => EVERYONE -1711462090.575675 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #11: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1) -1711462090.575677 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1603) scanning all rds of topic rr/topological_map_manager2/get_tagged_nodesReply -1711462090.575686 [0] recvMC: HEARTBEAT(F#17:11..11 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@11(sync) 110cd0d:e3b81b8d:1ccb65b1:504@11(sync) 110cd0d:79d6eeac:ea4a8907:504@11(sync)) -1711462090.575699 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #9: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.8;">,topic_name="rq/topological_map_manager2/get_tagged_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::GetTaggedNodes_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1704},adlink_entity_factory=1} -1711462090.575716 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1704 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_tagged_nodesRequest/topological_navigation_msgs::srv::dds_::GetTaggedNodes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.8;">,topic_name="rq/topological_map_manager2/get_tagged_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::GetTaggedNodes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.575726 [0] dq.builtin: => EVERYONE -1711462090.575734 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1704) scanning all wrs of topic rq/topological_map_manager2/get_tagged_nodesRequest -1711462090.575764 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 328 from udp/10.101.12.71:40473 -1711462090.575767 [0] recv: INFOTS(1711462090.575726889) -1711462090.575790 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #12 L(:1c1 16938.235271)) -1711462090.575801 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #12: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.9;">,topic_name="rr/topological_map_manager2/get_tagsReply",type_name="topological_navigation_msgs::srv::dds_::GetTags_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.575813 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 320 from udp/10.101.12.71:40473 -1711462090.575816 [0] recv: INFOTS(1711462090.575762290) -1711462090.575821 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #10 L(:1c1 16938.235320)) -1711462090.575817 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_tagsReply/topological_navigation_msgs::srv::dds_::GetTags_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.9;">,topic_name="rr/topological_map_manager2/get_tagsReply",type_name="topological_navigation_msgs::srv::dds_::GetTags_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.575840 [0] dq.builtin: => EVERYONE -1711462090.575848 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1803) scanning all rds of topic rr/topological_map_manager2/get_tagsReply -1711462090.575858 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #10: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.9;">,topic_name="rq/topological_map_manager2/get_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::GetTags_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1904},adlink_entity_factory=1} -1711462090.575862 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 632 from udp/10.101.12.71:40473 -1711462090.575866 [0] recvMC: INFOTS(1711462090.575799187) -1711462090.575874 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #12 L(:1c1 16938.235370) => EVERYONE -1711462090.575874 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_tagsRequest/topological_navigation_msgs::srv::dds_::GetTags_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.9;">,topic_name="rq/topological_map_manager2/get_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::GetTags_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.575888 [0] dq.builtin: => EVERYONE -1711462090.575895 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1904) scanning all wrs of topic rq/topological_map_manager2/get_tagsRequest -1711462090.575914 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1) -1711462090.575923 [0] recvMC: HEARTBEAT(F#18:12..12 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@12(sync) 110cd0d:e3b81b8d:1ccb65b1:504@12(sync) 110cd0d:79d6eeac:ea4a8907:504@12(sync)) -1711462090.576028 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.576031 [0] recv: INFOTS(1711462090.575995325) -1711462090.576034 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #13 L(:1c1 16938.235535)) -1711462090.576045 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #13: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.a;">,topic_name="rr/topological_map_manager2/get_node_tagsReply",type_name="topological_navigation_msgs::srv::dds_::GetNodeTags_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.576061 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1a03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_node_tagsReply/topological_navigation_msgs::srv::dds_::GetNodeTags_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.a;">,topic_name="rr/topological_map_manager2/get_node_tagsReply",type_name="topological_navigation_msgs::srv::dds_::GetNodeTags_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.576069 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.576072 [0] recv: INFOTS(1711462090.576028935) -1711462090.576069 [0] dq.builtin: => EVERYONE -1711462090.576077 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #11 L(:1c1 16938.235576)) -1711462090.576088 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1a03) scanning all rds of topic rr/topological_map_manager2/get_node_tagsReply -1711462090.576098 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #11: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.a;">,topic_name="rq/topological_map_manager2/get_node_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::GetNodeTags_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1b04},adlink_entity_factory=1} -1711462090.576113 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_node_tagsRequest/topological_navigation_msgs::srv::dds_::GetNodeTags_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.a;">,topic_name="rq/topological_map_manager2/get_node_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::GetNodeTags_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.576123 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 680 from udp/10.101.12.71:40473 -1711462090.576128 [0] recvMC: INFOTS(1711462090.576052639) -1711462090.576125 [0] dq.builtin: => EVERYONE -1711462090.576135 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #13 L(:1c1 16938.235632) => EVERYONE -1711462090.576185 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462090.576190 [0] recvMC: HEARTBEAT(F#19:13..13 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@13(sync) 110cd0d:e3b81b8d:1ccb65b1:504@13(sync) 110cd0d:79d6eeac:ea4a8907:504@13(sync)) -1711462090.576200 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1b04) scanning all wrs of topic rq/topological_map_manager2/get_node_tagsRequest -1711462090.576301 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.576304 [0] recv: INFOTS(1711462090.576266665) -1711462090.576309 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #14 L(:1c1 16938.235808)) -1711462090.576320 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #14: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.b;">,topic_name="rr/topological_map_manager2/get_edges_between_nodesReply",type_name="topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.576336 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/get_edges_between_nodesReply/topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.b;">,topic_name="rr/topological_map_manager2/get_edges_between_nodesReply",type_name="topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.576337 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.576344 [0] recv: INFOTS(1711462090.576303013) -1711462090.576345 [0] dq.builtin: => EVERYONE -1711462090.576347 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #12 L(:1c1 16938.235848)) -1711462090.576358 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1c03) scanning all rds of topic rr/topological_map_manager2/get_edges_between_nodesReply -1711462090.576368 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #12: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.b;">,topic_name="rq/topological_map_manager2/get_edges_between_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1d04},adlink_entity_factory=1} -1711462090.576382 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/get_edges_between_nodesRequest/topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.b;">,topic_name="rq/topological_map_manager2/get_edges_between_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::GetEdgesBetweenNodes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.576390 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 728 from udp/10.101.12.71:40473 -1711462090.576390 [0] dq.builtin: => EVERYONE -1711462090.576396 [0] recvMC: INFOTS(1711462090.576322325) -1711462090.576403 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #14 L(:1c1 16938.235899) => EVERYONE -1711462090.576407 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1d04) scanning all wrs of topic rq/topological_map_manager2/get_edges_between_nodesRequest -1711462090.576444 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0) -1711462090.576450 [0] recvMC: HEARTBEAT(F#20:14..14 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@14(sync) 110cd0d:e3b81b8d:1ccb65b1:504@14(sync) 110cd0d:79d6eeac:ea4a8907:504@14(sync)) -1711462090.576567 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.576570 [0] recv: INFOTS(1711462090.576532468) -1711462090.576576 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #15 L(:1c1 16938.236074)) -1711462090.576589 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #15: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.c;">,topic_name="rr/topological_map_manager2/write_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.576603 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1e03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/write_topological_mapReply/topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.c;">,topic_name="rr/topological_map_manager2/write_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.576607 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.576611 [0] recv: INFOTS(1711462090.576567173) -1711462090.576617 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #13 L(:1c1 16938.236115)) -1711462090.576611 [0] dq.builtin: => EVERYONE -1711462090.576631 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:1e03) scanning all rds of topic rr/topological_map_manager2/write_topological_mapReply -1711462090.576641 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #13: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.c;">,topic_name="rq/topological_map_manager2/write_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1f04},adlink_entity_factory=1} -1711462090.576653 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 776 from udp/10.101.12.71:40473 -1711462090.576659 [0] recvMC: INFOTS(1711462090.576579736) -1711462090.576655 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:1f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/write_topological_mapRequest/topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.c;">,topic_name="rq/topological_map_manager2/write_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.576666 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #15 L(:1c1 16938.236163) => EVERYONE -1711462090.576676 [0] dq.builtin: => EVERYONE -1711462090.576686 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:1f04) scanning all wrs of topic rq/topological_map_manager2/write_topological_mapRequest -1711462090.576716 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462090.576722 [0] recvMC: HEARTBEAT(F#21:15..15 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@15(sync) 110cd0d:e3b81b8d:1ccb65b1:504@15(sync) 110cd0d:79d6eeac:ea4a8907:504@15(sync)) -1711462090.576772 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.576776 [0] recv: INFOTS(1711462090.576742682) -1711462090.576779 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #16 L(:1c1 16938.236279)) -1711462090.576790 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #16: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.d;">,topic_name="rr/topological_map_manager2/switch_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.576806 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2003 reliable volatile writer unnamed: (default).rr/topological_map_manager2/switch_topological_mapReply/topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.d;">,topic_name="rr/topological_map_manager2/switch_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.576814 [0] dq.builtin: => EVERYONE -1711462090.576822 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2003) scanning all rds of topic rr/topological_map_manager2/switch_topological_mapReply -1711462090.576830 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.576838 [0] recv: INFOTS(1711462090.576793854) -1711462090.576845 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #14 L(:1c1 16938.236341)) -1711462090.576857 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #14: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.d;">,topic_name="rq/topological_map_manager2/switch_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2104},adlink_entity_factory=1} -1711462090.576877 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/switch_topological_mapRequest/topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.d;">,topic_name="rq/topological_map_manager2/switch_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::WriteTopologicalMap_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.576877 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 824 from udp/10.101.12.71:40473 -1711462090.576883 [0] dq.builtin: => EVERYONE -1711462090.576897 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2104) scanning all wrs of topic rq/topological_map_manager2/switch_topological_mapRequest -1711462090.576889 [0] recvMC: INFOTS(1711462090.576805226) -1711462090.576912 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #16 L(:1c1 16938.236393) => EVERYONE -1711462090.576956 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.576962 [0] recvMC: HEARTBEAT(F#22:16..16 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@16(sync) 110cd0d:e3b81b8d:1ccb65b1:504@16(sync) 110cd0d:79d6eeac:ea4a8907:504@16(sync)) -1711462090.577430 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 652 from udp/10.101.12.71:40473 -1711462090.577437 [0] recv: INFOTS(1711462090.577330990) -1711462090.577440 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 872 from udp/10.101.12.71:40473 -1711462090.577443 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #17 L(:1c1 16938.236940)) -1711462090.577448 [0] recv: INFOTS(1711462090.577371768) -1711462090.577454 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #15) -1711462090.577445 [0] recvMC: INFOTS(1711462090.577381731) -1711462090.577471 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #17 L(:1c1 16938.236948) => EVERYONE -1711462090.577475 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #17: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.e;">,topic_name="rr/topological_map_manager2/add_topological_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddNode_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.577502 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2203 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_topological_nodeReply/topological_navigation_msgs::srv::dds_::AddNode_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.e;">,topic_name="rr/topological_map_manager2/add_topological_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddNode_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.577512 [0] dq.builtin: => EVERYONE -1711462090.577521 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2203) scanning all rds of topic rr/topological_map_manager2/add_topological_nodeReply -1711462090.577523 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.577531 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #15: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.e;">,topic_name="rq/topological_map_manager2/add_topological_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddNode_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2304},adlink_entity_factory=1} -1711462090.577531 [0] recvMC: HEARTBEAT(F#23:17..17 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@17(sync) 110cd0d:e3b81b8d:1ccb65b1:504@17(sync) 110cd0d:79d6eeac:ea4a8907:504@17(sync)) -1711462090.577547 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_topological_nodeRequest/topological_navigation_msgs::srv::dds_::AddNode_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.e;">,topic_name="rq/topological_map_manager2/add_topological_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddNode_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.577560 [0] dq.builtin: => EVERYONE -1711462090.577568 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2304) scanning all wrs of topic rq/topological_map_manager2/add_topological_nodeRequest -1711462090.577629 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.577637 [0] recv: INFOTS(1711462090.577582984) -1711462090.577643 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #18 L(:1c1 16938.237138)) -1711462090.577654 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #18: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.f;">,topic_name="rr/topological_map_manager2/remove_topological_nodeReply",type_name="topological_navigation_msgs::srv::dds_::RmvNode_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.577668 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.577673 [0] recv: INFOTS(1711462090.577626247) -1711462090.577670 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/remove_topological_nodeReply/topological_navigation_msgs::srv::dds_::RmvNode_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.f;">,topic_name="rr/topological_map_manager2/remove_topological_nodeReply",type_name="topological_navigation_msgs::srv::dds_::RmvNode_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.577678 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #16 L(:1c1 16938.237176)) -1711462090.577688 [0] dq.builtin: => EVERYONE -1711462090.577696 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2403) scanning all rds of topic rr/topological_map_manager2/remove_topological_nodeReply -1711462090.577705 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #16: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.f;">,topic_name="rq/topological_map_manager2/remove_topological_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::RmvNode_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2504},adlink_entity_factory=1} -1711462090.577716 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 920 from udp/10.101.12.71:40473 -1711462090.577724 [0] recvMC: INFOTS(1711462090.577649910) -1711462090.577720 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/remove_topological_nodeRequest/topological_navigation_msgs::srv::dds_::RmvNode_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.f;">,topic_name="rq/topological_map_manager2/remove_topological_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::RmvNode_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.577731 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #18 L(:1c1 16938.237228) => EVERYONE -1711462090.577742 [0] dq.builtin: => EVERYONE -1711462090.577750 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2504) scanning all wrs of topic rq/topological_map_manager2/remove_topological_nodeRequest -1711462090.577784 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.577791 [0] recvMC: HEARTBEAT(F#24:18..18 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@18(sync) 110cd0d:e3b81b8d:1ccb65b1:504@18(sync) 110cd0d:79d6eeac:ea4a8907:504@18(sync)) -1711462090.577883 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.577885 [0] recv: INFOTS(1711462090.577848971) -1711462090.577888 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #19 L(:1c1 16938.237389)) -1711462090.577898 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #19: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.10;">,topic_name="rr/topological_map_manager2/add_edges_between_nodesReply",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.577913 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2603 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_edges_between_nodesReply/topological_navigation_msgs::srv::dds_::AddEdge_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.10;">,topic_name="rr/topological_map_manager2/add_edges_between_nodesReply",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.577923 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.577927 [0] recv: INFOTS(1711462090.577884615) -1711462090.577932 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #17 L(:1c1 16938.237431)) -1711462090.577924 [0] dq.builtin: => EVERYONE -1711462090.577946 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2603) scanning all rds of topic rr/topological_map_manager2/add_edges_between_nodesReply -1711462090.577955 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #17: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.10;">,topic_name="rq/topological_map_manager2/add_edges_between_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2704},adlink_entity_factory=1} -1711462090.577961 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 968 from udp/10.101.12.71:40473 -1711462090.577967 [0] recvMC: INFOTS(1711462090.577896758) -1711462090.577970 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2704 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_edges_between_nodesRequest/topological_navigation_msgs::srv::dds_::AddEdge_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.10;">,topic_name="rq/topological_map_manager2/add_edges_between_nodesRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.577973 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #19 L(:1c1 16938.237471) => EVERYONE -1711462090.577980 [0] dq.builtin: => EVERYONE -1711462090.577993 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2704) scanning all wrs of topic rq/topological_map_manager2/add_edges_between_nodesRequest -1711462090.578025 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.578032 [0] recvMC: HEARTBEAT(F#25:19..19 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@19(sync) 110cd0d:e3b81b8d:1ccb65b1:504@19(sync) 110cd0d:79d6eeac:ea4a8907:504@19(sync)) -1711462090.578071 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.578073 [0] recv: INFOTS(1711462090.578042902) -1711462090.578076 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #20 L(:1c1 16938.237578)) -1711462090.578086 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #20: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.11;">,topic_name="rr/topological_map_manager2/remove_edgeReply",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.578101 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/remove_edgeReply/topological_navigation_msgs::srv::dds_::AddEdge_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.11;">,topic_name="rr/topological_map_manager2/remove_edgeReply",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.578109 [0] dq.builtin: => EVERYONE -1711462090.578113 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.578116 [0] recv: INFOTS(1711462090.578086523) -1711462090.578120 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #18 L(:1c1 16938.237620)) -1711462090.578121 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2803) scanning all rds of topic rr/topological_map_manager2/remove_edgeReply -1711462090.578135 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #18: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.11;">,topic_name="rq/topological_map_manager2/remove_edgeRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2904},adlink_entity_factory=1} -1711462090.578150 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/remove_edgeRequest/topological_navigation_msgs::srv::dds_::AddEdge_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.11;">,topic_name="rq/topological_map_manager2/remove_edgeRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdge_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.578156 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1016 from udp/10.101.12.71:40473 -1711462090.578163 [0] recvMC: INFOTS(1711462090.578098378) -1711462090.578158 [0] dq.builtin: => EVERYONE -1711462090.578169 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #20 L(:1c1 16938.237666) => EVERYONE -1711462090.578177 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2904) scanning all wrs of topic rq/topological_map_manager2/remove_edgeRequest -1711462090.578234 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.578241 [0] recvMC: HEARTBEAT(F#26:20..20 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@20(sync) 110cd0d:e3b81b8d:1ccb65b1:504@20(sync) 110cd0d:79d6eeac:ea4a8907:504@20(sync)) -1711462090.578319 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.578321 [0] recv: INFOTS(1711462090.578286739) -1711462090.578325 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #21 L(:1c1 16938.237825)) -1711462090.578336 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #21: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.12;">,topic_name="rr/topological_map_manager2/add_content_to_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddContent_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.578351 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2a03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_content_to_nodeReply/topological_navigation_msgs::srv::dds_::AddContent_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.12;">,topic_name="rr/topological_map_manager2/add_content_to_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddContent_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.578359 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.578362 [0] recv: INFOTS(1711462090.578323981) -1711462090.578359 [0] dq.builtin: => EVERYONE -1711462090.578364 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #19 L(:1c1 16938.237866)) -1711462090.578379 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2a03) scanning all rds of topic rr/topological_map_manager2/add_content_to_nodeReply -1711462090.578388 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #19: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.12;">,topic_name="rq/topological_map_manager2/add_content_to_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddContent_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2b04},adlink_entity_factory=1} -1711462090.578406 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_content_to_nodeRequest/topological_navigation_msgs::srv::dds_::AddContent_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.12;">,topic_name="rq/topological_map_manager2/add_content_to_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddContent_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.578412 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1064 from udp/10.101.12.71:40473 -1711462090.578419 [0] recvMC: INFOTS(1711462090.578342953) -1711462090.578414 [0] dq.builtin: => EVERYONE -1711462090.578426 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #21 L(:1c1 16938.237923) => EVERYONE -1711462090.578434 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2b04) scanning all wrs of topic rq/topological_map_manager2/add_content_to_nodeRequest -1711462090.578484 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #21: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.578492 [0] recvMC: HEARTBEAT(F#27:21..21 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@21(sync) 110cd0d:e3b81b8d:1ccb65b1:504@21(sync) 110cd0d:79d6eeac:ea4a8907:504@21(sync)) -1711462090.578561 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.578563 [0] recv: INFOTS(1711462090.578531397) -1711462090.578565 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #22 L(:1c1 16938.238067)) -1711462090.578575 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #22: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.13;">,topic_name="rr/topological_map_manager2/update_node_nameReply",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeName_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.578595 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2c03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_node_nameReply/topological_navigation_msgs::srv::dds_::UpdateNodeName_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.13;">,topic_name="rr/topological_map_manager2/update_node_nameReply",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeName_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.578603 [0] dq.builtin: => EVERYONE -1711462090.578606 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.578608 [0] recv: INFOTS(1711462090.578575730) -1711462090.578612 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #20 L(:1c1 16938.238112)) -1711462090.578612 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2c03) scanning all rds of topic rr/topological_map_manager2/update_node_nameReply -1711462090.578629 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #20: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.13;">,topic_name="rq/topological_map_manager2/update_node_nameRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeName_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2d04},adlink_entity_factory=1} -1711462090.578644 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_node_nameRequest/topological_navigation_msgs::srv::dds_::UpdateNodeName_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.13;">,topic_name="rq/topological_map_manager2/update_node_nameRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeName_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.578653 [0] dq.builtin: => EVERYONE -1711462090.578661 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2d04) scanning all wrs of topic rq/topological_map_manager2/update_node_nameRequest -1711462090.578661 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1112 from udp/10.101.12.71:40473 -1711462090.578672 [0] recvMC: INFOTS(1711462090.578589065) -1711462090.578677 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #22 L(:1c1 16938.238175) => EVERYONE -1711462090.578723 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #22: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.578733 [0] recvMC: HEARTBEAT(F#28:22..22 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@22(sync) 110cd0d:e3b81b8d:1ccb65b1:504@22(sync) 110cd0d:79d6eeac:ea4a8907:504@22(sync)) -1711462090.578778 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.578780 [0] recv: INFOTS(1711462090.578749181) -1711462090.578782 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #23 L(:1c1 16938.238284)) -1711462090.578793 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #23: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.14;">,topic_name="rr/topological_map_manager2/update_node_poseReply",type_name="topological_navigation_msgs::srv::dds_::AddNode_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.578809 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2e03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_node_poseReply/topological_navigation_msgs::srv::dds_::AddNode_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.14;">,topic_name="rr/topological_map_manager2/update_node_poseReply",type_name="topological_navigation_msgs::srv::dds_::AddNode_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.578816 [0] dq.builtin: => EVERYONE -1711462090.578818 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 328 from udp/10.101.12.71:40473 -1711462090.578823 [0] recv: INFOTS(1711462090.578795007) -1711462090.578829 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #21 L(:1c1 16938.238327)) -1711462090.578825 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:2e03) scanning all rds of topic rr/topological_map_manager2/update_node_poseReply -1711462090.578842 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #21: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.14;">,topic_name="rq/topological_map_manager2/update_node_poseRequest",type_name="topological_navigation_msgs::srv::dds_::AddNode_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2f04},adlink_entity_factory=1} -1711462090.578856 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:2f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_node_poseRequest/topological_navigation_msgs::srv::dds_::AddNode_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.14;">,topic_name="rq/topological_map_manager2/update_node_poseRequest",type_name="topological_navigation_msgs::srv::dds_::AddNode_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.578865 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1160 from udp/10.101.12.71:40473 -1711462090.578873 [0] recvMC: INFOTS(1711462090.578803795) -1711462090.578866 [0] dq.builtin: => EVERYONE -1711462090.578880 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #23 L(:1c1 16938.238376) => EVERYONE -1711462090.578888 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:2f04) scanning all wrs of topic rq/topological_map_manager2/update_node_poseRequest -1711462090.578932 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #23: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.578938 [0] recvMC: HEARTBEAT(F#29:23..23 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@23(sync) 110cd0d:e3b81b8d:1ccb65b1:504@23(sync) 110cd0d:79d6eeac:ea4a8907:504@23(sync)) -1711462090.579027 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.579029 [0] recv: INFOTS(1711462090.578997155) -1711462090.579031 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #24 L(:1c1 16938.238533)) -1711462090.579041 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #24: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.15;">,topic_name="rr/topological_map_manager2/update_node_toleranceReply",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.579057 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3003 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_node_toleranceReply/topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.15;">,topic_name="rr/topological_map_manager2/update_node_toleranceReply",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.579068 [0] dq.builtin: => EVERYONE -1711462090.579076 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3003) scanning all rds of topic rr/topological_map_manager2/update_node_toleranceReply -1711462090.579076 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.579083 [0] recv: INFOTS(1711462090.579038346) -1711462090.579087 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #22 L(:1c1 16938.238587)) -1711462090.579098 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #22: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.15;">,topic_name="rq/topological_map_manager2/update_node_toleranceRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3104},adlink_entity_factory=1} -1711462090.579113 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_node_toleranceRequest/topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.15;">,topic_name="rq/topological_map_manager2/update_node_toleranceRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateNodeTolerance_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.579117 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1208 from udp/10.101.12.71:40473 -1711462090.579125 [0] recvMC: INFOTS(1711462090.579057465) -1711462090.579121 [0] dq.builtin: => EVERYONE -1711462090.579132 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #24 L(:1c1 16938.238629) => EVERYONE -1711462090.579140 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3104) scanning all wrs of topic rq/topological_map_manager2/update_node_toleranceRequest -1711462090.579184 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #24: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.579194 [0] recvMC: HEARTBEAT(F#30:24..24 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@24(sync) 110cd0d:e3b81b8d:1ccb65b1:504@24(sync) 110cd0d:79d6eeac:ea4a8907:504@24(sync)) -1711462090.579276 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.579310 [0] recv: INFOTS(1711462090.579246849) -1711462090.579315 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #25 L(:1c1 16938.238814)) -1711462090.579325 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #25: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.16;">,topic_name="rr/topological_map_manager2/modify_node_tagsReply",type_name="topological_navigation_msgs::srv::dds_::ModifyTag_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.579340 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3203 reliable volatile writer unnamed: (default).rr/topological_map_manager2/modify_node_tagsReply/topological_navigation_msgs::srv::dds_::ModifyTag_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.16;">,topic_name="rr/topological_map_manager2/modify_node_tagsReply",type_name="topological_navigation_msgs::srv::dds_::ModifyTag_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.579348 [0] dq.builtin: => EVERYONE -1711462090.579356 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3203) scanning all rds of topic rr/topological_map_manager2/modify_node_tagsReply -1711462090.579362 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 328 from udp/10.101.12.71:40473 -1711462090.579364 [0] recv: INFOTS(1711462090.579338744) -1711462090.579367 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #23 L(:1c1 16938.238868)) -1711462090.579377 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #23: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.16;">,topic_name="rq/topological_map_manager2/modify_node_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::ModifyTag_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3304},adlink_entity_factory=1} -1711462090.579393 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/modify_node_tagsRequest/topological_navigation_msgs::srv::dds_::ModifyTag_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.16;">,topic_name="rq/topological_map_manager2/modify_node_tagsRequest",type_name="topological_navigation_msgs::srv::dds_::ModifyTag_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.579403 [0] dq.builtin: => EVERYONE -1711462090.579404 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1256 from udp/10.101.12.71:40473 -1711462090.579417 [0] recvMC: INFOTS(1711462090.579348653) -1711462090.579413 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3304) scanning all wrs of topic rq/topological_map_manager2/modify_node_tagsRequest -1711462090.579424 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #25 L(:1c1 16938.238920) => EVERYONE -1711462090.579475 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #25: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.579481 [0] recvMC: HEARTBEAT(F#31:25..25 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@25(sync) 110cd0d:e3b81b8d:1ccb65b1:504@25(sync) 110cd0d:79d6eeac:ea4a8907:504@25(sync)) -1711462090.579560 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.579562 [0] recv: INFOTS(1711462090.579526903) -1711462090.579565 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #26 L(:1c1 16938.239066)) -1711462090.579574 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #26: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.17;">,topic_name="rr/topological_map_manager2/add_tag_to_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddTag_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.579589 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_tag_to_nodeReply/topological_navigation_msgs::srv::dds_::AddTag_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.17;">,topic_name="rr/topological_map_manager2/add_tag_to_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddTag_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.579597 [0] dq.builtin: => EVERYONE -1711462090.579604 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3403) scanning all rds of topic rr/topological_map_manager2/add_tag_to_nodeReply -1711462090.579613 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.579625 [0] recv: INFOTS(1711462090.579582890) -1711462090.579631 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #24 L(:1c1 16938.239128)) -1711462090.579642 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #24: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.17;">,topic_name="rq/topological_map_manager2/add_tag_to_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddTag_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3504},adlink_entity_factory=1} -1711462090.579653 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1304 from udp/10.101.12.71:40473 -1711462090.579657 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_tag_to_nodeRequest/topological_navigation_msgs::srv::dds_::AddTag_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.17;">,topic_name="rq/topological_map_manager2/add_tag_to_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddTag_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.579668 [0] dq.builtin: => EVERYONE -1711462090.579659 [0] recvMC: INFOTS(1711462090.579592017) -1711462090.579677 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3504) scanning all wrs of topic rq/topological_map_manager2/add_tag_to_nodeRequest -1711462090.579678 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #26 L(:1c1 16938.239163) => EVERYONE -1711462090.579730 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #26: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.579737 [0] recvMC: HEARTBEAT(F#32:26..26 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@26(sync) 110cd0d:e3b81b8d:1ccb65b1:504@26(sync) 110cd0d:79d6eeac:ea4a8907:504@26(sync)) -1711462090.579765 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.579772 [0] recv: INFOTS(1711462090.579726226) -1711462090.579777 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #27 L(:1c1 16938.239275)) -1711462090.579789 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #27: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.18;">,topic_name="rr/topological_map_manager2/rm_tag_from_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddTag_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.579804 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.579811 [0] recv: INFOTS(1711462090.579761832) -1711462090.579807 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3603 reliable volatile writer unnamed: (default).rr/topological_map_manager2/rm_tag_from_nodeReply/topological_navigation_msgs::srv::dds_::AddTag_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.18;">,topic_name="rr/topological_map_manager2/rm_tag_from_nodeReply",type_name="topological_navigation_msgs::srv::dds_::AddTag_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.579817 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #25 L(:1c1 16938.239314)) -1711462090.579826 [0] dq.builtin: => EVERYONE -1711462090.579835 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3603) scanning all rds of topic rr/topological_map_manager2/rm_tag_from_nodeReply -1711462090.579843 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1352 from udp/10.101.12.71:40473 -1711462090.579848 [0] recvMC: INFOTS(1711462090.579784458) -1711462090.579844 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #25: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.18;">,topic_name="rq/topological_map_manager2/rm_tag_from_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddTag_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3704},adlink_entity_factory=1} -1711462090.579854 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #27 L(:1c1 16938.239352) => EVERYONE -1711462090.579870 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3704 reliable volatile reader unnamed: (default).rq/topological_map_manager2/rm_tag_from_nodeRequest/topological_navigation_msgs::srv::dds_::AddTag_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.18;">,topic_name="rq/topological_map_manager2/rm_tag_from_nodeRequest",type_name="topological_navigation_msgs::srv::dds_::AddTag_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.579878 [0] dq.builtin: => EVERYONE -1711462090.579886 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3704) scanning all wrs of topic rq/topological_map_manager2/rm_tag_from_nodeRequest -1711462090.579906 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #27: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.579916 [0] recvMC: HEARTBEAT(F#33:27..27 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@27(sync) 110cd0d:e3b81b8d:1ccb65b1:504@27(sync) 110cd0d:79d6eeac:ea4a8907:504@27(sync)) -1711462090.580005 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.580010 [0] recv: INFOTS(1711462090.579973596) -1711462090.580016 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #28 L(:1c1 16938.239513)) -1711462090.580028 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #28: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.19;">,topic_name="rr/topological_map_manager2/add_param_to_edge_configReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580042 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_param_to_edge_configReply/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.19;">,topic_name="rr/topological_map_manager2/add_param_to_edge_configReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580051 [0] dq.builtin: => EVERYONE -1711462090.580055 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.580061 [0] recv: INFOTS(1711462090.580027501) -1711462090.580059 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3803) scanning all rds of topic rr/topological_map_manager2/add_param_to_edge_configReply -1711462090.580067 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #26 L(:1c1 16938.239565)) -1711462090.580077 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #26: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.19;">,topic_name="rq/topological_map_manager2/add_param_to_edge_configRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3904},adlink_entity_factory=1} -1711462090.580092 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_param_to_edge_configRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.19;">,topic_name="rq/topological_map_manager2/add_param_to_edge_configRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.580095 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1400 from udp/10.101.12.71:40473 -1711462090.580107 [0] recvMC: INFOTS(1711462090.580038123) -1711462090.580103 [0] dq.builtin: => EVERYONE -1711462090.580113 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #28 L(:1c1 16938.239610) => EVERYONE -1711462090.580124 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3904) scanning all wrs of topic rq/topological_map_manager2/add_param_to_edge_configRequest -1711462090.580167 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #28: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.580173 [0] recvMC: HEARTBEAT(F#34:28..28 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@28(sync) 110cd0d:e3b81b8d:1ccb65b1:504@28(sync) 110cd0d:79d6eeac:ea4a8907:504@28(sync)) -1711462090.580220 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.580224 [0] recv: INFOTS(1711462090.580189530) -1711462090.580230 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #29 L(:1c1 16938.239728)) -1711462090.580242 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #29: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1a;">,topic_name="rr/topological_map_manager2/rm_param_from_edge_configReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580257 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3a03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/rm_param_from_edge_configReply/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1a;">,topic_name="rr/topological_map_manager2/rm_param_from_edge_configReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580267 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 348 from udp/10.101.12.71:40473 -1711462090.580273 [0] recv: INFOTS(1711462090.580241067) -1711462090.580269 [0] dq.builtin: => EVERYONE -1711462090.580280 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #27 L(:1c1 16938.239776)) -1711462090.580288 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3a03) scanning all rds of topic rr/topological_map_manager2/rm_param_from_edge_configReply -1711462090.580298 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #27: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1a;">,topic_name="rq/topological_map_manager2/rm_param_from_edge_configRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3b04},adlink_entity_factory=1} -1711462090.580309 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1448 from udp/10.101.12.71:40473 -1711462090.580316 [0] recvMC: INFOTS(1711462090.580250371) -1711462090.580312 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/rm_param_from_edge_configRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1a;">,topic_name="rq/topological_map_manager2/rm_param_from_edge_configRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.580322 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #29 L(:1c1 16938.239820) => EVERYONE -1711462090.580330 [0] dq.builtin: => EVERYONE -1711462090.580338 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3b04) scanning all wrs of topic rq/topological_map_manager2/rm_param_from_edge_configRequest -1711462090.580374 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #29: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.580384 [0] recvMC: HEARTBEAT(F#35:29..29 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@29(sync) 110cd0d:e3b81b8d:1ccb65b1:504@29(sync) 110cd0d:79d6eeac:ea4a8907:504@29(sync)) -1711462090.580427 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.580433 [0] recv: INFOTS(1711462090.580393021) -1711462090.580438 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #30 L(:1c1 16938.239937)) -1711462090.580449 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #30: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1b;">,topic_name="rr/topological_map_manager2/rm_param_from_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580463 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3c03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/rm_param_from_topological_mapReply/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1b;">,topic_name="rr/topological_map_manager2/rm_param_from_topological_mapReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580470 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.580476 [0] recv: INFOTS(1711462090.580434684) -1711462090.580471 [0] dq.builtin: => EVERYONE -1711462090.580482 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #28 L(:1c1 16938.239980)) -1711462090.580489 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3c03) scanning all rds of topic rr/topological_map_manager2/rm_param_from_topological_mapReply -1711462090.580508 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #28: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1b;">,topic_name="rq/topological_map_manager2/rm_param_from_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3d04},adlink_entity_factory=1} -1711462090.580520 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1496 from udp/10.101.12.71:40473 -1711462090.580523 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/rm_param_from_topological_mapRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1b;">,topic_name="rq/topological_map_manager2/rm_param_from_topological_mapRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfig_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.580525 [0] recvMC: INFOTS(1711462090.580451532) -1711462090.580537 [0] dq.builtin: => EVERYONE -1711462090.580539 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #30 L(:1c1 16938.240028) => EVERYONE -1711462090.580546 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3d04) scanning all wrs of topic rq/topological_map_manager2/rm_param_from_topological_mapRequest -1711462090.580590 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #30: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.580596 [0] recvMC: HEARTBEAT(F#36:30..30 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@30(sync) 110cd0d:e3b81b8d:1ccb65b1:504@30(sync) 110cd0d:79d6eeac:ea4a8907:504@30(sync)) -1711462090.580695 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.580702 [0] recv: INFOTS(1711462090.580658980) -1711462090.580707 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #31 L(:1c1 16938.240205)) -1711462090.580718 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #31: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1c;">,topic_name="rr/topological_map_manager2/update_node_restrictionsReply",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580733 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3e03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_node_restrictionsReply/topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1c;">,topic_name="rr/topological_map_manager2/update_node_restrictionsReply",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580741 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.580750 [0] recv: INFOTS(1711462090.580705816) -1711462090.580743 [0] dq.builtin: => EVERYONE -1711462090.580755 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #29 L(:1c1 16938.240254)) -1711462090.580765 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:3e03) scanning all rds of topic rr/topological_map_manager2/update_node_restrictionsReply -1711462090.580775 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #29: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1c;">,topic_name="rq/topological_map_manager2/update_node_restrictionsRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3f04},adlink_entity_factory=1} -1711462090.580785 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1544 from udp/10.101.12.71:40473 -1711462090.580789 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:3f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_node_restrictionsRequest/topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1c;">,topic_name="rq/topological_map_manager2/update_node_restrictionsRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.580790 [0] recvMC: INFOTS(1711462090.580722130) -1711462090.580796 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #31 L(:1c1 16938.240293) => EVERYONE -1711462090.580801 [0] dq.builtin: => EVERYONE -1711462090.580809 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:3f04) scanning all wrs of topic rq/topological_map_manager2/update_node_restrictionsRequest -1711462090.580843 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #31: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.580850 [0] recvMC: HEARTBEAT(F#37:31..31 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@31(sync) 110cd0d:e3b81b8d:1ccb65b1:504@31(sync) 110cd0d:79d6eeac:ea4a8907:504@31(sync)) -1711462090.580902 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.580905 [0] recv: INFOTS(1711462090.580867994) -1711462090.580911 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #32 L(:1c1 16938.240409)) -1711462090.580923 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #32: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1d;">,topic_name="rr/topological_map_manager2/update_edge_restrictionsReply",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580938 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4003 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_edge_restrictionsReply/topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1d;">,topic_name="rr/topological_map_manager2/update_edge_restrictionsReply",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.580943 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.580946 [0] dq.builtin: => EVERYONE -1711462090.580947 [0] recv: INFOTS(1711462090.580901717) -1711462090.580955 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4003) scanning all rds of topic rr/topological_map_manager2/update_edge_restrictionsReply -1711462090.580956 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #30 L(:1c1 16938.240451)) -1711462090.580965 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #30: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1d;">,topic_name="rq/topological_map_manager2/update_edge_restrictionsRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4104},adlink_entity_factory=1} -1711462090.580979 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_edge_restrictionsRequest/topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1d;">,topic_name="rq/topological_map_manager2/update_edge_restrictionsRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateRestrictions_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.580990 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1592 from udp/10.101.12.71:40473 -1711462090.580995 [0] recvMC: INFOTS(1711462090.580918119) -1711462090.580990 [0] dq.builtin: => EVERYONE -1711462090.581002 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #32 L(:1c1 16938.240499) => EVERYONE -1711462090.581010 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4104) scanning all wrs of topic rq/topological_map_manager2/update_edge_restrictionsRequest -1711462090.581055 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #32: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.581061 [0] recvMC: HEARTBEAT(F#38:32..32 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@32(sync) 110cd0d:e3b81b8d:1ccb65b1:504@32(sync) 110cd0d:79d6eeac:ea4a8907:504@32(sync)) -1711462090.581194 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.581198 [0] recv: INFOTS(1711462090.581146130) -1711462090.581204 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #33 L(:1c1 16938.240702)) -1711462090.581216 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #33: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1e;">,topic_name="rr/topological_map_manager2/update_edgeReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdge_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581232 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4203 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_edgeReply/topological_navigation_msgs::srv::dds_::UpdateEdge_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1e;">,topic_name="rr/topological_map_manager2/update_edgeReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdge_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581237 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.581241 [0] dq.builtin: => EVERYONE -1711462090.581242 [0] recv: INFOTS(1711462090.581210847) -1711462090.581251 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #31 L(:1c1 16938.240746)) -1711462090.581252 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4203) scanning all rds of topic rr/topological_map_manager2/update_edgeReply -1711462090.581269 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #31: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1e;">,topic_name="rq/topological_map_manager2/update_edgeRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdge_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4304},adlink_entity_factory=1} -1711462090.581280 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1640 from udp/10.101.12.71:40473 -1711462090.581284 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_edgeRequest/topological_navigation_msgs::srv::dds_::UpdateEdge_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1e;">,topic_name="rq/topological_map_manager2/update_edgeRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdge_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.581285 [0] recvMC: INFOTS(1711462090.581220310) -1711462090.581295 [0] dq.builtin: => EVERYONE -1711462090.581300 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #33 L(:1c1 16938.240789) => EVERYONE -1711462090.581310 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4304) scanning all wrs of topic rq/topological_map_manager2/update_edgeRequest -1711462090.581355 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #33: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.581362 [0] recvMC: HEARTBEAT(F#39:33..33 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@33(sync) 110cd0d:e3b81b8d:1ccb65b1:504@33(sync) 110cd0d:79d6eeac:ea4a8907:504@33(sync)) -1711462090.581461 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 336 from udp/10.101.12.71:40473 -1711462090.581465 [0] recv: INFOTS(1711462090.581430143) -1711462090.581470 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #34 L(:1c1 16938.240969)) -1711462090.581481 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #34: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1f;">,topic_name="rr/topological_map_manager2/update_actionReply",type_name="topological_navigation_msgs::srv::dds_::UpdateAction_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581501 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_actionReply/topological_navigation_msgs::srv::dds_::UpdateAction_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1f;">,topic_name="rr/topological_map_manager2/update_actionReply",type_name="topological_navigation_msgs::srv::dds_::UpdateAction_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581509 [0] dq.builtin: => EVERYONE -1711462090.581516 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 332 from udp/10.101.12.71:40473 -1711462090.581518 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4403) scanning all rds of topic rr/topological_map_manager2/update_actionReply -1711462090.581520 [0] recv: INFOTS(1711462090.581483917) -1711462090.581529 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #32 L(:1c1 16938.241024)) -1711462090.581541 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #32: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1f;">,topic_name="rq/topological_map_manager2/update_actionRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateAction_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4504},adlink_entity_factory=1} -1711462090.581556 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_actionRequest/topological_navigation_msgs::srv::dds_::UpdateAction_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.1f;">,topic_name="rq/topological_map_manager2/update_actionRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateAction_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.581563 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1688 from udp/10.101.12.71:40473 -1711462090.581569 [0] recvMC: INFOTS(1711462090.581502801) -1711462090.581565 [0] dq.builtin: => EVERYONE -1711462090.581577 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #34 L(:1c1 16938.241073) => EVERYONE -1711462090.581587 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4504) scanning all wrs of topic rq/topological_map_manager2/update_actionRequest -1711462090.581634 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #34: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.581645 [0] recvMC: HEARTBEAT(F#40:34..34 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@34(sync) 110cd0d:e3b81b8d:1ccb65b1:504@34(sync) 110cd0d:79d6eeac:ea4a8907:504@34(sync)) -1711462090.581762 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 328 from udp/10.101.12.71:40473 -1711462090.581766 [0] recv: INFOTS(1711462090.581730334) -1711462090.581772 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #35 L(:1c1 16938.241270)) -1711462090.581783 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #35: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.20;">,topic_name="rr/topological_map_manager2/add_datumReply",type_name="topological_navigation_msgs::srv::dds_::AddDatum_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581798 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4603 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_datumReply/topological_navigation_msgs::srv::dds_::AddDatum_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.20;">,topic_name="rr/topological_map_manager2/add_datumReply",type_name="topological_navigation_msgs::srv::dds_::AddDatum_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.581807 [0] dq.builtin: => EVERYONE -1711462090.581810 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.581817 [0] recv: INFOTS(1711462090.581783677) -1711462090.581821 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #33 L(:1c1 16938.241321)) -1711462090.581819 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4603) scanning all rds of topic rr/topological_map_manager2/add_datumReply -1711462090.581840 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #33: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.20;">,topic_name="rq/topological_map_manager2/add_datumRequest",type_name="topological_navigation_msgs::srv::dds_::AddDatum_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4704},adlink_entity_factory=1} -1711462090.581855 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4704 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_datumRequest/topological_navigation_msgs::srv::dds_::AddDatum_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.20;">,topic_name="rq/topological_map_manager2/add_datumRequest",type_name="topological_navigation_msgs::srv::dds_::AddDatum_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.581867 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1736 from udp/10.101.12.71:40473 -1711462090.581875 [0] recvMC: INFOTS(1711462090.581796803) -1711462090.581868 [0] dq.builtin: => EVERYONE -1711462090.581884 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #35 L(:1c1 16938.241379) => EVERYONE -1711462090.581892 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4704) scanning all wrs of topic rq/topological_map_manager2/add_datumRequest -1711462090.581945 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #35: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.581953 [0] recvMC: HEARTBEAT(F#41:35..35 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@35(sync) 110cd0d:e3b81b8d:1ccb65b1:504@35(sync) 110cd0d:79d6eeac:ea4a8907:504@35(sync)) -1711462090.582076 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.582084 [0] recv: INFOTS(1711462090.582034642) -1711462090.582090 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #36 L(:1c1 16938.241587)) -1711462090.582102 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #36: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rr/topological_map_manager2/update_fail_policyReply",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582118 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4803 reliable volatile writer unnamed: (default).rr/topological_map_manager2/update_fail_policyReply/topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rr/topological_map_manager2/update_fail_policyReply",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.582131 [0] dq.builtin: => EVERYONE -1711462090.582139 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4803) scanning all rds of topic rr/topological_map_manager2/update_fail_policyReply -1711462090.582179 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1784 from udp/10.101.12.71:40473 -1711462090.582189 [0] recvMC: INFOTS(1711462090.582110521) -1711462090.582196 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 340 from udp/10.101.12.71:40473 -1711462090.582203 [0] recv: INFOTS(1711462090.582090581) -1711462090.582196 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #36 L(:1c1 16938.241692) => EVERYONE -1711462090.582213 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #34 L(:1c1 16938.241706)) -1711462090.582229 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #34: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rq/topological_map_manager2/update_fail_policyRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4904},adlink_entity_factory=1} -1711462090.582248 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4904 reliable volatile reader unnamed: (default).rq/topological_map_manager2/update_fail_policyRequest/topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.21;">,topic_name="rq/topological_map_manager2/update_fail_policyRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateFailPolicy_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.582259 [0] dq.builtin: => EVERYONE -1711462090.582273 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #36: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.582276 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4904) scanning all wrs of topic rq/topological_map_manager2/update_fail_policyRequest -1711462090.582286 [0] recvMC: HEARTBEAT(F#42:36..36 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@36(sync) 110cd0d:e3b81b8d:1ccb65b1:504@36(sync) 110cd0d:79d6eeac:ea4a8907:504@36(sync)) -1711462090.582543 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1832 from udp/10.101.12.71:40473 -1711462090.582548 [0] recvMC: INFOTS(1711462090.582480087) -1711462090.582554 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #37 L(:1c1 16938.242052) => EVERYONE -1711462090.582603 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #37: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.582609 [0] recvMC: HEARTBEAT(F#43:37..37 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@37(sync) 110cd0d:e3b81b8d:1ccb65b1:504@37(sync) 110cd0d:79d6eeac:ea4a8907:504@37(sync)) -1711462090.582888 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1880 from udp/10.101.12.71:40473 -1711462090.582897 [0] recvMC: INFOTS(1711462090.582807206) -1711462090.582902 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #38 L(:1c1 16938.242401) => EVERYONE -1711462090.582952 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #38: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.582958 [0] recvMC: HEARTBEAT(F#44:38..38 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@38(sync) 110cd0d:e3b81b8d:1ccb65b1:504@38(sync) 110cd0d:79d6eeac:ea4a8907:504@38(sync)) -1711462090.583091 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 352 from udp/10.101.12.71:40473 -1711462090.583101 [0] recv: INFOTS(1711462090.582385748) -1711462090.583107 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #37 L(:1c1 16938.242604)) -1711462090.583120 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.583125 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.583125 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #37: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rr/topological_map_manager2/set_node_influence_zoneReply",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583132 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.242628) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:56a27910:57318171:3c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 - queue for transmit -1711462090.583143 [0] recv: non-timed queue now has 1 items -1711462090.583148 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4a03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/set_node_influence_zoneReply/topological_navigation_msgs::srv::dds_::SetInfluenceZone_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rr/topological_map_manager2/set_node_influence_zoneReply",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583149 [0] recv: ) -1711462090.583154 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40088f0 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.583158 [0] dq.builtin: => EVERYONE -1711462090.583169 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4a03) scanning all rds of topic rr/topological_map_manager2/set_node_influence_zoneReply -1711462090.583171 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408c40089d8:48 [ udp/10.101.12.71:7418@2 ] -1711462090.583180 [0] tev: traffic-xmit (1) 68 -1711462090.583172 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.583198 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.583205 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.242701) 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:56a27910:57318171:4c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 - queue for transmit -1711462090.583210 [0] recv: non-timed queue now has 1 items -1711462090.583214 [0] recv: ) -1711462090.583217 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc007020 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.583223 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462090.583227 [0] recv: INFOTS(1711462090.582465762) -1711462090.583228 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc007108:48 [ udp/10.101.12.71:7418@2 ] -1711462090.583240 [0] tev: traffic-xmit (1) 68 -1711462090.583234 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #35 L(:1c1 16938.242731)) -1711462090.583259 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #35: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rq/topological_map_manager2/set_node_influence_zoneRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4b04},adlink_entity_factory=1} -1711462090.583261 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1928 from udp/10.101.12.71:40473 -1711462090.583273 [0] recvMC: INFOTS(1711462090.583170500) -1711462090.583276 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4b04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/set_node_influence_zoneRequest/topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.22;">,topic_name="rq/topological_map_manager2/set_node_influence_zoneRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZone_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583280 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #39 L(:1c1 16938.242776) => EVERYONE -1711462090.583286 [0] dq.builtin: => EVERYONE -1711462090.583295 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4b04) scanning all wrs of topic rq/topological_map_manager2/set_node_influence_zoneRequest -1711462090.583305 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 156 from udp/10.101.12.71:52025 -1711462090.583312 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.583318 [0] recv: ACKNACK(F#1:1/9:111111111 L(:1c1 16938.242816) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:56a27910:57318171:3c2 complying RX1non-timed queue now has 1 items -1711462090.583320 [0] recv: RX2non-timed queue now has 2 items -1711462090.583323 [0] recv: RX3non-timed queue now has 3 items -1711462090.583326 [0] recv: RX4non-timed queue now has 4 items -1711462090.583328 [0] recv: RX5non-timed queue now has 5 items -1711462090.583331 [0] recv: RX6non-timed queue now has 6 items -1711462090.583336 [0] recv: RX7non-timed queue now has 6 items -1711462090.583338 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc006d20 0(rexmit(110cd0d:56a27910:57318171:3c2:#1/1)): niov 0 sz 0 => now niov 3 sz 244 -1711462090.583339 [0] recv: RX8non-timed queue now has 7 items -1711462090.583350 [0] recv: RX9non-timed queue now has 8 items -1711462090.583340 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #39: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.583352 [0] recv: rexmit#9 maxseq:9<9<=9defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 - queue for transmit -1711462090.583363 [0] recv: ) -1711462090.583366 [0] recv: ACKNACK(#0:1/0: 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 - queue for transmit -1711462090.583368 [0] recv: non-timed queue now has 8 items -1711462090.583371 [0] recv: non-timed queue now has 9 items -1711462090.583373 [0] recv: ) -1711462090.583363 [0] recvMC: HEARTBEAT(F#45:39..39 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@39(sync) 110cd0d:e3b81b8d:1ccb65b1:504@39(sync) 110cd0d:79d6eeac:ea4a8907:504@39(sync)) -1711462090.583375 [0] recv: ACKNACK(#0:1/0: 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:56a27910:57318171:300c3 setting-has-replied-to-hb preemptive-nack whc-empty defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:300c3 -> 110aba1:7b19badd:ce0adb73:300c4 - queue for transmit -1711462090.583382 [0] recv: non-timed queue now has 10 items -1711462090.583384 [0] recv: ) -1711462090.583355 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc006ea0 0(rexmit(110cd0d:56a27910:57318171:3c2:#2/1)): niov 3 sz 244 => now niov 5 sz 480 -1711462090.583389 [0] recv: ACKNACK(F#1:1/10:1111111111 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:56a27910:57318171:4c2 complying RX1non-timed queue now has 11 items -1711462090.583396 [0] recv: RX2non-timed queue now has 12 items -1711462090.583400 [0] recv: RX3non-timed queue now has 13 items -1711462090.583402 [0] recv: RX4non-timed queue now has 14 items -1711462090.583404 [0] recv: RX5non-timed queue now has 15 items -1711462090.583406 [0] recv: RX6non-timed queue now has 16 items -1711462090.583409 [0] recv: RX7non-timed queue now has 17 items -1711462090.583412 [0] recv: RX8non-timed queue now has 18 items -1711462090.583414 [0] recv: RX9non-timed queue now has 19 items -1711462090.583417 [0] recv: RX10non-timed queue now has 20 items -1711462090.583419 [0] recv: rexmit#10 maxseq:10<10<=10defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 - queue for transmit -1711462090.583420 [0] recv: ) -1711462090.583421 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0069c0 0(rexmit(110cd0d:56a27910:57318171:3c2:#3/1)): niov 5 sz 480 => now niov 7 sz 684 -1711462090.583422 [0] recv: send_deferred_heartbeat: e22762d101915c4a -> 79bd04c9dfe259de - queue for transmit -1711462090.583427 [0] recv: non-timed queue now has 19 items -1711462090.583426 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc006840 0(rexmit(110cd0d:56a27910:57318171:3c2:#4/1)): niov 7 sz 684 => now niov 9 sz 984 -1711462090.583432 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.583437 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.583435 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0066c0 0(rexmit(110cd0d:56a27910:57318171:3c2:#5/1)): niov 9 sz 984 => now niov 11 sz 1276 -1711462090.583441 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.242941) 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:56a27910:57318171:301c3 setting-has-replied-to-hb preemptive-nack whc-empty defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:301c3 -> 110aba1:7b19badd:ce0adb73:301c4 - queue for transmit -1711462090.583448 [0] recv: non-timed queue now has 18 items -1711462090.583451 [0] recv: ) -1711462090.583452 [0] tev: nn_xpack_send 1276: 0x7408c000139c:20 0x7408bc006e08:52 0x7408ac01de94:172 0x7408bc006f98:36 0x7408ac010284:200 0x7408bc006ab8:36 0x7408ac0240d4:168 0x7408bc006938:36 0x7408ac015d34:264 0x7408bc0067b8:36 0x7408ac028794:256 [ udp/10.101.12.71:7418@2 ] -1711462090.583457 [0] tev: traffic-xmit (1) 1276 -1711462090.583454 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.583464 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.583461 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4009f70 0(rexmit(110cd0d:56a27910:57318171:3c2:#6/1)): niov 0 sz 0 => now niov 3 sz 336 -1711462090.583468 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.242968) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:e3b81b8d:1ccb65b1:3c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 - queue for transmit -1711462090.583475 [0] recv: non-timed queue now has 18 items -1711462090.583477 [0] recv: ) -1711462090.583481 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 324 from udp/10.101.12.71:40473 -1711462090.583484 [0] recv: INFOTS(1711462090.582707552) -1711462090.583472 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc002560 0(rexmit(110cd0d:56a27910:57318171:3c2:#7/1)): niov 3 sz 336 => now niov 5 sz 628 -1711462090.583487 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #38 L(:1c1 16938.242988)) -1711462090.583495 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0023e0 0(rexmit(110cd0d:56a27910:57318171:3c2:#8/1)): niov 5 sz 628 => now niov 7 sz 920 -1711462090.583503 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40091f0 0(rexmit(110cd0d:56a27910:57318171:3c2:#9/1)): niov 7 sz 920 => now niov 9 sz 1232 -1711462090.583508 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40094f0 0(control): niov 9 sz 1232 => now niov 10 sz 1264 -1711462090.583508 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #38: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rr/topological_map_manager2/clear_topological_nodesReply",type_name="std_srvs::srv::dds_::Empty_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583513 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4009370 0(control): niov 10 sz 1264 => now niov 11 sz 1296 -1711462090.583520 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4009670 0(control): niov 11 sz 1296 => now niov 12 sz 1328 -1711462090.583540 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 92 from udp/10.101.12.71:52025 -1711462090.583543 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.583540 [0] tev: nn_xpack_send 1328: 0x7408c000139c:20 0x7408c400a058:52 0x7408ac02b704:264 0x7408bc002658:36 0x7408ac02f294:256 0x7408bc0024d8:36 0x7408ac034034:256 0x7408c40092e8:36 0x7408ac037b04:276 0x7408c40095e8:32 0x7408c4009468:32 0x7408c4009768:32 [ udp/10.101.12.71:7418@2 ] -1711462090.583545 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.243047) 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:e3b81b8d:1ccb65b1:4c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 - queue for transmit -1711462090.583550 [0] tev: traffic-xmit (1) 1328 -1711462090.583556 [0] recv: non-timed queue now has 13 items -1711462090.583559 [0] recv: ) -1711462090.583557 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40097f0 0(rexmit(110cd0d:56a27910:57318171:4c2:#1/1)): niov 0 sz 0 => now niov 3 sz 248 -1711462090.583561 [0] recv: ACKNACK(#0:1/0: 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 - queue for transmit -1711462090.583547 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4c03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/clear_topological_nodesReply/std_srvs::srv::dds_::Empty_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rr/topological_map_manager2/clear_topological_nodesReply",type_name="std_srvs::srv::dds_::Empty_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583569 [0] recv: non-timed queue now has 13 items -1711462090.583567 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400a3f0 0(rexmit(110cd0d:56a27910:57318171:4c2:#2/1)): niov 3 sz 248 => now niov 5 sz 540 -1711462090.583586 [0] recv: ) -1711462090.583596 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 964 from udp/10.101.12.71:40473 -1711462090.583599 [0] recv: INFOTS(1711462090.582794562) -1711462090.583604 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #36 L(:1c1 16938.243103)) -1711462090.583607 [0] recv: INFOTS(1711462090.583082734) -1711462090.583611 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #39) -1711462090.583614 [0] recv: INFOTS(1711462090.583152869) -1711462090.583617 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #37) -1711462090.583599 [0] dq.builtin: => EVERYONE -1711462090.583620 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.583591 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400a570 0(rexmit(110cd0d:56a27910:57318171:4c2:#3/1)): niov 5 sz 540 => now niov 7 sz 824 -1711462090.583623 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.583629 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400a6f0 0(rexmit(110cd0d:56a27910:57318171:4c2:#4/1)): niov 7 sz 824 => now niov 9 sz 1116 -1711462090.583635 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4009df0 0(rexmit(110cd0d:56a27910:57318171:4c2:#5/1)): niov 9 sz 1116 => now niov 11 sz 1400 -1711462090.583630 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.243127) 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:e3b81b8d:1ccb65b1:300c3 setting-has-replied-to-hb preemptive-nack whc-empty defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:300c3 -> 110aba1:7b19badd:ce0adb73:300c4 - queue for transmit -1711462090.583646 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4c03) scanning all rds of topic rr/topological_map_manager2/clear_topological_nodesReply -1711462090.583642 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1976 from udp/10.101.12.71:40473 -1711462090.583657 [0] recvMC: INFOTS(1711462090.583554164) -1711462090.583649 [0] tev: nn_xpack_send 1400: 0x7408c000139c:20 0x7408c40098d8:52 0x7408ac01feb4:176 0x7408c400a4e8:36 0x7408ac024e54:256 0x7408c400a668:36 0x7408ac029384:248 0x7408c400a7e8:36 0x7408ac02c7e4:256 0x7408c4009ee8:36 0x7408ac030404:248 [ udp/10.101.12.71:7418@2 ] -1711462090.583661 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #36: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rq/topological_map_manager2/clear_topological_nodesRequest",type_name="std_srvs::srv::dds_::Empty_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4d04},adlink_entity_factory=1} -1711462090.583663 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #40 L(:1c1 16938.243161) => EVERYONE -1711462090.583649 [0] recv: non-timed queue now has 10 items -1711462090.583679 [0] recv: ) -1711462090.583683 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4d04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/clear_topological_nodesRequest/std_srvs::srv::dds_::Empty_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.23;">,topic_name="rq/topological_map_manager2/clear_topological_nodesRequest",type_name="std_srvs::srv::dds_::Empty_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583680 [0] tev: traffic-xmit (1) 1400 -1711462090.583688 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 680 from udp/10.101.12.71:40473 -1711462090.583699 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0026e0 0(rexmit(110cd0d:56a27910:57318171:4c2:#6/1)): niov 0 sz 0 => now niov 3 sz 320 -1711462090.583698 [0] dq.builtin: => EVERYONE -1711462090.583700 [0] recv: INFOTS(1711462090.583470556) -1711462090.583705 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400a870 0(rexmit(110cd0d:56a27910:57318171:4c2:#7/1)): niov 3 sz 320 => now niov 5 sz 624 -1711462090.583711 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4d04) scanning all wrs of topic rq/topological_map_manager2/clear_topological_nodesRequest -1711462090.583713 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #40 L(:1c1 16938.243204)) -1711462090.583715 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400a9f0 0(rexmit(110cd0d:56a27910:57318171:4c2:#8/1)): niov 5 sz 624 => now niov 7 sz 800 -1711462090.583716 [0] recv: INFOTS(1711462090.583536280) -1711462090.583720 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ab70 0(rexmit(110cd0d:56a27910:57318171:4c2:#9/1)): niov 7 sz 800 => now niov 9 sz 968 -1711462090.583725 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400acf0 0(rexmit(110cd0d:56a27910:57318171:4c2:#10/1)): niov 9 sz 968 => now niov 11 sz 1136 -1711462090.583726 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #38) -1711462090.583731 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #39: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rr/topological_map_manager2/add_topological_node_multiReply",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583728 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ae70 0(control): niov 11 sz 1136 => now niov 12 sz 1168 -1711462090.583709 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #40: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.583733 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.583886 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.583739 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400a0f0 0(control): niov 12 sz 1168 => now niov 13 sz 1200 -1711462090.583896 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400a270 0(control): niov 13 sz 1200 => now niov 15 sz 1256 -1711462090.583900 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400bef0 0(control): niov 15 sz 1256 => now niov 16 sz 1288 -1711462090.583892 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.243390) 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:e3b81b8d:1ccb65b1:301c3 setting-has-replied-to-hb preemptive-nack whc-empty defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:301c3 -> 110aba1:7b19badd:ce0adb73:301c4 - queue for transmit -1711462090.583902 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400aff0 0(control): niov 16 sz 1288 => now niov 17 sz 1320 -1711462090.583906 [0] recv: non-timed queue now has 2 items -1711462090.583909 [0] recv: ) -1711462090.583911 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400b170 0(control): niov 17 sz 1320 => now niov 18 sz 1352 -1711462090.583915 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400b2f0 0(control): niov 18 sz 1352 => now niov 19 sz 1384 -1711462090.583883 [0] recvMC: HEARTBEAT(F#46:40..40 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@40(sync) 110cd0d:e3b81b8d:1ccb65b1:504@40(sync) 110cd0d:79d6eeac:ea4a8907:504@40(sync)) -1711462090.583748 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4e03 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_topological_node_multiReply/topological_navigation_msgs::srv::dds_::AddNodeArray_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rr/topological_map_manager2/add_topological_node_multiReply",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.583926 [0] tev: nn_xpack_send 1384: 0x7408c000139c:20 0x7408bc0027c8:52 0x7408ac034b44:248 0x7408c400a968:36 0x7408ac038ff4:268 0x7408c400aae8:36 0x7408ac03b2c4:140 0x7408c400ac68:36 0x7408ac03cef4:132 0x7408c400ade8:36 0x7408ac03eef4:132 0x7408c400af68:32 0x7408c400a1e8:32 0x7408c400a340:24 0x7408c400a368:32 0x7408c400bfe8:32 0x7408c400b0e8:32 0x7408c400b268:32 0x7408c400b3e8:32 [ udp/10.101.12.71:7418@2 ] -1711462090.583932 [0] tev: traffic-xmit (1) 1384 -1711462090.583936 [0] dq.builtin: => EVERYONE -1711462090.583937 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.583943 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.583945 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:4e03) scanning all rds of topic rr/topological_map_manager2/add_topological_node_multiReply -1711462090.583945 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.243447) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:79d6eeac:ea4a8907:3c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 - queue for transmit -1711462090.583953 [0] recv: non-timed queue now has 1 items -1711462090.583956 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #37: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rq/topological_map_manager2/add_topological_node_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4f04},adlink_entity_factory=1} -1711462090.583956 [0] recv: ) -1711462090.583958 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400b470 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.583969 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408c400b558:48 [ udp/10.101.12.71:7418@2 ] -1711462090.583971 [0] tev: traffic-xmit (1) 68 -1711462090.583977 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:4f04 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_topological_node_multiRequest/topological_navigation_msgs::srv::dds_::AddNodeArray_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.24;">,topic_name="rq/topological_map_manager2/add_topological_node_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddNodeArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.583986 [0] dq.builtin: => EVERYONE -1711462090.583991 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 360 from udp/10.101.12.71:40473 -1711462090.583993 [0] recv: INFOTS(1711462090.583967232) -1711462090.583996 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #41 L(:1c1 16938.243497)) -1711462090.583999 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:4f04) scanning all wrs of topic rq/topological_map_manager2/add_topological_node_multiRequest -1711462090.584016 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #40: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rr/topological_map_manager2/add_edges_between_nodes_multiReply",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584033 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5003 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_edges_between_nodes_multiReply/topological_navigation_msgs::srv::dds_::AddEdgeArray_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rr/topological_map_manager2/add_edges_between_nodes_multiReply",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584041 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.584043 [0] recv: INFOTS(1711462090.584023477) -1711462090.584046 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #39 L(:1c1 16938.243547)) -1711462090.584051 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.584052 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.584055 [0] gc: gc 0x7408c8084400: not yet, shortsleep -1711462090.584057 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.243556) 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:79d6eeac:ea4a8907:4c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 - queue for transmit -1711462090.584063 [0] recv: non-timed queue now has 1 items -1711462090.584057 [0] dq.builtin: => EVERYONE -1711462090.584064 [0] recv: ) -1711462090.584066 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400c470 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.584074 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5003) scanning all rds of topic rr/topological_map_manager2/add_edges_between_nodes_multiReply -1711462090.584078 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408c400c558:48 [ udp/10.101.12.71:7418@2 ] -1711462090.584085 [0] tev: traffic-xmit (1) 68 -1711462090.584089 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #38: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rq/topological_map_manager2/add_edges_between_nodes_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5104},adlink_entity_factory=1} -1711462090.584094 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2024 from udp/10.101.12.71:40473 -1711462090.584097 [0] recvMC: INFOTS(1711462090.584035736) -1711462090.584099 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #41 L(:1c1 16938.243601) => EVERYONE -1711462090.584106 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5104 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_edges_between_nodes_multiRequest/topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.25;">,topic_name="rq/topological_map_manager2/add_edges_between_nodes_multiRequest",type_name="topological_navigation_msgs::srv::dds_::AddEdgeArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.584119 [0] dq.builtin: => EVERYONE -1711462090.584129 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:5104) scanning all wrs of topic rq/topological_map_manager2/add_edges_between_nodes_multiRequest -1711462090.584131 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #41: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.584139 [0] recvMC: HEARTBEAT(F#47:41..41 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@41(sync) 110cd0d:e3b81b8d:1ccb65b1:504@41(sync) 110cd0d:79d6eeac:ea4a8907:504@41(sync)) -1711462090.584140 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #41: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rr/topological_map_manager2/add_param_to_edge_config_multiReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584159 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5203 reliable volatile writer unnamed: (default).rr/topological_map_manager2/add_param_to_edge_config_multiReply/topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rr/topological_map_manager2/add_param_to_edge_config_multiReply",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584168 [0] dq.builtin: => EVERYONE -1711462090.584175 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5203) scanning all rds of topic rr/topological_map_manager2/add_param_to_edge_config_multiReply -1711462090.584184 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #39: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rq/topological_map_manager2/add_param_to_edge_config_multiRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5304},adlink_entity_factory=1} -1711462090.584193 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.584195 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.584198 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.243699) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 preemptive-nack rebase defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 - queue for transmit -1711462090.584201 [0] recv: non-timed queue now has 1 items -1711462090.584204 [0] recv: ) -1711462090.584206 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400c2f0 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.584198 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5304 reliable volatile reader unnamed: (default).rq/topological_map_manager2/add_param_to_edge_config_multiRequest/topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.26;">,topic_name="rq/topological_map_manager2/add_param_to_edge_config_multiRequest",type_name="topological_navigation_msgs::srv::dds_::UpdateEdgeConfigArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.584211 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408c400c3d8:48 [ udp/10.101.12.71:7418@2 ] -1711462090.584221 [0] tev: traffic-xmit (1) 68 -1711462090.584227 [0] dq.builtin: => EVERYONE -1711462090.584228 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.584235 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.584236 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:5304) scanning all wrs of topic rq/topological_map_manager2/add_param_to_edge_config_multiRequest -1711462090.584238 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.243739) 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:79d6eeac:ea4a8907:300c3 setting-has-replied-to-hb preemptive-nack whc-empty defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:300c3 -> 110aba1:7b19badd:ce0adb73:300c4 - queue for transmit -1711462090.584244 [0] recv: non-timed queue now has 1 items -1711462090.584246 [0] recv: ) -1711462090.584246 [0] tev: xpack_addmsg 0x7408c0001390 0x7408d0001390 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.584254 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408d0001478:48 [ udp/10.101.12.71:7418@2 ] -1711462090.584255 [0] tev: traffic-xmit (1) 68 -1711462090.584324 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 360 from udp/10.101.12.71:40473 -1711462090.584326 [0] recv: INFOTS(1711462090.584293877) -1711462090.584328 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #42 L(:1c1 16938.243830)) -1711462090.584347 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #42: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.27;">,topic_name="rr/topological_map_manager2/set_node_influence_zone_multiReply",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584372 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5403 reliable volatile writer unnamed: (default).rr/topological_map_manager2/set_node_influence_zone_multiReply/topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.27;">,topic_name="rr/topological_map_manager2/set_node_influence_zone_multiReply",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.584373 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 356 from udp/10.101.12.71:40473 -1711462090.584379 [0] recv: INFOTS(1711462090.584335539) -1711462090.584382 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #40 L(:1c1 16938.243883)) -1711462090.584385 [0] dq.builtin: => EVERYONE -1711462090.584395 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5403) scanning all rds of topic rr/topological_map_manager2/set_node_influence_zone_multiReply -1711462090.584407 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #40: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.27;">,topic_name="rq/topological_map_manager2/set_node_influence_zone_multiRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5504},adlink_entity_factory=1} -1711462090.584425 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.584427 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.584431 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5504 reliable volatile reader unnamed: (default).rq/topological_map_manager2/set_node_influence_zone_multiRequest/topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=54<"serviceid= 1.10.d7.b4.17.c5.b8.c5.94.bd.f.f4.0.0.0.27;">,topic_name="rq/topological_map_manager2/set_node_influence_zone_multiRequest",type_name="topological_navigation_msgs::srv::dds_::SetInfluenceZoneArray_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.584431 [0] recv: ACKNACK(#0:1/0: L(:1c1 16938.243931) 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:79d6eeac:ea4a8907:301c3 setting-has-replied-to-hb preemptive-nack whc-empty defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:301c3 -> 110aba1:7b19badd:ce0adb73:301c4 - queue for transmit -1711462090.584439 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2072 from udp/10.101.12.71:40473 -1711462090.584443 [0] recvMC: INFOTS(1711462090.584362470) -1711462090.584440 [0] recv: non-timed queue now has 1 items -1711462090.584448 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #42 L(:1c1 16938.243947) => EVERYONE -1711462090.584451 [0] recv: ) -1711462090.584447 [0] dq.builtin: => EVERYONE -1711462090.584456 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400b770 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.584467 [0] dq.builtin: match_proxy_reader_with_writers(prd 110d7b4:17c5b8c5:94bd0ff4:5504) scanning all wrs of topic rq/topological_map_manager2/set_node_influence_zone_multiRequest -1711462090.584480 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408c400b858:48 [ udp/10.101.12.71:7418@2 ] -1711462090.584484 [0] tev: traffic-xmit (1) 68 -1711462090.584491 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #42: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462090.584499 [0] recvMC: HEARTBEAT(F#49:42..42 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@42(sync) 110cd0d:e3b81b8d:1ccb65b1:504@42(sync) 110cd0d:79d6eeac:ea4a8907:504@42(sync)) -1711462090.584618 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 124 from udp/10.101.12.71:52025 -1711462090.584620 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.584623 [0] recv: ACKNACK(F#2:10/0: L(:1c1 16938.244124) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:56a27910:57318171:3c2 setting-has-replied-to-hb happy-now) -1711462090.584627 [0] recv: ACKNACK(F#1:1/1:1 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2 complying RX1non-timed queue now has 1 items -1711462090.584629 [0] recv: rexmit#1 maxseq:1<1<=1defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 - queue for transmit -1711462090.584631 [0] recv: ) -1711462090.584633 [0] recv: ACKNACK(F#1:1/0: 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:56a27910:57318171:300c3 happy-now) -1711462090.584635 [0] recv: send_deferred_heartbeat: b0f11d4f1d29d666 -> 2e9a32ef85ddbc26 - queue for transmit -1711462090.584633 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc0029e0 0(rexmit(110cd0d:56a27910:57318171:200c2:#1/1)): niov 0 sz 0 => now niov 3 sz 100 -1711462090.584636 [0] recv: non-timed queue now has 1 items -1711462090.584646 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc002860 0(control): niov 3 sz 100 => now niov 4 sz 132 -1711462090.584658 [0] tev: nn_xpack_send 132: 0x7408c000139c:20 0x7408bc002ac8:52 0x7408c800f16c:28 0x7408bc002958:32 [ udp/10.101.12.71:7418@2 ] -1711462090.584661 [0] tev: traffic-xmit (1) 132 -1711462090.584757 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 92 from udp/10.101.12.71:52025 -1711462090.584759 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.584761 [0] recv: ACKNACK(F#2:11/0: L(:1c1 16938.244263) 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:56a27910:57318171:4c2 setting-has-replied-to-hb happy-now) -1711462090.584763 [0] recv: ACKNACK(F#1:1/0: 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:56a27910:57318171:301c3 happy-now) -1711462090.584854 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 268 from udp/10.101.12.71:52025 -1711462090.584856 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.584860 [0] recv: ACKNACK(F#1:1/10:1111111111 L(:1c1 16938.244360) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:e3b81b8d:1ccb65b1:3c2 complying RX1non-timed queue now has 1 items -1711462090.584862 [0] recv: M2 RX3non-timed queue now has 2 items -1711462090.584864 [0] recv: RX4non-timed queue now has 3 items -1711462090.584868 [0] recv: RX5non-timed queue now has 3 items -1711462090.584871 [0] recv: RX6non-timed queue now has 4 items -1711462090.584873 [0] recv: RX7non-timed queue now has 5 items -1711462090.584876 [0] recv: RX8non-timed queue now has 6 items -1711462090.584878 [0] recv: RX9non-timed queue now has 7 items -1711462090.584869 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400b5f0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:3c2:#1/1)): niov 0 sz 0 => now niov 3 sz 244 -1711462090.584881 [0] recv: M10 FXGAP2..3/8:00000001non-timed queue now has 8 items -1711462090.584886 [0] recv: rexmit#9 maxseq:10<10<=10defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 - queue for transmit -1711462090.584888 [0] recv: ) -1711462090.584890 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400cef0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:3c2:#3/1)): niov 3 sz 244 => now niov 5 sz 448 -1711462090.584892 [0] recv: ACKNACK(F#1:1/10:1111111111 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:e3b81b8d:1ccb65b1:4c2 complying RX1non-timed queue now has 8 items -1711462090.584897 [0] recv: RX2non-timed queue now has 9 items -1711462090.584900 [0] recv: RX3non-timed queue now has 10 items -1711462090.584903 [0] recv: RX4non-timed queue now has 11 items -1711462090.584905 [0] recv: RX5non-timed queue now has 12 items -1711462090.584909 [0] recv: RX6non-timed queue now has 13 items -1711462090.584911 [0] recv: RX7non-timed queue now has 14 items -1711462090.584913 [0] recv: RX8non-timed queue now has 15 items -1711462090.584916 [0] recv: RX9non-timed queue now has 16 items -1711462090.584918 [0] recv: RX10non-timed queue now has 17 items -1711462090.584920 [0] recv: rexmit#10 maxseq:10<10<=10defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 - queue for transmit -1711462090.584922 [0] recv: non-timed queue now has 17 items -1711462090.584923 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ba70 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:3c2:#4/1)): niov 5 sz 448 => now niov 7 sz 748 -1711462090.584923 [0] recv: ) -1711462090.584929 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400b8f0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:3c2:#5/1)): niov 7 sz 748 => now niov 9 sz 1040 -1711462090.584931 [0] recv: ACKNACK(F#1:3/1:1 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 complying RX3non-timed queue now has 17 items -1711462090.584936 [0] recv: rexmit#1 maxseq:3<3<=3defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 - queue for transmit -1711462090.584940 [0] recv: non-timed queue now has 17 items -1711462090.584942 [0] recv: ) -1711462090.584940 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400bbf0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:3c2:#6/1)): niov 9 sz 1040 => now niov 11 sz 1340 -1711462090.584945 [0] recv: ACKNACK(F#1:1/0: 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:e3b81b8d:1ccb65b1:300c3 happy-now) -1711462090.584953 [0] recv: ACKNACK(F#1:1/0: 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:e3b81b8d:1ccb65b1:301c3 happy-now) -1711462090.584955 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.584958 [0] recv: ACKNACK(F#1:1/10:1111111111 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:79d6eeac:ea4a8907:3c2 complying RX1non-timed queue now has 17 items -1711462090.584960 [0] recv: M2 RX3non-timed queue now has 18 items -1711462090.584961 [0] tev: nn_xpack_send 1340: 0x7408c000139c:20 0x7408c400b6d8:52 0x7408d4029a84:172 0x7408c400cfe8:36 0x7408d4095d14:168 0x7408c400bb68:36 0x7408d409b634:264 0x7408c400b9e8:36 0x7408d405e424:256 0x7408c400bce8:36 0x7408d4063364:264 [ udp/10.101.12.71:7418@2 ] -1711462090.584962 [0] recv: RX4non-timed queue now has 19 items -1711462090.584965 [0] tev: traffic-xmit (1) 1340 -1711462090.584968 [0] recv: RX5non-timed queue now has 20 items -1711462090.584971 [0] recv: RX6non-timed queue now has 21 items -1711462090.584973 [0] recv: RX7non-timed queue now has 22 items -1711462090.584975 [0] recv: RX8non-timed queue now has 23 items -1711462090.584978 [0] recv: RX9non-timed queue now has 24 items -1711462090.584981 [0] recv: M10 FXGAP2..3/8:00000001non-timed queue now has 25 items -1711462090.584982 [0] recv: rexmit#9 maxseq:10<10<=10defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:3c2 -> 110aba1:7b19badd:ce0adb73:3c7 - queue for transmit -1711462090.584984 [0] recv: non-timed queue now has 26 items -1711462090.584983 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400bd70 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:3c2:#7/1)): niov 0 sz 0 => now niov 3 sz 328 -1711462090.584986 [0] recv: ) -1711462090.584991 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400c170 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:3c2:#8/1)): niov 3 sz 328 => now niov 5 sz 620 -1711462090.584994 [0] recv: ACKNACK(F#1:1/10:1111111111 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:79d6eeac:ea4a8907:4c2 complying RX1non-timed queue now has 26 items -1711462090.584997 [0] recv: RX2non-timed queue now has 27 items -1711462090.584999 [0] recv: RX3non-timed queue now has 28 items -1711462090.585001 [0] recv: RX4non-timed queue now has 29 items -1711462090.585004 [0] recv: RX5non-timed queue now has 30 items -1711462090.585006 [0] recv: RX6non-timed queue now has 31 items -1711462090.585010 [0] recv: RX7non-timed queue now has 32 items -1711462090.585012 [0] recv: RX8non-timed queue now has 33 items -1711462090.585014 [0] recv: RX9non-timed queue now has 34 items -1711462090.585016 [0] recv: RX10non-timed queue now has 35 items -1711462090.585018 [0] recv: rexmit#10 maxseq:10<10<=10defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:4c2 -> 110aba1:7b19badd:ce0adb73:4c7 - queue for transmit -1711462090.585020 [0] recv: non-timed queue now has 36 items -1711462090.585022 [0] recv: ) -1711462090.585023 [0] recv: send_deferred_heartbeat: e6906c1267fe410d -> 79bd04c9dfe259de - queue for transmit -1711462090.585025 [0] recv: non-timed queue now has 36 items -1711462090.585025 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400d1f0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:3c2:#9/1)): niov 5 sz 620 => now niov 7 sz 932 -1711462090.585028 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 68 from udp/10.101.12.71:52025 -1711462090.585033 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400d370 0(control): niov 7 sz 932 => now niov 8 sz 968 -1711462090.585034 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.585039 [0] tev: xpack_addmsg 0x7408c0001390 0x7408e000a1a0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#1/1)): niov 8 sz 968 => now niov 10 sz 1180 -1711462090.585042 [0] recv: ACKNACK(F#1:3/1:1 L(:1c1 16938.244539) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 complying RX3non-timed queue now has 35 items -1711462090.585044 [0] recv: rexmit#1 maxseq:3<3<=3defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:200c2 -> 110aba1:7b19badd:ce0adb73:200c7 - queue for transmit -1711462090.585046 [0] recv: ) -1711462090.585047 [0] recv: send_deferred_heartbeat: c4ced519f5211b97 -> 2e9a32ef85ddbc26 - queue for transmit -1711462090.585049 [0] recv: non-timed queue now has 35 items -1711462090.585051 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.585053 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.585055 [0] recv: ACKNACK(F#1:1/0: L(:1c1 16938.244557) 110aba1:7b19badd:ce0adb73:300c4 -> 110cd0d:79d6eeac:ea4a8907:300c3 happy-now) -1711462090.585058 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.585060 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.585055 [0] tev: nn_xpack_send 1180: 0x7408c000139c:20 0x7408c400be58:52 0x7408d4068954:256 0x7408c400c268:36 0x7408d4101cf4:256 0x7408c400d2e8:36 0x7408d4085c94:276 0x7408c400d468:36 0x7408e0009cf8:36 0x7408d402b444:176 [ udp/10.101.12.71:7418@2 ] -1711462090.585062 [0] recv: ACKNACK(F#1:1/0: L(:1c1 16938.244564) 110aba1:7b19badd:ce0adb73:301c4 -> 110cd0d:79d6eeac:ea4a8907:301c3 happy-now) -1711462090.585066 [0] tev: traffic-xmit (1) 1180 -1711462090.585069 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.585071 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.585073 [0] recv: ACKNACK(F#2:2/0: L(:1c1 16938.244575) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2 setting-has-replied-to-hb happy-now) -1711462090.585078 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400daf0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#2/1)): niov 0 sz 0 => now niov 3 sz 328 -1711462090.585083 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400cbf0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#3/1)): niov 3 sz 328 => now niov 5 sz 612 -1711462090.585088 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400c8f0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#4/1)): niov 5 sz 612 => now niov 7 sz 904 -1711462090.585092 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400cd70 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#5/1)): niov 7 sz 904 => now niov 9 sz 1188 -1711462090.585102 [0] tev: nn_xpack_send 1188: 0x7408c000139c:20 0x7408c400dbd8:52 0x7408d409cd74:256 0x7408c400cce8:36 0x7408d405f624:248 0x7408c400c9e8:36 0x7408d4064584:256 0x7408c400ce68:36 0x7408d40fadd4:248 [ udp/10.101.12.71:7418@2 ] -1711462090.585109 [0] tev: traffic-xmit (1) 1188 -1711462090.585110 [0] gc: gc 0x7408c8084400: deleting -1711462090.585114 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400d070 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#6/1)): niov 0 sz 0 => now niov 3 sz 320 -1711462090.585122 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e0f0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#7/1)): niov 3 sz 320 => now niov 5 sz 624 -1711462090.585126 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400d670 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#8/1)): niov 5 sz 624 => now niov 7 sz 800 -1711462090.585130 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e270 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#9/1)): niov 7 sz 800 => now niov 9 sz 968 -1711462090.585134 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400d7f0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:4c2:#10/1)): niov 9 sz 968 => now niov 11 sz 1136 -1711462090.585137 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ca70 0(control): niov 11 sz 1136 => now niov 12 sz 1168 -1711462090.585141 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400d4f0 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:200c2:#3/1)): niov 12 sz 1168 => now niov 14 sz 1232 -1711462090.585145 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400d970 0(control): niov 14 sz 1232 => now niov 15 sz 1264 -1711462090.585156 [0] tev: nn_xpack_send 1264: 0x7408c000139c:20 0x7408c400d158:52 0x7408d4103124:248 0x7408c400e1e8:36 0x7408d4087084:268 0x7408c400d768:36 0x7408d408f7c4:140 0x7408c400e368:36 0x7408d4091824:132 0x7408c400d8e8:36 0x7408d4093804:132 0x7408c400cb68:32 0x7408c400d5e8:36 0x7408c0002f6c:28 0x7408c400da68:32 [ udp/10.101.12.71:7418@2 ] -1711462090.585159 [0] tev: traffic-xmit (1) 1264 -1711462090.585163 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ddf0 0(rexmit(110cd0d:79d6eeac:ea4a8907:3c2:#1/1)): niov 0 sz 0 => now niov 3 sz 244 -1711462090.585167 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400dc70 0(rexmit(110cd0d:79d6eeac:ea4a8907:3c2:#3/1)): niov 3 sz 244 => now niov 5 sz 448 -1711462090.585171 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc005b30 0(rexmit(110cd0d:79d6eeac:ea4a8907:3c2:#4/1)): niov 5 sz 448 => now niov 7 sz 744 -1711462090.585175 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc003170 0(rexmit(110cd0d:79d6eeac:ea4a8907:3c2:#5/1)): niov 7 sz 744 => now niov 9 sz 1032 -1711462090.585179 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc002e70 0(rexmit(110cd0d:79d6eeac:ea4a8907:3c2:#6/1)): niov 9 sz 1032 => now niov 11 sz 1328 -1711462090.585189 [0] tev: nn_xpack_send 1328: 0x7408c000139c:20 0x7408c400ded8:52 0x7408e0019a34:172 0x7408c400dd68:36 0x7408e0021b34:168 0x7408bc005c28:36 0x7408e00143e4:260 0x7408bc005aa8:36 0x7408e0020064:252 0x7408bc002f68:36 0x7408e00296e4:260 [ udp/10.101.12.71:7418@2 ] -1711462090.585193 [0] tev: traffic-xmit (1) 1328 -1711462090.585196 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc002ff0 0(rexmit(110cd0d:79d6eeac:ea4a8907:3c2:#7/1)): niov 0 sz 0 => now niov 3 sz 324 -1711462090.585200 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400c5f0 0(rexmit(110cd0d:79d6eeac:ea4a8907:3c2:#8/1)): niov 3 sz 324 => now niov 5 sz 612 -1711462090.585203 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e870 0(rexmit(110cd0d:79d6eeac:ea4a8907:3c2:#9/1)): niov 5 sz 612 => now niov 7 sz 920 -1711462090.585207 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400c770 0(control): niov 7 sz 920 => now niov 8 sz 956 -1711462090.585210 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400df70 0(control): niov 8 sz 956 => now niov 10 sz 1012 -1711462090.585214 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e9f0 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#1/1)): niov 10 sz 1012 => now niov 13 sz 1248 -1711462090.585225 [0] tev: nn_xpack_send 1248: 0x7408c000139c:20 0x7408bc0030d8:52 0x7408e002dcd4:252 0x7408c400c6e8:36 0x7408e0033864:252 0x7408c400e968:36 0x7408e0037c74:272 0x7408c400c868:36 0x7408c400e040:24 0x7408c400e068:32 0x7408c400eac0:24 0x7408c400eae8:36 0x7408e001b744:176 [ udp/10.101.12.71:7418@2 ] -1711462090.585231 [0] tev: traffic-xmit (1) 1248 -1711462090.585236 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400eb70 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#2/1)): niov 0 sz 0 => now niov 3 sz 324 -1711462090.585240 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ecf0 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#3/1)): niov 3 sz 324 => now niov 5 sz 604 -1711462090.585244 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e3f0 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#4/1)): niov 5 sz 604 => now niov 7 sz 892 -1711462090.585248 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4011870 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#5/1)): niov 7 sz 892 => now niov 9 sz 1172 -1711462090.585252 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f170 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#6/1)): niov 9 sz 1172 => now niov 11 sz 1452 -1711462090.585256 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.585258 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.585260 [0] recv: ACKNACK(F#2:11/0: L(:1c1 16938.244762) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:e3b81b8d:1ccb65b1:3c2 setting-has-replied-to-hb happy-now) -1711462090.585263 [0] tev: nn_xpack_send 1452: 0x7408c000139c:20 0x7408c400ec58:52 0x7408e001fee4:252 0x7408c400ede8:36 0x7408e00030a4:244 0x7408c400e4e8:36 0x7408e002a714:252 0x7408c4011968:36 0x7408e002ed84:244 0x7408c400f268:36 0x7408e0034a24:244 [ udp/10.101.12.71:7418@2 ] -1711462090.585266 [0] tev: traffic-xmit (1) 1452 -1711462090.585270 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#7/1)): niov 0 sz 0 => now niov 3 sz 336 -1711462090.585274 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#8/1)): niov 3 sz 336 => now niov 5 sz 512 -1711462090.585278 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#9/1)): niov 5 sz 512 => now niov 7 sz 680 -1711462090.585282 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e6f0 0(rexmit(110cd0d:79d6eeac:ea4a8907:4c2:#10/1)): niov 7 sz 680 => now niov 9 sz 848 -1711462090.585286 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400eff0 0(control): niov 9 sz 848 => now niov 10 sz 880 -1711462090.585289 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f2f0 0(control): niov 10 sz 880 => now niov 11 sz 912 -1711462090.585290 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.585295 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.585293 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f470 0(rexmit(110cd0d:79d6eeac:ea4a8907:200c2:#3/1)): niov 11 sz 912 => now niov 13 sz 976 -1711462090.585297 [0] recv: ACKNACK(F#2:11/0: L(:1c1 16938.244799) 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:e3b81b8d:1ccb65b1:4c2 setting-has-replied-to-hb happy-now) -1711462090.585302 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 13 sz 976 => now niov 14 sz 1008 -1711462090.585316 [0] tev: nn_xpack_send 1008: 0x7408c000139c:20 0x7408c400f858:52 0x7408e0038d24:264 0x7408c400e668:36 0x7408e002c194:140 0x7408c400ef68:36 0x7408e003cc64:132 0x7408c400e7e8:36 0x7408e003ee54:132 0x7408c400f0e8:32 0x7408c400f3e8:32 0x7408c400f568:36 0x7408c800d1dc:28 0x7408c400f6e8:32 [ udp/10.101.12.71:7418@2 ] -1711462090.585319 [0] tev: traffic-xmit (1) 1008 -1711462090.585387 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.585390 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.585392 [0] recv: ACKNACK(F#2:4/0: L(:1c1 16938.244894) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 setting-has-replied-to-hb happy-now) -1711462090.585516 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 92 from udp/10.101.12.71:52025 -1711462090.585518 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.585522 [0] recv: ACKNACK(F#2:11/0: L(:1c1 16938.245022) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:79d6eeac:ea4a8907:3c2 setting-has-replied-to-hb happy-now) -1711462090.585524 [0] recv: ACKNACK(F#2:11/0: 110aba1:7b19badd:ce0adb73:4c7 -> 110cd0d:79d6eeac:ea4a8907:4c2 setting-has-replied-to-hb happy-now) -1711462090.585532 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.585534 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.585536 [0] recv: ACKNACK(F#2:4/0: L(:1c1 16938.245038) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 setting-has-replied-to-hb happy-now) -1711462090.588384 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 224 from udp/10.101.12.71:52025 -1711462090.588388 [0] recv: INFOTS(1711462090.588344046) -1711462090.588393 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #3 L(:1c1 16938.247891)) -1711462090.588409 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588431 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.588443 [0] dq.builtin: => EVERYONE -1711462090.588452 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:703) scanning all rds of topic rt/parameter_events -1711462090.588466 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 216 from udp/10.101.12.71:52025 -1711462090.588471 [0] recvMC: INFOTS(1711462090.588369545) -1711462090.588474 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #3 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.588476 [0] recvMC: HEARTBEAT(#4:3..3 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.589381 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 336 from udp/10.101.12.71:52025 -1711462090.589392 [0] recv: INFOTS(1711462090.589341876) -1711462090.589401 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #4 L(:1c1 16938.248895)) -1711462090.589416 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rr/topological_navigation_core/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589435 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:803 reliable volatile writer unnamed: (default).rr/topological_navigation_core/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rr/topological_navigation_core/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589450 [0] dq.builtin: => EVERYONE -1711462090.589459 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:803) scanning all rds of topic rr/topological_navigation_core/describe_parametersReply -1711462090.589474 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 332 from udp/10.101.12.71:52025 -1711462090.589480 [0] recv: INFOTS(1711462090.589420872) -1711462090.589489 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #2 L(:1c1 16938.248983)) -1711462090.589502 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rq/topological_navigation_core/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:904},adlink_entity_factory=1} -1711462090.589520 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:904 reliable volatile reader unnamed: (default).rq/topological_navigation_core/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rq/topological_navigation_core/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589538 [0] dq.builtin: => EVERYONE -1711462090.589540 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 264 from udp/10.101.12.71:52025 -1711462090.589550 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:904) scanning all wrs of topic rq/topological_navigation_core/describe_parametersRequest -1711462090.589551 [0] recvMC: INFOTS(1711462090.589459043) -1711462090.589561 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #4 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.589565 [0] recvMC: HEARTBEAT(F#7:4..4 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.589871 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 328 from udp/10.101.12.71:52025 -1711462090.589879 [0] recv: INFOTS(1711462090.589837664) -1711462090.589885 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #5 L(:1c1 16938.249382)) -1711462090.589899 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rr/topological_navigation_core/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589917 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:a03 reliable volatile writer unnamed: (default).rr/topological_navigation_core/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rr/topological_navigation_core/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.589938 [0] dq.builtin: => EVERYONE -1711462090.589939 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 324 from udp/10.101.12.71:52025 -1711462090.589948 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:a03) scanning all rds of topic rr/topological_navigation_core/get_parametersReply -1711462090.589950 [0] recv: INFOTS(1711462090.589906240) -1711462090.589961 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #3 L(:1c1 16938.249453)) -1711462090.589973 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rq/topological_navigation_core/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:b04},adlink_entity_factory=1} -1711462090.589990 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:b04 reliable volatile reader unnamed: (default).rq/topological_navigation_core/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rq/topological_navigation_core/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.589998 [0] dq.builtin: => EVERYONE -1711462090.590008 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 312 from udp/10.101.12.71:52025 -1711462090.590011 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:b04) scanning all wrs of topic rq/topological_navigation_core/get_parametersRequest -1711462090.590011 [0] recvMC: INFOTS(1711462090.589932268) -1711462090.590020 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #5 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590022 [0] recvMC: HEARTBEAT(F#8:5..5 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590284 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 336 from udp/10.101.12.71:52025 -1711462090.590291 [0] recv: INFOTS(1711462090.590253167) -1711462090.590298 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #6 L(:1c1 16938.249794)) -1711462090.590310 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rr/topological_navigation_core/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.590329 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:c03 reliable volatile writer unnamed: (default).rr/topological_navigation_core/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rr/topological_navigation_core/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.590338 [0] dq.builtin: => EVERYONE -1711462090.590346 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:c03) scanning all rds of topic rr/topological_navigation_core/get_parameter_typesReply -1711462090.590371 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 332 from udp/10.101.12.71:52025 -1711462090.590377 [0] recv: INFOTS(1711462090.590339038) -1711462090.590386 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #4 L(:1c1 16938.249880)) -1711462090.590397 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rq/topological_navigation_core/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:d04},adlink_entity_factory=1} -1711462090.590413 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:d04 reliable volatile reader unnamed: (default).rq/topological_navigation_core/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rq/topological_navigation_core/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.590422 [0] dq.builtin: => EVERYONE -1711462090.590434 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:d04) scanning all wrs of topic rq/topological_navigation_core/get_parameter_typesRequest -1711462090.590437 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 360 from udp/10.101.12.71:52025 -1711462090.590440 [0] recvMC: INFOTS(1711462090.590361377) -1711462090.590442 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #6 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590445 [0] recvMC: HEARTBEAT(F#9:6..6 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590766 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 328 from udp/10.101.12.71:52025 -1711462090.590774 [0] recv: INFOTS(1711462090.590733363) -1711462090.590781 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #7 L(:1c1 16938.250278)) -1711462090.590798 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rr/topological_navigation_core/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.590815 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:e03 reliable volatile writer unnamed: (default).rr/topological_navigation_core/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rr/topological_navigation_core/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.590825 [0] dq.builtin: => EVERYONE -1711462090.590833 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:e03) scanning all rds of topic rr/topological_navigation_core/list_parametersReply -1711462090.590839 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 324 from udp/10.101.12.71:52025 -1711462090.590845 [0] recv: INFOTS(1711462090.590809109) -1711462090.590851 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #5 L(:1c1 16938.250348)) -1711462090.590862 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rq/topological_navigation_core/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:f04},adlink_entity_factory=1} -1711462090.590878 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:f04 reliable volatile reader unnamed: (default).rq/topological_navigation_core/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rq/topological_navigation_core/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.590886 [0] dq.builtin: => EVERYONE -1711462090.590899 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:f04) scanning all wrs of topic rq/topological_navigation_core/list_parametersRequest -1711462090.590916 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 408 from udp/10.101.12.71:52025 -1711462090.590918 [0] recvMC: INFOTS(1711462090.590832188) -1711462090.590920 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #7 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.590923 [0] recvMC: HEARTBEAT(F#10:7..7 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591254 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 328 from udp/10.101.12.71:52025 -1711462090.591261 [0] recv: INFOTS(1711462090.591221553) -1711462090.591267 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #8 L(:1c1 16938.250764)) -1711462090.591280 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rr/topological_navigation_core/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591297 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1003 reliable volatile writer unnamed: (default).rr/topological_navigation_core/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rr/topological_navigation_core/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591306 [0] dq.builtin: => EVERYONE -1711462090.591314 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:1003) scanning all rds of topic rr/topological_navigation_core/set_parametersReply -1711462090.591340 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 324 from udp/10.101.12.71:52025 -1711462090.591346 [0] recv: INFOTS(1711462090.591307020) -1711462090.591352 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #6 L(:1c1 16938.250849)) -1711462090.591385 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rq/topological_navigation_core/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1104},adlink_entity_factory=1} -1711462090.591399 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1104 reliable volatile reader unnamed: (default).rq/topological_navigation_core/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rq/topological_navigation_core/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.591406 [0] dq.builtin: => EVERYONE -1711462090.591415 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 456 from udp/10.101.12.71:52025 -1711462090.591422 [0] recvMC: INFOTS(1711462090.591332052) -1711462090.591418 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:1104) scanning all wrs of topic rq/topological_navigation_core/set_parametersRequest -1711462090.591428 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #8 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591439 [0] recvMC: HEARTBEAT(F#11:8..8 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591650 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 352 from udp/10.101.12.71:52025 -1711462090.591653 [0] recv: INFOTS(1711462090.591617144) -1711462090.591659 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #9 L(:1c1 16938.251156)) -1711462090.591673 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rr/topological_navigation_core/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591693 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1203 reliable volatile writer unnamed: (default).rr/topological_navigation_core/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rr/topological_navigation_core/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591698 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 340 from udp/10.101.12.71:52025 -1711462090.591700 [0] dq.builtin: => EVERYONE -1711462090.591701 [0] recv: INFOTS(1711462090.591678024) -1711462090.591708 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #7 L(:1c1 16938.251205)) -1711462090.591709 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:1203) scanning all rds of topic rr/topological_navigation_core/set_parameters_atomicallyReply -1711462090.591718 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rq/topological_navigation_core/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1304},adlink_entity_factory=1} -1711462090.591729 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1304 reliable volatile reader unnamed: (default).rq/topological_navigation_core/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rq/topological_navigation_core/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.591743 [0] dq.builtin: => EVERYONE -1711462090.591747 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:1304) scanning all wrs of topic rq/topological_navigation_core/set_parameters_atomicallyRequest -1711462090.591774 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 504 from udp/10.101.12.71:52025 -1711462090.591778 [0] recvMC: INFOTS(1711462090.591692230) -1711462090.591781 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #9 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591786 [0] recvMC: HEARTBEAT(F#12:9..9 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.591919 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 316 from udp/10.101.12.71:52025 -1711462090.591922 [0] recv: INFOTS(1711462090.591900460) -1711462090.591925 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #10 L(:1c1 16938.251426)) -1711462090.591932 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #10: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rq/controller_server/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591941 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1403 reliable volatile writer unnamed: (default).rq/controller_server/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rq/controller_server/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.591946 [0] dq.builtin: => EVERYONE -1711462090.591951 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:1403) scanning all rds of topic rq/controller_server/set_parametersRequest -1711462090.591985 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 308 from udp/10.101.12.71:52025 -1711462090.591992 [0] recv: INFOTS(1711462090.591954823) -1711462090.591996 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #8 L(:1c1 16938.251496)) -1711462090.592004 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rr/controller_server/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1504},adlink_entity_factory=1} -1711462090.592012 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1504 reliable volatile reader unnamed: (default).rr/controller_server/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rr/controller_server/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.592022 [0] dq.builtin: => EVERYONE -1711462090.592026 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:1504) scanning all wrs of topic rr/controller_server/set_parametersReply -1711462090.592046 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 552 from udp/10.101.12.71:52025 -1711462090.592050 [0] recvMC: INFOTS(1711462090.591969903) -1711462090.592053 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #10 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592058 [0] recvMC: HEARTBEAT(F#13:10..10 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592175 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 316 from udp/10.101.12.71:52025 -1711462090.592178 [0] recv: INFOTS(1711462090.592154676) -1711462090.592183 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #11 L(:1c1 16938.251682)) -1711462090.592190 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #11: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rq/controller_server/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.592199 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1603 reliable volatile writer unnamed: (default).rq/controller_server/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rq/controller_server/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.592203 [0] dq.builtin: => EVERYONE -1711462090.592208 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:1603) scanning all rds of topic rq/controller_server/list_parametersRequest -1711462090.592227 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 308 from udp/10.101.12.71:52025 -1711462090.592230 [0] recv: INFOTS(1711462090.592204340) -1711462090.592235 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #9 L(:1c1 16938.251734)) -1711462090.592243 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #9: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rr/controller_server/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1704},adlink_entity_factory=1} -1711462090.592251 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1704 reliable volatile reader unnamed: (default).rr/controller_server/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rr/controller_server/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.592260 [0] dq.builtin: => EVERYONE -1711462090.592264 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:1704) scanning all wrs of topic rr/controller_server/list_parametersReply -1711462090.592290 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 600 from udp/10.101.12.71:52025 -1711462090.592295 [0] recvMC: INFOTS(1711462090.592217036) -1711462090.592300 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #11 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592304 [0] recvMC: HEARTBEAT(F#14:11..11 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592425 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 316 from udp/10.101.12.71:52025 -1711462090.592428 [0] recv: INFOTS(1711462090.592403130) -1711462090.592433 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #12 L(:1c1 16938.251932)) -1711462090.592440 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #12: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rq/controller_server/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.592450 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1803 reliable volatile writer unnamed: (default).rq/controller_server/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rq/controller_server/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.592454 [0] dq.builtin: => EVERYONE -1711462090.592459 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:1803) scanning all rds of topic rq/controller_server/get_parametersRequest -1711462090.592475 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 308 from udp/10.101.12.71:52025 -1711462090.592478 [0] recv: INFOTS(1711462090.592455025) -1711462090.592483 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #10 L(:1c1 16938.251982)) -1711462090.592490 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #10: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rr/controller_server/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1904},adlink_entity_factory=1} -1711462090.592559 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 648 from udp/10.101.12.71:52025 -1711462090.592568 [0] recvMC: INFOTS(1711462090.592467231) -1711462090.592563 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1904 reliable volatile reader unnamed: (default).rr/controller_server/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rr/controller_server/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.592586 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #12 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592596 [0] recvMC: HEARTBEAT(F#15:12..12 110aba1:7b19badd:ce0adb73:403? -> 0:0:0:0) -1711462090.592597 [0] dq.builtin: => EVERYONE -1711462090.592606 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:1904) scanning all wrs of topic rr/controller_server/get_parametersReply -1711462090.597380 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.597395 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.597403 [0] recvUC: ACKNACK(#0:1/0: 110aba1:7b19badd:ce0adb73:504? -> 110cd0d:56a27910:57318171:403) -1711462090.598416 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.598431 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.598438 [0] recvUC: ACKNACK(#0:1/0: 110aba1:7b19badd:ce0adb73:504? -> 110cd0d:e3b81b8d:1ccb65b1:403) -1711462090.598970 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.598995 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.599001 [0] recvUC: ACKNACK(#0:1/0: 110aba1:7b19badd:ce0adb73:504? -> 110cd0d:79d6eeac:ea4a8907:403) -1711462090.619793 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462090.619823 [0] recv: INFOTS(1711462090.519442682) -1711462090.619845 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462090.619865 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462090.619877 [0] recv: INFOTS(1711462090.519442682) -1711462090.619888 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462090.619940 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.619975 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16938.279476) -1711462090.619997 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.620019 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16938.279522) -1711462090.642272 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462090.642301 [0] recv: INFOTS(1711462090.541682763) -1711462090.642319 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462090.642337 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462090.642353 [0] recv: INFOTS(1711462090.541682763) -1711462090.642364 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462090.642387 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.642407 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16938.301909) -1711462090.642424 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.642431 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16938.301934) -1711462090.649692 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462090.649726 [0] recv: INFOTS(1711462090.549308855) -1711462090.649746 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462090.649758 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462090.649767 [0] recv: INFOTS(1711462090.549308855) -1711462090.649778 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462090.649807 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.649818 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16938.309321) -1711462090.649839 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.649854 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16938.309357) -1711462090.653403 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:403) suppressed, resched in inf s (min-ack 12, avail-seq 12, xmit 12) -1711462090.653437 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:403) suppressed, resched in inf s (min-ack 13, avail-seq 13, xmit 13) -1711462090.653584 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:403) suppressed, resched in inf s (min-ack 13, avail-seq 13, xmit 13) -1711462090.670768 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462090.670793 [0] recv: INFOTS(1711462090.570393375) -1711462090.670818 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462090.670831 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462090.670839 [0] recv: INFOTS(1711462090.570393375) -1711462090.670845 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.670860 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (known) L(:1c1 16938.330362) -1711462090.670848 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462090.670874 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462090.670882 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (known) L(:1c1 16938.330385) -1711462090.670875 [0] recv: thread_cputime 0.046884000 -1711462090.671961 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 84 from udp/10.101.12.71:58212 -1711462090.671977 [0] tev: thread_cputime 0.010776000 -1711462090.672006 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:3c2) suppressed, resched in inf s (min-ack 9, avail-seq 9, xmit 9) -1711462090.672010 [0] recv: HEARTBEAT(#9:1..10 L(:1c1 16938.331485)110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@10(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@10(sync) 110cd0d:79d6eeac:ea4a8907:3c7@10(sync)) -1711462090.672023 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:3c2) suppressed, resched in inf s (min-ack 10, avail-seq 9, xmit 10) -1711462090.672067 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#3:11/0: -1711462090.672075 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462090.672089 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672100 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#13:11/0: -1711462090.672107 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462090.672042 [0] recv: HEARTBEAT(#9:1..8 110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@8(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@8(sync) 110cd0d:79d6eeac:ea4a8907:4c7@8(sync)) -1711462090.672130 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.672168 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 68 from udp/10.101.12.71:58212 -1711462090.672183 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.672171 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#7:11/0: -1711462090.672217 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462090.672229 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.672203 [0] recv: HEARTBEAT(#9:1..1 L(:1c1 16938.331684)110e21c:d0762c48:a9f0fb28:200c2 -> 110cd0d:79d6eeac:ea4a8907:0: 110cd0d:79d6eeac:ea4a8907:200c7@1(sync)) -1711462090.672243 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:3c2) suppressed, resched in inf s (min-ack 10, avail-seq 9, xmit 10) -1711462090.672254 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#6:9/0: -1711462090.672263 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672264 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462090.672293 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e6f0 0(control): niov 6 sz 168 => now niov 7 sz 196 -1711462090.672285 [0] recv: HEARTBEAT(#7:1..10 L(:1c1 16938.331779)110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@10(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@10(sync) 110cd0d:79d6eeac:ea4a8907:3c7@10(sync)) -1711462090.672327 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462090.672346 [0] recv: HEARTBEAT(#5:1..0 L(:1c1 16938.331842)110e21c:d0762c48:a9f0fb28:300c3 -> 0:0:0:0: 110cd0d:56a27910:57318171:300c4@0(sync) 110cd0d:e3b81b8d:1ccb65b1:300c4@0(sync) 110cd0d:79d6eeac:ea4a8907:300c4@0(sync)) -1711462090.672305 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#3:9/0: -1711462090.672360 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672373 [0] recv: HEARTBEAT(#7:1..7 L(:1c1 16938.331871)110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@7(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@7(sync) 110cd0d:79d6eeac:ea4a8907:4c7@7(sync)) -1711462090.672388 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462090.672407 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462090.672425 [0] recv: HEARTBEAT(#8:1..42 L(:1c1 16938.331920)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@42(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@42(sync) 110cd0d:79d6eeac:ea4a8907:3c7@42(sync)) -1711462090.672451 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400eff0 0(control): niov 7 sz 196 => now niov 9 sz 248 -1711462090.672482 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#11:9/0: -1711462090.672487 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462090.672516 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f2f0 0(control): niov 9 sz 248 => now niov 11 sz 300 -1711462090.672528 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:4c2) suppressed, resched in inf s (min-ack 10, avail-seq 10, xmit 10) -1711462090.672516 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462090.672538 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#6:2/0: -1711462090.672554 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462090.672558 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f470 0(control): niov 11 sz 300 => now niov 13 sz 352 -1711462090.672564 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#7:11/0: -1711462090.672568 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462090.672588 [0] recv: HEARTBEAT(#5:1..0 L(:1c1 16938.332051)110e21c:d0762c48:a9f0fb28:301c3 -> 0:0:0:0: 110cd0d:56a27910:57318171:301c4@0(sync) 110cd0d:e3b81b8d:1ccb65b1:301c4@0(sync) 110cd0d:79d6eeac:ea4a8907:301c4@0(sync)) -1711462090.672607 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672616 [0] recv: HEARTBEAT(#7:1..1 L(:1c1 16938.332115)110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@1(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@1(sync) 110cd0d:79d6eeac:ea4a8907:200c7@1(sync)) -1711462090.672623 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462090.672634 [0] recv: HEARTBEAT(#8:1..40 L(:1c1 16938.332134)110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@40(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@40(sync) 110cd0d:79d6eeac:ea4a8907:4c7@40(sync)) -1711462090.672640 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672651 [0] recv: HEARTBEAT(#3:1..0 L(:1c1 16938.332149)110443d:bb7f10a5:18901533:300c3 -> 0:0:0:0: 110cd0d:56a27910:57318171:300c4@0(sync) 110cd0d:e3b81b8d:1ccb65b1:300c4@0(sync) 110cd0d:79d6eeac:ea4a8907:300c4@0(sync)) -1711462090.672593 [0] tev: nn_xpack_send 352: 0x7408c000139c:20 0x7408c400f858:44 0x7408c400e640:24 0x7408c400e668:28 0x7408c400ef40:24 0x7408c400ef68:28 0x7408c400e7e8:28 0x7408c400f0c0:24 0x7408c400f0e8:28 0x7408c400f3c0:24 0x7408c400f3e8:28 0x7408c400f540:24 0x7408c400f568:28 [ udp/10.101.12.71:7412@2 ] -1711462090.672683 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.672708 [0] tev: traffic-xmit (1) 352 -1711462090.672723 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672740 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#3:11/0: -1711462090.672748 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462090.672756 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.672714 [0] recv: HEARTBEAT(#3:1..0 L(:1c1 16938.332214)110443d:bb7f10a5:18901533:301c3 -> 0:0:0:0: 110cd0d:56a27910:57318171:301c4@0(sync) 110cd0d:e3b81b8d:1ccb65b1:301c4@0(sync) 110cd0d:79d6eeac:ea4a8907:301c4@0(sync)) -1711462090.672769 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#13:11/0: -1711462090.672792 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462090.672799 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.672812 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:4c2) suppressed, resched in inf s (min-ack 10, avail-seq 10, xmit 10) -1711462090.672827 [0] tev: acknack 110cd0d:56a27910:57318171:300c4 -> 110e21c:d0762c48:a9f0fb28:300c3: F#2:1/0: -1711462090.672845 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:300c4 -> pwr 110e21c:d0762c48:a9f0fb28:300c3) -1711462090.672880 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400f6d8:44 0x7408c400f840:24 0x7408c400f868:28 0x7408c400e640:24 0x7408c400e668:28 [ udp/10.101.12.71:7416@2 ] -1711462090.672890 [0] tev: traffic-xmit (1) 168 -1711462090.672898 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.672908 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:300c4 -> 110e21c:d0762c48:a9f0fb28:300c3: F#6:1/0: -1711462090.672915 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:300c4 -> pwr 110e21c:d0762c48:a9f0fb28:300c3) -1711462090.672922 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.672931 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:300c4 -> 110e21c:d0762c48:a9f0fb28:300c3: F#4:1/0: -1711462090.672937 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:300c4 -> pwr 110e21c:d0762c48:a9f0fb28:300c3) -1711462090.672945 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.672955 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#3:8/0: -1711462090.672961 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462090.672979 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400ef58:44 0x7408c400f6c0:24 0x7408c400f6e8:28 0x7408c400f840:24 0x7408c400f868:28 [ udp/10.101.12.71:7412@2 ] -1711462090.672987 [0] tev: traffic-xmit (1) 168 -1711462090.672994 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.673002 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#11:8/0: -1711462090.673009 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462090.673016 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.673026 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#6:8/0: -1711462090.673032 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462090.673038 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.673049 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:4c2) suppressed, resched in inf s (min-ack 10, avail-seq 10, xmit 10) -1711462090.673058 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#3:43/0: -1711462090.673065 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462090.673083 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400e658:44 0x7408c400ef40:24 0x7408c400ef68:28 0x7408c400f6c0:24 0x7408c400f6e8:28 [ udp/10.101.12.71:7416@2 ] -1711462090.673090 [0] tev: traffic-xmit (1) 168 -1711462090.673097 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.673106 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#13:43/0: -1711462090.673113 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462090.673119 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.673127 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#7:43/0: -1711462090.673134 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462090.673142 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.673151 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:301c4 -> 110e21c:d0762c48:a9f0fb28:301c3: F#6:1/0: -1711462090.673164 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:301c4 -> pwr 110e21c:d0762c48:a9f0fb28:301c3) -1711462090.673182 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400f858:44 0x7408c400e640:24 0x7408c400e668:28 0x7408c400ef40:24 0x7408c400ef68:28 [ udp/10.101.12.71:7414@2 ] -1711462090.673189 [0] tev: traffic-xmit (1) 168 -1711462090.673195 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.673204 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:301c4 -> 110e21c:d0762c48:a9f0fb28:301c3: F#4:1/0: -1711462090.673210 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:301c4 -> pwr 110e21c:d0762c48:a9f0fb28:301c3) -1711462090.673216 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.673225 [0] tev: acknack 110cd0d:56a27910:57318171:301c4 -> 110e21c:d0762c48:a9f0fb28:301c3: F#2:1/0: -1711462090.673231 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:301c4 -> pwr 110e21c:d0762c48:a9f0fb28:301c3) -1711462090.673237 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.673248 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#8:2/0: -1711462090.673255 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462090.673271 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400f6d8:44 0x7408c400f840:24 0x7408c400f868:28 0x7408c400e640:24 0x7408c400e668:28 [ udp/10.101.12.71:7412@2 ] -1711462090.673277 [0] tev: traffic-xmit (1) 168 -1711462090.673285 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.673293 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#6:2/0: -1711462090.673300 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462090.673307 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.673315 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#3:2/0: -1711462090.673321 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462090.673328 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.673338 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#3:41/0: -1711462090.673344 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462090.673359 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400ef58:44 0x7408c400f6c0:24 0x7408c400f6e8:28 0x7408c400f840:24 0x7408c400f868:28 [ udp/10.101.12.71:7416@2 ] -1711462090.673367 [0] tev: traffic-xmit (1) 168 -1711462090.673374 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.673382 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#6:41/0: -1711462090.673388 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462090.673396 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.673405 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#11:41/0: -1711462090.673412 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462090.673419 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.673428 [0] tev: acknack 110cd0d:56a27910:57318171:300c4 -> 110443d:bb7f10a5:18901533:300c3: F#2:1/0: -1711462090.673434 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:300c4 -> pwr 110443d:bb7f10a5:18901533:300c3) -1711462090.673459 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400e658:44 0x7408c400ef40:24 0x7408c400ef68:28 0x7408c400f6c0:24 0x7408c400f6e8:28 [ udp/10.101.12.71:7414@2 ] -1711462090.673466 [0] tev: traffic-xmit (1) 168 -1711462090.673473 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.673481 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:300c4 -> 110443d:bb7f10a5:18901533:300c3: F#6:1/0: -1711462090.673488 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:300c4 -> pwr 110443d:bb7f10a5:18901533:300c3) -1711462090.673495 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.673503 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:300c4 -> 110443d:bb7f10a5:18901533:300c3: F#4:1/0: -1711462090.673508 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:300c4 -> pwr 110443d:bb7f10a5:18901533:300c3) -1711462090.673514 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.673522 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:200c2) suppressed, resched in inf s (min-ack 1, avail-seq 1, xmit 1) -1711462090.673531 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:200c2) suppressed, resched in inf s (min-ack 3, avail-seq 3, xmit 3) -1711462090.673540 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:301c4 -> 110443d:bb7f10a5:18901533:301c3: F#4:1/0: -1711462090.673549 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:301c4 -> pwr 110443d:bb7f10a5:18901533:301c3) -1711462090.673555 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 6 sz 168 => now niov 7 sz 196 -1711462090.673564 [0] tev: acknack 110cd0d:56a27910:57318171:301c4 -> 110443d:bb7f10a5:18901533:301c3: F#2:1/0: -1711462090.673569 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:301c4 -> pwr 110443d:bb7f10a5:18901533:301c3) -1711462090.673575 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e6f0 0(control): niov 7 sz 196 => now niov 9 sz 248 -1711462090.673582 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:301c4 -> 110443d:bb7f10a5:18901533:301c3: F#6:1/0: -1711462090.673588 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:301c4 -> pwr 110443d:bb7f10a5:18901533:301c3) -1711462090.673593 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400eff0 0(control): niov 9 sz 248 => now niov 11 sz 300 -1711462090.673601 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:200c2) suppressed, resched in inf s (min-ack 3, avail-seq 3, xmit 3) -1711462090.673610 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:300c3) suppressed, resched in inf s (min-ack 0, avail-seq 0, xmit 0) -1711462090.673618 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:300c3) suppressed, resched in inf s (min-ack 0, avail-seq 0, xmit 0) -1711462090.673625 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:300c3) suppressed, resched in inf s (min-ack 0, avail-seq 0, xmit 0) -1711462090.673633 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:301c3) suppressed, resched in inf s (min-ack 0, avail-seq 0, xmit 0) -1711462090.673640 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:301c3) suppressed, resched in inf s (min-ack 0, avail-seq 0, xmit 0) -1711462090.673661 [0] tev: nn_xpack_send 300: 0x7408c000139c:20 0x7408c400f858:44 0x7408c400e640:24 0x7408c400e668:28 0x7408c400ef40:24 0x7408c400ef68:28 0x7408c400f6e8:28 0x7408c400e7c0:24 0x7408c400e7e8:28 0x7408c400f0c0:24 0x7408c400f0e8:28 [ udp/10.101.12.71:7416@2 ] -1711462090.673668 [0] tev: traffic-xmit (1) 300 -1711462090.673679 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:301c3) suppressed, resched in inf s (min-ack 0, avail-seq 0, xmit 0) -1711462090.674357 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 116 from udp/10.101.12.71:52025 -1711462090.674378 [0] recv: HEARTBEAT(#5:1..12 L(:1c1 16938.333868)110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@0 110cd0d:e3b81b8d:1ccb65b1:3c7@12(sync) 110cd0d:79d6eeac:ea4a8907:3c7@12(sync)) -1711462090.674389 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462090.674407 [0] recv: HEARTBEAT(#5:1..10 110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@0 110cd0d:e3b81b8d:1ccb65b1:4c7@10(sync) 110cd0d:79d6eeac:ea4a8907:4c7@10(sync)) -1711462090.674461 [0] recv: HEARTBEAT(#5:1..1 110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@1(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@1(sync) 110cd0d:79d6eeac:ea4a8907:200c7@1(sync)) -1711462090.674461 [0] recvMC: HEARTBEAT(#18:11..11 L(:1c1 16938.333937)110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@11(sync) 110cd0d:e3b81b8d:1ccb65b1:504@11(sync) 110cd0d:79d6eeac:ea4a8907:504@11(sync)) -1711462090.674485 [0] recvMC: thread_cputime 0.010238000 -1711462090.674502 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 84 from udp/10.101.12.71:52025 -1711462090.674469 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#2:1/12:110000000000 -1711462090.674521 [0] recv: HEARTBEAT(#2:1..0 L(:1c1 16938.334017)110aba1:7b19badd:ce0adb73:300c3 -> 0:0:0:0: 110cd0d:56a27910:57318171:300c4@0(sync) 110cd0d:e3b81b8d:1ccb65b1:300c4@0(sync) 110cd0d:79d6eeac:ea4a8907:300c4@0(sync)) -1711462090.674545 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462090.674557 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.674566 [0] recv: HEARTBEAT(#2:1..0 110aba1:7b19badd:ce0adb73:301c3 -> 0:0:0:0: 110cd0d:56a27910:57318171:301c4@0(sync) 110cd0d:e3b81b8d:1ccb65b1:301c4@0(sync) 110cd0d:79d6eeac:ea4a8907:301c4@0(sync)) -1711462090.674505 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462090.674607 [0] recvMC: HEARTBEAT(#18:10..10 L(:1c1 16938.334102)110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@10(sync) 110cd0d:e3b81b8d:1ccb65b1:504@10(sync) 110cd0d:79d6eeac:ea4a8907:504@10(sync)) -1711462090.674574 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#13:13/0: -1711462090.674672 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462090.674684 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 2 sz 68 => now niov 4 sz 120 -1711462090.674699 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#7:13/0: -1711462090.674707 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462090.674718 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 4 sz 120 => now niov 6 sz 172 -1711462090.674732 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#2:1/10:1000000000 -1711462090.674740 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462090.674749 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 6 sz 172 => now niov 8 sz 228 -1711462090.674761 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#11:11/0: -1711462090.674808 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462090.674816 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e6f0 0(control): niov 8 sz 228 => now niov 10 sz 280 -1711462090.674831 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#6:11/0: -1711462090.674839 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462090.674848 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400eff0 0(control): niov 10 sz 280 => now niov 12 sz 332 -1711462090.674858 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110e21c:d0762c48:a9f0fb28:403: F#4:12/0: -1711462090.674876 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462090.674919 [0] tev: nn_xpack_send 332: 0x7408c000139c:20 0x7408c400f858:48 0x7408c400e640:24 0x7408c400e668:28 0x7408c400ef40:24 0x7408c400ef68:28 0x7408c400f6c0:24 0x7408c400f6e8:32 0x7408c400e7c0:24 0x7408c400e7e8:28 0x7408c400f0c0:24 0x7408c400f0e8:28 [ udp/10.101.12.71:7418@2 ] -1711462090.674930 [0] tev: traffic-xmit (1) 332 -1711462090.674939 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f2f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.674950 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110e21c:d0762c48:a9f0fb28:403: F#18:12/0: -1711462090.674956 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462090.674964 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.674974 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110e21c:d0762c48:a9f0fb28:403: F#9:12/0: -1711462090.674980 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462090.674987 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.674996 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#2:2/0: -1711462090.675002 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462090.675022 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400f3d8:44 0x7408c400f840:24 0x7408c400f868:28 0x7408c400e640:24 0x7408c400e668:28 [ udp/10.101.12.71:7413@2 ] -1711462090.675030 [0] tev: traffic-xmit (1) 168 -1711462090.675038 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.675048 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#8:2/0: -1711462090.675055 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462090.675062 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f2f0 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.675072 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#6:2/0: -1711462090.675078 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462090.675085 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.675087 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 480 from udp/10.101.12.71:52025 -1711462090.675107 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.675116 [0] recv: INFOTS(1711462090.570840580) -1711462090.675096 [0] tev: acknack 110cd0d:56a27910:57318171:300c4 -> 110aba1:7b19badd:ce0adb73:300c3: F#2:1/0: -1711462090.675135 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:300c4 -> pwr 110aba1:7b19badd:ce0adb73:300c3) -1711462090.675141 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e570 0(control): niov 6 sz 168 => now niov 8 sz 220 -1711462090.675153 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:300c4 -> 110aba1:7b19badd:ce0adb73:300c3: F#6:1/0: -1711462090.675160 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:300c4 -> pwr 110aba1:7b19badd:ce0adb73:300c3) -1711462090.675143 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 110cd0d:56a27910:57318171:3c7 #1 L(:1c1 16938.334609)) -1711462090.675178 [0] recv: INFOTS(1711462090.571289169) -1711462090.675191 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 110cd0d:56a27910:57318171:3c7 #2 msr_in_sync(110cd0d:56a27910:57318171:3c7 out-of-sync to tlcatchup)) -1711462090.675205 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #1: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675206 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 280 from udp/10.101.12.71:52025 -1711462090.675238 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.675246 [0] recv: INFOTS(1711462090.570915636) -1711462090.675257 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 110cd0d:56a27910:57318171:4c7 #1 L(:1c1 16938.334740) msr_in_sync(110cd0d:56a27910:57318171:4c7 out-of-sync to tlcatchup)) -1711462090.675263 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:403 reliable transient-local writer unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675269 [0] recv: HEARTBEAT(#6:1..12 110aba1:7b19badd:ce0adb73:3c2 -> 110cd0d:56a27910:57318171:3c7: 110cd0d:56a27910:57318171:3c7@12(sync)) -1711462090.675296 [0] dq.builtin: => EVERYONE -1711462090.675311 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 68 from udp/10.101.12.71:52025 -1711462090.675339 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462090.675346 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:403) scanning all rds of topic ros_discovery_info -1711462090.675351 [0] recv: HEARTBEAT(#6:1..10 L(:1c1 16938.334840)110aba1:7b19badd:ce0adb73:4c2 -> 110cd0d:56a27910:57318171:4c7: 110cd0d:56a27910:57318171:4c7@10(sync)) -1711462090.675365 [0] dq.builtin: reader 110cd0d:56a27910:57318171:504 init_acknack_count = 1 -1711462090.675374 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:403 rd 110cd0d:56a27910:57318171:504) -1711462090.675387 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:403 rd 110cd0d:56a27910:57318171:504) - out-of-sync -1711462090.675395 [0] dq.builtin: reader 110cd0d:e3b81b8d:1ccb65b1:504 init_acknack_count = 17 -1711462090.675401 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:403 rd 110cd0d:e3b81b8d:1ccb65b1:504) -1711462090.675409 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:403 rd 110cd0d:e3b81b8d:1ccb65b1:504) - out-of-sync -1711462090.675418 [0] dq.builtin: reader 110cd0d:79d6eeac:ea4a8907:504 init_acknack_count = 8 -1711462090.675425 [0] dq.builtin: reader_add_connection(pwr 110aba1:7b19badd:ce0adb73:403 rd 110cd0d:79d6eeac:ea4a8907:504) -1711462090.675167 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f5f0 0(control): niov 8 sz 220 => now niov 10 sz 272 -1711462090.675452 [0] dq.builtin: proxy_writer_add_connection(pwr 110aba1:7b19badd:ce0adb73:403 rd 110cd0d:79d6eeac:ea4a8907:504) - out-of-sync -1711462090.675458 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:300c4 -> 110aba1:7b19badd:ce0adb73:300c3: F#4:1/0: -1711462090.675466 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:300c4 -> pwr 110aba1:7b19badd:ce0adb73:300c3) -1711462090.675472 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400e6f0 0(control): niov 10 sz 272 => now niov 12 sz 324 -1711462090.675477 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #2: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",durability=1,durability_service=0:0:1000:-1:-1:-1,reliability=1:9223372036854775807,lifespan=10000000000,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675481 [0] tev: acknack 110cd0d:56a27910:57318171:301c4 -> 110aba1:7b19badd:ce0adb73:301c3: F#2:1/0: -1711462090.675505 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:301c4 -> pwr 110aba1:7b19badd:ce0adb73:301c3) -1711462090.675513 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400eff0 0(control): niov 12 sz 324 => now niov 14 sz 376 -1711462090.675517 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:603 reliable transient-local writer unnamed: (default).rt/rosout/rcl_interfaces::msg::dds_::Log_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=0<>,topic_name="rt/rosout",type_name="rcl_interfaces::msg::dds_::Log_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:1000:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=10000000000,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675521 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:301c4 -> 110aba1:7b19badd:ce0adb73:301c3: F#6:1/0: -1711462090.675529 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:301c4 -> pwr 110aba1:7b19badd:ce0adb73:301c3) -1711462090.675535 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f470 0(control): niov 14 sz 376 => now niov 16 sz 428 -1711462090.675539 [0] dq.builtin: => EVERYONE -1711462090.675543 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:301c4 -> 110aba1:7b19badd:ce0adb73:301c3: F#4:1/0: -1711462090.675551 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:301c4 -> pwr 110aba1:7b19badd:ce0adb73:301c3) -1711462090.675557 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110aba1:7b19badd:ce0adb73:603) scanning all rds of topic rt/rosout -1711462090.675557 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400eb70 0(control): niov 16 sz 428 => now niov 18 sz 480 -1711462090.675574 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #3: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675578 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110443d:bb7f10a5:18901533:403: F#3:11/0: -1711462090.675588 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.675583 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:703 reliable volatile writer unnamed: (default).rt/parameter_events/rcl_interfaces::msg::dds_::ParameterEvent_ p(open) known -1711462090.675615 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=0<>,topic_name="rt/parameter_events",type_name="rcl_interfaces::msg::dds_::ParameterEvent_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675619 [0] tev: nn_xpack_send 480: 0x7408c000139c:20 0x7408c400ef58:44 0x7408c400f3c0:24 0x7408c400f3e8:28 0x7408c400f840:24 0x7408c400f868:28 0x7408c400e640:24 0x7408c400e668:28 0x7408c400f6c0:24 0x7408c400f6e8:28 0x7408c400e7c0:24 0x7408c400e7e8:28 0x7408c400f0c0:24 0x7408c400f0e8:28 0x7408c400f540:24 0x7408c400f568:28 0x7408c400ec40:24 0x7408c400ec68:28 [ udp/10.101.12.71:7418@2 ] -1711462090.675638 [0] tev: traffic-xmit (1) 480 -1711462090.675631 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #4: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rr/topological_navigation_core/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675658 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:803 reliable volatile writer unnamed: (default).rr/topological_navigation_core/describe_parametersReply/rcl_interfaces::srv::dds_::DescribeParameters_Response_ p(open) known -1711462090.675647 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ecf0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.675676 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rr/topological_navigation_core/describe_parametersReply",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675685 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110443d:bb7f10a5:18901533:403: F#19:11/0: -1711462090.675696 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #5: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rr/topological_navigation_core/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:a03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675704 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.675716 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:a03 reliable volatile writer unnamed: (default).rr/topological_navigation_core/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) known -1711462090.675719 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ee70 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.675734 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rr/topological_navigation_core/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675738 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110443d:bb7f10a5:18901533:403: F#10:11/0: -1711462090.675746 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #6: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rr/topological_navigation_core/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:c03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675747 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462090.675759 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:c03 reliable volatile writer unnamed: (default).rr/topological_navigation_core/get_parameter_typesReply/rcl_interfaces::srv::dds_::GetParameterTypes_Response_ p(open) known -1711462090.675773 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f2f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.675784 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#3:13/0: -1711462090.675792 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462090.675786 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rr/topological_navigation_core/get_parameter_typesReply",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675813 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #7: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rr/topological_navigation_core/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:e03},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675822 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:e03 reliable volatile writer unnamed: (default).rr/topological_navigation_core/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) known -1711462090.675815 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c400edd8:44 0x7408c400ef40:24 0x7408c400ef68:28 0x7408c400f3c0:24 0x7408c400f3e8:28 [ udp/10.101.12.71:7417@2 ] -1711462090.675838 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rr/topological_navigation_core/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675839 [0] tev: traffic-xmit (1) 168 -1711462090.675850 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #8: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rr/topological_navigation_core/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1003},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675867 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1003 reliable volatile writer unnamed: (default).rr/topological_navigation_core/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) known -1711462090.675858 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.675894 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rr/topological_navigation_core/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675898 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#3:11/0: -1711462090.675907 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462090.675913 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ecf0 0(control): niov 2 sz 64 => now niov 3 sz 92 -1711462090.675922 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #9: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rr/topological_navigation_core/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1203},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675930 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1203 reliable volatile writer unnamed: (default).rr/topological_navigation_core/set_parameters_atomicallyReply/rcl_interfaces::srv::dds_::SetParametersAtomically_Response_ p(open) known -1711462090.675930 [0] tev: nn_xpack_send 92: 0x7408c000139c:20 0x7408c400f858:44 0x7408c400ede8:28 [ udp/10.101.12.71:7418@2 ] -1711462090.675956 [0] tev: traffic-xmit (1) 92 -1711462090.675947 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rr/topological_navigation_core/set_parameters_atomicallyReply",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Response_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675977 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #10: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rq/controller_server/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1403},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.675984 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1403 reliable volatile writer unnamed: (default).rq/controller_server/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) known -1711462090.675999 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rq/controller_server/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.676012 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #11: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rq/controller_server/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.676018 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1603 reliable volatile writer unnamed: (default).rq/controller_server/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) known -1711462090.676033 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rq/controller_server/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.676043 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #12: ST0 DCPSPublication/PublicationBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rq/controller_server/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.676049 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1803 reliable volatile writer unnamed: (default).rq/controller_server/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) known -1711462090.676063 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rq/controller_server/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462090.676073 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #1: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",durability=1,reliability=1:9223372036854775807,history=1:-1,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:504},adlink_entity_factory=1} -1711462090.676094 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:504 reliable transient-local reader unnamed: (default).ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=0<>,topic_name="ros_discovery_info",type_name="rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_",topic_data=0<>,group_data=0<>,durability=1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=1:-1,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676109 [0] dq.builtin: => EVERYONE -1711462090.676122 [0] dq.builtin: match_proxy_reader_with_writers(prd 110aba1:7b19badd:ce0adb73:504) scanning all wrs of topic ros_discovery_info -1711462090.676132 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:56a27910:57318171:403 prd 110aba1:7b19badd:ce0adb73:504) -1711462090.676138 [0] dq.builtin: writer_add_connection(wr 110cd0d:56a27910:57318171:403 prd 110aba1:7b19badd:ce0adb73:504) - ack seq 12 -1711462090.676151 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 udp/10.101.12.71:7417@2 udp/10.101.12.71:7419@2 -1711462090.676157 [0] dq.builtin: reduced nlocs=5 -1711462090.676163 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676168 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462090.676173 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676176 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676181 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7417@2 2 -> 0 -1711462090.676185 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676188 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676193 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7419@2 3 -> 0 -1711462090.676197 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676201 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676205 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462090.676209 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676215 [0] dq.builtin: a b c d -1711462090.676222 [0] dq.builtin: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. .. .. } -1711462090.676227 [0] dq.builtin: loc 1 = udp/10.101.12.71:7415@2 1 { .. .. .. +u } -1711462090.676232 [0] dq.builtin: loc 2 = udp/10.101.12.71:7417@2 1 { .. +u .. .. } -1711462090.676238 [0] dq.builtin: loc 3 = udp/10.101.12.71:7419@2 1 { .. .. +u .. } -1711462090.676244 [0] dq.builtin: loc 4 = udp/239.255.0.1:7401@2 -1 { +1 +1 +1 +1 } -1711462090.676248 [0] dq.builtin: best = 4 -1711462090.676252 [0] dq.builtin: simple udp/239.255.0.1:7401@2 -1711462090.676259 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462090.676268 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:403 prd 110aba1:7b19badd:ce0adb73:504) -1711462090.676272 [0] dq.builtin: writer_add_connection(wr 110cd0d:e3b81b8d:1ccb65b1:403 prd 110aba1:7b19badd:ce0adb73:504) - ack seq 13 -1711462090.676281 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 udp/10.101.12.71:7417@2 udp/10.101.12.71:7419@2 -1711462090.676286 [0] dq.builtin: reduced nlocs=5 -1711462090.676290 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676294 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462090.676298 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676302 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676306 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7417@2 2 -> 0 -1711462090.676310 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676317 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676321 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7419@2 3 -> 0 -1711462090.676325 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676329 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676333 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462090.676337 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676343 [0] dq.builtin: a b c d -1711462090.676348 [0] dq.builtin: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. .. .. } -1711462090.676353 [0] dq.builtin: loc 1 = udp/10.101.12.71:7415@2 1 { .. .. .. +u } -1711462090.676358 [0] dq.builtin: loc 2 = udp/10.101.12.71:7417@2 1 { .. +u .. .. } -1711462090.676363 [0] dq.builtin: loc 3 = udp/10.101.12.71:7419@2 1 { .. .. +u .. } -1711462090.676368 [0] dq.builtin: loc 4 = udp/239.255.0.1:7401@2 -1 { +1 +1 +1 +1 } -1711462090.676372 [0] dq.builtin: best = 4 -1711462090.676375 [0] dq.builtin: simple udp/239.255.0.1:7401@2 -1711462090.676381 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462090.676387 [0] dq.builtin: proxy_reader_add_connection(wr 110cd0d:79d6eeac:ea4a8907:403 prd 110aba1:7b19badd:ce0adb73:504) -1711462090.676392 [0] dq.builtin: writer_add_connection(wr 110cd0d:79d6eeac:ea4a8907:403 prd 110aba1:7b19badd:ce0adb73:504) - ack seq 13 -1711462090.676401 [0] dq.builtin: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 udp/10.101.12.71:7417@2 udp/10.101.12.71:7419@2 -1711462090.676404 [0] dq.builtin: reduced nlocs=5 -1711462090.676408 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676413 [0] dq.builtin: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462090.676417 [0] dq.builtin: rdidx 0 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676420 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676424 [0] dq.builtin: rdidx 1 lidx udp/10.101.12.71:7417@2 2 -> 0 -1711462090.676429 [0] dq.builtin: rdidx 1 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676433 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676437 [0] dq.builtin: rdidx 2 lidx udp/10.101.12.71:7419@2 3 -> 0 -1711462090.676441 [0] dq.builtin: rdidx 2 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676444 [0] dq.builtin: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462090.676449 [0] dq.builtin: rdidx 3 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462090.676453 [0] dq.builtin: rdidx 3 lidx udp/239.255.0.1:7401@2 4 -> 8 -1711462090.676457 [0] dq.builtin: a b c d -1711462090.676463 [0] dq.builtin: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. .. .. } -1711462090.676467 [0] dq.builtin: loc 1 = udp/10.101.12.71:7415@2 1 { .. .. .. +u } -1711462090.676472 [0] dq.builtin: loc 2 = udp/10.101.12.71:7417@2 1 { .. +u .. .. } -1711462090.676477 [0] dq.builtin: loc 3 = udp/10.101.12.71:7419@2 1 { .. .. +u .. } -1711462090.676483 [0] dq.builtin: loc 4 = udp/239.255.0.1:7401@2 -1 { +1 +1 +1 +1 } -1711462090.676486 [0] dq.builtin: best = 4 -1711462090.676490 [0] dq.builtin: simple udp/239.255.0.1:7401@2 -1711462090.676510 [0] dq.builtin: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462090.676524 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #2: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rq/topological_navigation_core/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:904},adlink_entity_factory=1} -1711462090.676541 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:904 reliable volatile reader unnamed: (default).rq/topological_navigation_core/describe_parametersRequest/rcl_interfaces::srv::dds_::DescribeParameters_Request_ p(open) known -1711462090.676557 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.1;">,topic_name="rq/topological_navigation_core/describe_parametersRequest",type_name="rcl_interfaces::srv::dds_::DescribeParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676570 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #3: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rq/topological_navigation_core/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:b04},adlink_entity_factory=1} -1711462090.676576 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:b04 reliable volatile reader unnamed: (default).rq/topological_navigation_core/get_parametersRequest/rcl_interfaces::srv::dds_::GetParameters_Request_ p(open) known -1711462090.676591 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.2;">,topic_name="rq/topological_navigation_core/get_parametersRequest",type_name="rcl_interfaces::srv::dds_::GetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676600 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #4: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rq/topological_navigation_core/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:d04},adlink_entity_factory=1} -1711462090.676607 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:d04 reliable volatile reader unnamed: (default).rq/topological_navigation_core/get_parameter_typesRequest/rcl_interfaces::srv::dds_::GetParameterTypes_Request_ p(open) known -1711462090.676621 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.3;">,topic_name="rq/topological_navigation_core/get_parameter_typesRequest",type_name="rcl_interfaces::srv::dds_::GetParameterTypes_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676634 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #5: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rq/topological_navigation_core/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:f04},adlink_entity_factory=1} -1711462090.676641 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:f04 reliable volatile reader unnamed: (default).rq/topological_navigation_core/list_parametersRequest/rcl_interfaces::srv::dds_::ListParameters_Request_ p(open) known -1711462090.676655 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.4;">,topic_name="rq/topological_navigation_core/list_parametersRequest",type_name="rcl_interfaces::srv::dds_::ListParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676664 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #6: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rq/topological_navigation_core/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1104},adlink_entity_factory=1} -1711462090.676670 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1104 reliable volatile reader unnamed: (default).rq/topological_navigation_core/set_parametersRequest/rcl_interfaces::srv::dds_::SetParameters_Request_ p(open) known -1711462090.676684 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.5;">,topic_name="rq/topological_navigation_core/set_parametersRequest",type_name="rcl_interfaces::srv::dds_::SetParameters_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676694 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #7: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rq/topological_navigation_core/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",reliability=1:9223372036854775807,history=0:1000,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1304},adlink_entity_factory=1} -1711462090.676703 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1304 reliable volatile reader unnamed: (default).rq/topological_navigation_core/set_parameters_atomicallyRequest/rcl_interfaces::srv::dds_::SetParametersAtomically_Request_ p(open) known -1711462090.676717 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=53<"serviceid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.6;">,topic_name="rq/topological_navigation_core/set_parameters_atomicallyRequest",type_name="rcl_interfaces::srv::dds_::SetParametersAtomically_Request_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:1000,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676726 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #8: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rr/controller_server/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1504},adlink_entity_factory=1} -1711462090.676733 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1504 reliable volatile reader unnamed: (default).rr/controller_server/set_parametersReply/rcl_interfaces::srv::dds_::SetParameters_Response_ p(open) known -1711462090.676748 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.7;">,topic_name="rr/controller_server/set_parametersReply",type_name="rcl_interfaces::srv::dds_::SetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676757 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #9: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rr/controller_server/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1704},adlink_entity_factory=1} -1711462090.676763 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1704 reliable volatile reader unnamed: (default).rr/controller_server/list_parametersReply/rcl_interfaces::srv::dds_::ListParameters_Response_ p(open) known -1711462090.676777 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.8;">,topic_name="rr/controller_server/list_parametersReply",type_name="rcl_interfaces::srv::dds_::ListParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.676789 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #10: ST0 DCPSSubscription/SubscriptionBuiltinTopicData:{user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rr/controller_server/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110aba1:7b19badd:ce0adb73:1904},adlink_entity_factory=1} -1711462090.676796 [0] dq.builtin: SEDP ST0 110aba1:7b19badd:ce0adb73:1904 reliable volatile reader unnamed: (default).rr/controller_server/get_parametersReply/rcl_interfaces::srv::dds_::GetParameters_Response_ p(open) known -1711462090.676810 [0] dq.builtin: (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7419@2 ssm=0) QOS={user_data=52<"clientid= 1.10.ab.a1.7b.19.ba.dd.ce.a.db.73.0.0.0.9;">,topic_name="rr/controller_server/get_parametersReply",type_name="rcl_interfaces::srv::dds_::GetParameters_Response_",topic_data=0<>,group_data=0<>,durability=0,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,presentation=0:0:0,partition={},time_based_filter=0,transport_priority=0,type_consistency_enforcement=1:11000,data_representation=1(0),adlink_entity_factory=1,adlink_reader_lifespan=0:9223372036854775807,adlink_reader_data_lifecycle=9223372036854775807:9223372036854775807,adlink_subscription_keys=0:{}} -1711462090.677358 [0] tev: writer_hbcontrol: wr 110cd0d:56a27910:57318171:403 unicasting to prd 110aba1:7b19badd:ce0adb73:504 (rel-prd 4 seq-eq-max 3 seq 12 maxseq 12) -1711462090.677379 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:403) sent, resched in 0.1 s (min-ack 12!, avail-seq 12, xmit 12) -1711462090.677387 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.677397 [0] tev: writer_hbcontrol: wr 110cd0d:e3b81b8d:1ccb65b1:403 unicasting to prd 110aba1:7b19badd:ce0adb73:504 (rel-prd 4 seq-eq-max 3 seq 13 maxseq 13) -1711462090.677405 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:403) sent, resched in 0.1 s (min-ack 13!, avail-seq 13, xmit 13) -1711462090.677412 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400ecf0 0(control): niov 2 sz 68 => now niov 4 sz 124 -1711462090.677434 [0] tev: nn_xpack_send 124: 0x7408c000139c:20 0x7408c400f858:48 0x7408c400edc0:24 0x7408c400ede8:32 [ udp/10.101.12.71:7419@2 ] -1711462090.677441 [0] tev: traffic-xmit (1) 124 -1711462090.677578 [0] tev: writer_hbcontrol: wr 110cd0d:79d6eeac:ea4a8907:403 unicasting to prd 110aba1:7b19badd:ce0adb73:504 (rel-prd 4 seq-eq-max 3 seq 13 maxseq 13) -1711462090.677588 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:403) sent, resched in 0.1 s (min-ack 13!, avail-seq 13, xmit 13) -1711462090.677596 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f770 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.677613 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408c400f858:48 [ udp/10.101.12.71:7419@2 ] -1711462090.677621 [0] tev: traffic-xmit (1) 68 -1711462090.677623 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 116 from udp/10.101.12.71:52025 -1711462090.677641 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.677666 [0] recvUC: ACKNACK(F#1:12/1:1 L(:1c1 16938.337142) 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:56a27910:57318171:403 complying RX12non-timed queue now has 1 items -1711462090.677678 [0] recvUC: rexmit#1 maxseq:12<12<=12defer_heartbeat_to_peer: 110cd0d:56a27910:57318171:403 -> 110aba1:7b19badd:ce0adb73:504 - queue for transmit -1711462090.677683 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009100 0(rexmit(110cd0d:56a27910:57318171:403:#12/1)): niov 0 sz 0 => now niov 3 sz 544 -1711462090.677687 [0] recvUC: ) -1711462090.677707 [0] tev: nn_xpack_send 544: 0x7408c000139c:20 0x7408bc0091e8:52 0x7408ac01b0d0:472 [ udp/10.101.12.71:7419@2 ] -1711462090.677725 [0] tev: traffic-xmit (1) 544 -1711462090.677708 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.677758 [0] recvUC: ACKNACK(F#1:13/1:1 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:e3b81b8d:1ccb65b1:403 complying RX13non-timed queue now has 1 items -1711462090.677767 [0] recvUC: rexmit#1 maxseq:13<13<=13defer_heartbeat_to_peer: 110cd0d:e3b81b8d:1ccb65b1:403 -> 110aba1:7b19badd:ce0adb73:504 - queue for transmit -1711462090.677773 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(rexmit(110cd0d:e3b81b8d:1ccb65b1:403:#13/1)): niov 0 sz 0 => now niov 3 sz 520 -1711462090.677773 [0] recvUC: non-timed queue now has 1 items -1711462090.677796 [0] recvUC: ) -1711462090.677807 [0] recvUC: send_deferred_heartbeat: 18ed9dbea84bb67e -> daaf4d312a3532b - queue for transmit -1711462090.677815 [0] recvUC: non-timed queue now has 1 items -1711462090.677800 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009280 0(control): niov 3 sz 520 => now niov 5 sz 576 -1711462090.677836 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009580 0(control): niov 5 sz 576 => now niov 7 sz 632 -1711462090.677825 [0] recvUC: thread_cputime 0.005695000 -1711462090.677856 [0] tev: nn_xpack_send 632: 0x7408c000139c:20 0x7408bc0094e8:52 0x7408d406c3b0:448 0x7408bc009350:24 0x7408bc009378:32 0x7408bc009650:24 0x7408bc009678:32 [ udp/10.101.12.71:7419@2 ] -1711462090.677867 [0] tev: traffic-xmit (1) 632 -1711462090.677862 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 68 from udp/10.101.12.71:52025 -1711462090.677889 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.677906 [0] recvUC: ACKNACK(F#1:13/1:1 L(:1c1 16938.337391) 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:79d6eeac:ea4a8907:403 complying RX13non-timed queue now has 1 items -1711462090.677916 [0] recvUC: rexmit#1 maxseq:13<13<=13defer_heartbeat_to_peer: 110cd0d:79d6eeac:ea4a8907:403 -> 110aba1:7b19badd:ce0adb73:504 - queue for transmit -1711462090.677922 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(rexmit(110cd0d:79d6eeac:ea4a8907:403:#13/1)): niov 0 sz 0 => now niov 3 sz 520 -1711462090.677923 [0] recvUC: ) -1711462090.677942 [0] recvUC: send_deferred_heartbeat: abaf4210bc473a65 -> daaf4d312a3532b - queue for transmit -1711462090.677950 [0] recvUC: non-timed queue now has 1 items -1711462090.677942 [0] tev: nn_xpack_send 520: 0x7408c000139c:20 0x7408bc0097e8:52 0x7408e003d100:448 [ udp/10.101.12.71:7419@2 ] -1711462090.677968 [0] tev: traffic-xmit (1) 520 -1711462090.677976 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.677991 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc009968:48 [ udp/10.101.12.71:7419@2 ] -1711462090.677998 [0] tev: traffic-xmit (1) 68 -1711462090.678142 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 108 from udp/10.101.12.71:52025 -1711462090.678152 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.678161 [0] recvUC: ACKNACK(F#2:13/0: L(:1c1 16938.337654) 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:56a27910:57318171:403 setting-has-replied-to-hb happy-now) -1711462090.678168 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.678175 [0] recvUC: ACKNACK(F#2:14/0: 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:e3b81b8d:1ccb65b1:403 setting-has-replied-to-hb happy-now) -1711462090.678292 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462090.678300 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.678309 [0] recvUC: ACKNACK(F#2:14/0: L(:1c1 16938.337803) 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:79d6eeac:ea4a8907:403 setting-has-replied-to-hb happy-now) -1711462090.685371 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.685394 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.685403 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.685437 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7419@2 ] -1711462090.685444 [0] tev: traffic-xmit (1) 168 -1711462090.685586 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 116 from udp/10.101.12.71:52025 -1711462090.685598 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.685617 [0] recvUC: HEARTBEAT(#16:12..12 L(:1c1 16938.345100)110aba1:7b19badd:ce0adb73:403 -> 110cd0d:56a27910:57318171:504: end-of-tl-seq(rd 110cd0d:56a27910:57318171:504 #12) end-of-tl-seq(rd 110cd0d:e3b81b8d:1ccb65b1:504 #12) end-of-tl-seq(rd 110cd0d:79d6eeac:ea4a8907:504 #12) 110cd0d:56a27910:57318171:504@11) -1711462090.685625 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.685628 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403: F#1:12/1:1 -1711462090.685646 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.685654 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.685665 [0] recvUC: HEARTBEAT(#17:12..12 110aba1:7b19badd:ce0adb73:403 -> 110cd0d:79d6eeac:ea4a8907:504: 110cd0d:79d6eeac:ea4a8907:504@11) -1711462090.685671 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc009968:48 [ udp/10.101.12.71:7419@2 ] -1711462090.685691 [0] tev: traffic-xmit (1) 68 -1711462090.685682 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 68 from udp/10.101.12.71:52025 -1711462090.685703 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403: F#8:12/1:1 -1711462090.685711 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.685724 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.685736 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.685746 [0] recvUC: HEARTBEAT(#18:12..12 L(:1c1 16938.345212)110aba1:7b19badd:ce0adb73:403 -> 110cd0d:e3b81b8d:1ccb65b1:504: 110cd0d:e3b81b8d:1ccb65b1:504@11) -1711462090.685752 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc009968:48 [ udp/10.101.12.71:7419@2 ] -1711462090.685771 [0] tev: traffic-xmit (1) 68 -1711462090.685763 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 632 from udp/10.101.12.71:52025 -1711462090.685784 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403: F#17:12/1:1 -1711462090.685808 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.685816 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 68 -1711462090.685796 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.685834 [0] tev: nn_xpack_send 68: 0x7408c000139c:20 0x7408bc009968:48 [ udp/10.101.12.71:7419@2 ] -1711462090.685844 [0] tev: traffic-xmit (1) 68 -1711462090.685837 [0] recvUC: INFOTS(1711462090.592467231) -1711462090.685944 [0] recvUC: DATA(110aba1:7b19badd:ce0adb73:403 -> 110cd0d:56a27910:57318171:504 #12 L(:1c1 16938.345298) msr_in_sync(110cd0d:56a27910:57318171:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, =>110cd0d:56a27910:57318171:504 -1711462090.686044 [0] recvUC: msr_in_sync(110cd0d:e3b81b8d:1ccb65b1:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, =>110cd0d:e3b81b8d:1ccb65b1:504 -1711462090.686129 [0] recvUC: msr_in_sync(110cd0d:79d6eeac:ea4a8907:504 out-of-sync to tlcatchup)data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206, =>110cd0d:79d6eeac:ea4a8907:504 -1711462090.686141 [0] recvUC: ) -1711462090.686153 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 68 from udp/10.101.12.71:52025 -1711462090.686161 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462090.686174 [0] recvUC: HEARTBEAT(#19:12..12 L(:1c1 16938.345663)110aba1:7b19badd:ce0adb73:403 -> 110cd0d:56a27910:57318171:504: 110cd0d:56a27910:57318171:504@12(sync)) -1711462090.686185 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 664 from udp/10.101.12.71:52025 -1711462090.686191 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462090.686197 [0] recvUC: INFOTS(1711462090.592467231) -1711462090.686213 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403: F#2:13/0: -1711462090.686225 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.686233 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.686239 [0] recvUC: DATA(110aba1:7b19badd:ce0adb73:403 -> 110cd0d:79d6eeac:ea4a8907:504 #12 L(:1c1 16938.345694)) -1711462090.686258 [0] tev: nn_xpack_send 64: 0x7408c000139c:20 0x7408bc009968:44 [ udp/10.101.12.71:7419@2 ] -1711462090.686267 [0] tev: traffic-xmit (1) 64 -1711462090.686262 [0] recvUC: HEARTBEAT(#20:12..12 110aba1:7b19badd:ce0adb73:403 -> 110cd0d:79d6eeac:ea4a8907:504: 110cd0d:79d6eeac:ea4a8907:504@12(sync)) -1711462090.686278 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403: F#9:13/0: -1711462090.686301 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.686309 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.686291 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 632 from udp/10.101.12.71:52025 -1711462090.686327 [0] tev: nn_xpack_send 64: 0x7408c000139c:20 0x7408bc009968:44 [ udp/10.101.12.71:7419@2 ] -1711462090.686337 [0] tev: traffic-xmit (1) 64 -1711462090.686328 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.686357 [0] recvUC: INFOTS(1711462090.592467231) -1711462090.686366 [0] recvUC: DATA(110aba1:7b19badd:ce0adb73:403 -> 110cd0d:e3b81b8d:1ccb65b1:504 #12 L(:1c1 16938.345830)) -1711462090.686376 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 68 from udp/10.101.12.71:52025 -1711462090.686383 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462090.686394 [0] recvUC: HEARTBEAT(#21:12..12 L(:1c1 16938.345886)110aba1:7b19badd:ce0adb73:403 -> 110cd0d:e3b81b8d:1ccb65b1:504: 110cd0d:e3b81b8d:1ccb65b1:504@12(sync)) -1711462090.686403 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403: F#18:13/0: -1711462090.686412 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.686418 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.686434 [0] tev: nn_xpack_send 64: 0x7408c000139c:20 0x7408bc009968:44 [ udp/10.101.12.71:7419@2 ] -1711462090.686440 [0] tev: traffic-xmit (1) 64 -1711462090.774545 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462090.774584 [0] recvMC: HEARTBEAT(#50:42..42 L(:1c1 16938.434073)110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@42(sync) 110cd0d:e3b81b8d:1ccb65b1:504@42(sync) 110cd0d:79d6eeac:ea4a8907:504@42(sync)) -1711462090.774661 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#3:43/0: -1711462090.774680 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462090.774689 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.774697 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#19:43/0: -1711462090.774703 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462090.774708 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.774715 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#10:43/0: -1711462090.774720 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462090.774725 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.774751 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7415@2 ] -1711462090.774758 [0] tev: traffic-xmit (1) 168 -1711462090.777536 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:403) suppressed, resched in inf s (min-ack 12, avail-seq 12, xmit 12) -1711462090.777573 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:403) suppressed, resched in inf s (min-ack 13, avail-seq 13, xmit 13) -1711462090.777582 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:403) suppressed, resched in inf s (min-ack 13, avail-seq 13, xmit 13) -1711462090.790633 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462090.790667 [0] recvMC: HEARTBEAT(#22:12..12 L(:1c1 16938.450158)110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@12(sync) 110cd0d:e3b81b8d:1ccb65b1:504@12(sync) 110cd0d:79d6eeac:ea4a8907:504@12(sync)) -1711462090.790740 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403: F#3:13/0: -1711462090.790759 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.790766 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462090.790781 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403: F#19:13/0: -1711462090.790786 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.790793 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462090.790801 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403: F#10:13/0: -1711462090.790806 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462090.790811 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462090.790836 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7419@2 ] -1711462090.790843 [0] tev: traffic-xmit (1) 168 -1711462091.469275 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 256 from udp/10.101.12.71:40473 -1711462091.469305 [0] recv: INFOTS(1711462091.469007481) -1711462091.469323 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #43 L(:1c1 16939.128806)) -1711462091.469367 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #43: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",durability=1,durability_service=0:0:10:-1:-1:-1,reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5603},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.469416 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5603 reliable transient-local writer unnamed: (default).rt/topological_map_2/std_msgs::msg::dds_::String_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="rt/topological_map_2",type_name="std_msgs::msg::dds_::String_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:10:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.469430 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2096 from udp/10.101.12.71:40473 -1711462091.469454 [0] recvMC: INFOTS(1711462091.469060530) -1711462091.469457 [0] dq.builtin: => EVERYONE -1711462091.469475 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #43 L(:1c1 16939.128956) => EVERYONE -1711462091.469511 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5603) scanning all rds of topic rt/topological_map_2 -1711462091.469524 [0] dq.builtin: reader 110cd0d:56a27910:57318171:1404 init_acknack_count = 1 -1711462091.469528 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:5603 rd 110cd0d:56a27910:57318171:1404) -1711462091.469549 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:5603 rd 110cd0d:56a27910:57318171:1404) - out-of-sync -1711462091.469555 [0] dq.builtin: reader 110cd0d:e3b81b8d:1ccb65b1:1404 init_acknack_count = 9 -1711462091.469559 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:5603 rd 110cd0d:e3b81b8d:1ccb65b1:1404) -1711462091.469564 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:5603 rd 110cd0d:e3b81b8d:1ccb65b1:1404) - out-of-sync -1711462091.469569 [0] dq.builtin: reader 110cd0d:79d6eeac:ea4a8907:1404 init_acknack_count = 5 -1711462091.469575 [0] dq.builtin: reader_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:5603 rd 110cd0d:79d6eeac:ea4a8907:1404) -1711462091.469580 [0] dq.builtin: proxy_writer_add_connection(pwr 110d7b4:17c5b8c5:94bd0ff4:5603 rd 110cd0d:79d6eeac:ea4a8907:1404) - out-of-sync -1711462091.469590 [0] dq.builtin: thread_cputime 0.018514000 -1711462091.469683 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #43: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462091.469805 [0] recvMC: HEARTBEAT(#51:43..43 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@43(sync) 110cd0d:e3b81b8d:1ccb65b1:504@43(sync) 110cd0d:79d6eeac:ea4a8907:504@43(sync)) -1711462091.469887 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#4:44/0: -1711462091.469907 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462091.469892 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462091.469917 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.469943 [0] recvMC: HEARTBEAT(#1:1..0 L(:1c1 16939.129433)110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: end-of-tl-seq(rd 110cd0d:56a27910:57318171:1404 #0) msr_in_sync(110cd0d:56a27910:57318171:1404 out-of-sync to tlcatchup) end-of-tl-seq(rd 110cd0d:e3b81b8d:1ccb65b1:1404 #0) msr_in_sync(110cd0d:e3b81b8d:1ccb65b1:1404 out-of-sync to tlcatchup) end-of-tl-seq(rd 110cd0d:79d6eeac:ea4a8907:1404 #0) msr_in_sync(110cd0d:79d6eeac:ea4a8907:1404 out-of-sync to tlcatchup) 110cd0d:56a27910:57318171:1404@0(sync) 110cd0d:e3b81b8d:1ccb65b1:1404@0(sync) 110cd0d:79d6eeac:ea4a8907:1404@0(sync)) -1711462091.469948 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#20:44/0: -1711462091.469956 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462091.469963 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462091.469972 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#11:44/0: -1711462091.469978 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462091.469995 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462091.470005 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#9:1/0: -1711462091.470010 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.470015 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009280 0(control): niov 6 sz 168 => now niov 8 sz 220 -1711462091.470022 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#5:1/0: -1711462091.470028 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.470033 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009580 0(control): niov 8 sz 220 => now niov 10 sz 272 -1711462091.470039 [0] tev: acknack 110cd0d:56a27910:57318171:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#1:1/0: -1711462091.470045 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.470050 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009100 0(control): niov 10 sz 272 => now niov 12 sz 324 -1711462091.470083 [0] tev: nn_xpack_send 324: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408bc009350:24 0x7408bc009378:28 0x7408bc009650:24 0x7408bc009678:28 0x7408bc0091d0:24 0x7408bc0091f8:28 [ udp/10.101.12.71:7415@2 ] -1711462091.470092 [0] tev: traffic-xmit (1) 324 -1711462091.481721 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2120 from udp/10.101.12.71:40473 -1711462091.481733 [0] recvMC: INFOTS(1711462091.481540997) -1711462091.481734 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 200 from udp/10.101.12.71:40473 -1711462091.481743 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #44 L(:1c1 16939.141236) => EVERYONE -1711462091.481764 [0] recv: INFOTS(1711462091.481496692) -1711462091.481792 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #44 L(:1c1 16939.141266)) -1711462091.481846 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #44: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462091.481856 [0] recvMC: HEARTBEAT(F#52:44..44 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@44(sync) 110cd0d:e3b81b8d:1ccb65b1:504@44(sync) 110cd0d:79d6eeac:ea4a8907:504@44(sync)) -1711462091.481914 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #44: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/tf",type_name="tf2_msgs::msg::dds_::TFMessage_",reliability=1:9223372036854775807,history=0:100,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5703},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.482007 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5703 reliable volatile writer unnamed: (default).rt/tf/tf2_msgs::msg::dds_::TFMessage_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="rt/tf",type_name="tf2_msgs::msg::dds_::TFMessage_",topic_data=0<>,group_data=0<>,durability=0,durability_service=0:0:1:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:100,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.482106 [0] dq.builtin: => EVERYONE -1711462091.482132 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5703) scanning all rds of topic rt/tf -1711462091.482914 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2144 from udp/10.101.12.71:40473 -1711462091.482920 [0] recvMC: INFOTS(1711462091.482763810) -1711462091.482926 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #45 L(:1c1 16939.142423) => EVERYONE -1711462091.482946 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 276 from udp/10.101.12.71:40473 -1711462091.482972 [0] recv: INFOTS(1711462091.482728775) -1711462091.482985 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #45 L(:1c1 16939.142474)) -1711462091.482993 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #45: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462091.483011 [0] recvMC: HEARTBEAT(F#53:45..45 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@45(sync) 110cd0d:e3b81b8d:1ccb65b1:504@45(sync) 110cd0d:79d6eeac:ea4a8907:504@45(sync)) -1711462091.483095 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #45: ST0 DCPSPublication/PublicationBuiltinTopicData:{topic_name="rt/topological_map",type_name="topological_navigation_msgs::msg::dds_::TopologicalMap_",durability=1,durability_service=0:0:10:-1:-1:-1,reliability=1:9223372036854775807,history=0:10,protocol_version=2:1,vendorid=1:16,endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5803},adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.483150 [0] dq.builtin: SEDP ST0 110d7b4:17c5b8c5:94bd0ff4:5803 reliable transient-local writer unnamed: (default).rt/topological_map/topological_navigation_msgs::msg::dds_::TopologicalMap_ p(open) NEW (as udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 ssm=0) QOS={user_data=0<>,topic_name="rt/topological_map",type_name="topological_navigation_msgs::msg::dds_::TopologicalMap_",topic_data=0<>,group_data=0<>,durability=1,durability_service=0:0:10:-1:-1:-1,deadline=9223372036854775807,latency_budget=0,liveliness=0:9223372036854775807,reliability=1:9223372036854775807,lifespan=9223372036854775807,destination_order=0,history=0:10,resource_limits=-1:-1:-1,ownership=0,ownership_strength=0,presentation=0:0:0,partition={},transport_priority=0,data_representation=1(0),adlink_entity_factory=1,adlink_writer_data_lifecycle=0} -1711462091.483185 [0] dq.builtin: => EVERYONE -1711462091.483240 [0] dq.builtin: match_proxy_writer_with_readers(pwr 110d7b4:17c5b8c5:94bd0ff4:5803) scanning all rds of topic rt/topological_map -1711462091.521386 [0] tev: non-timed queue now has 1 items -1711462091.521441 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110e21c:d0762c48:a9f0fb28:100c7 (resched 1s) -1711462091.521460 [0] tev: non-timed queue now has 2 items -1711462091.521469 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110e21c:d0762c48:a9f0fb28:100c7 (resched 1s) -1711462091.521483 [0] tev: non-timed queue now has 3 items -1711462091.521491 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110e21c:d0762c48:a9f0fb28:100c7 (resched 1s) -1711462091.521503 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462091.521511 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462091.521518 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462091.521578 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7412@2 ] -1711462091.521587 [0] tev: traffic-xmit (1) 1356 -1711462091.543207 [0] tev: non-timed queue now has 1 items -1711462091.543245 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110d7b4:17c5b8c5:94bd0ff4:100c7 (resched 1s) -1711462091.543263 [0] tev: non-timed queue now has 2 items -1711462091.543272 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110d7b4:17c5b8c5:94bd0ff4:100c7 (resched 1s) -1711462091.543287 [0] tev: non-timed queue now has 3 items -1711462091.543295 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110d7b4:17c5b8c5:94bd0ff4:100c7 (resched 1s) -1711462091.543306 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462091.543315 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462091.543323 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462091.543375 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7414@2 ] -1711462091.543384 [0] tev: traffic-xmit (1) 1356 -1711462091.550579 [0] tev: non-timed queue now has 1 items -1711462091.550599 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110443d:bb7f10a5:18901533:100c7 (resched 1s) -1711462091.550614 [0] tev: non-timed queue now has 2 items -1711462091.550624 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110443d:bb7f10a5:18901533:100c7 (resched 1s) -1711462091.550636 [0] tev: non-timed queue now has 3 items -1711462091.550646 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110443d:bb7f10a5:18901533:100c7 (resched 1s) -1711462091.550657 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462091.550666 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462091.550674 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462091.550707 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7416@2 ] -1711462091.550716 [0] tev: traffic-xmit (1) 1356 -1711462091.569318 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462091.569392 [0] recv: HEARTBEAT(#9:1..45 L(:1c1 16939.228869)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@45(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@45(sync) 110cd0d:79d6eeac:ea4a8907:3c7@45(sync)) -1711462091.569450 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#4:46/0: -1711462091.569471 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462091.569481 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.569497 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#14:46/0: -1711462091.569503 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462091.569510 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462091.569520 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#8:46/0: -1711462091.569527 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462091.569534 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462091.569568 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7414@2 ] -1711462091.569578 [0] tev: traffic-xmit (1) 168 -1711462091.572845 [0] tev: non-timed queue now has 1 items -1711462091.572881 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462091.572897 [0] tev: non-timed queue now has 2 items -1711462091.572906 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462091.572920 [0] tev: non-timed queue now has 3 items -1711462091.572927 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462091.572937 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462091.572945 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462091.572952 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462091.572997 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7418@2 ] -1711462091.573005 [0] tev: traffic-xmit (1) 1356 -1711462091.669841 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462091.669946 [0] recvMC: HEARTBEAT(#54:45..45 L(:1c1 16939.329405)110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@45(sync) 110cd0d:e3b81b8d:1ccb65b1:504@45(sync) 110cd0d:79d6eeac:ea4a8907:504@45(sync)) -1711462091.670142 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#5:46/0: -1711462091.670146 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462091.670254 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462091.670283 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.670307 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#21:46/0: -1711462091.670324 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462091.670341 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462091.670348 [0] recvMC: HEARTBEAT(#3:1..1 L(:1c1 16939.329818)110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0: 110cd0d:56a27910:57318171:1404@0(sync) 110cd0d:e3b81b8d:1ccb65b1:1404@0(sync) 110cd0d:79d6eeac:ea4a8907:1404@0(sync)) -1711462091.670376 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#12:46/0: -1711462091.670520 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462091.670565 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462091.670599 [0] tev: acknack 110cd0d:56a27910:57318171:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#2:1/1:1 -1711462091.670616 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.670631 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009280 0(control): niov 6 sz 168 => now niov 8 sz 224 -1711462091.670651 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#10:1/1:1 -1711462091.670665 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.670682 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009580 0(control): niov 8 sz 224 => now niov 10 sz 280 -1711462091.670700 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#6:1/1:1 -1711462091.670719 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.670733 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009100 0(control): niov 10 sz 280 => now niov 12 sz 336 -1711462091.670809 [0] tev: nn_xpack_send 336: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408bc009350:24 0x7408bc009378:32 0x7408bc009650:24 0x7408bc009678:32 0x7408bc0091d0:24 0x7408bc0091f8:32 [ udp/10.101.12.71:7415@2 ] -1711462091.670830 [0] tev: traffic-xmit (1) 336 -1711462091.671617 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20568 from udp/10.101.12.71:40473 -1711462091.671673 [0] recvMC: INFOTS(1711462091.478307355) -1711462091.671739 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[1..1]/[0..20500) of 340680 L(:1c1 16939.331168)) -1711462091.671989 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 164 from udp/10.101.12.71:40473 -1711462091.672038 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462091.672078 [0] recvUC: HEARTBEAT(#5:1..1 L(:1c1 16939.331535)110d7b4:17c5b8c5:94bd0ff4:5603 -> 110cd0d:56a27910:57318171:1404: 110cd0d:56a27910:57318171:1404@0(sync)) -1711462091.672099 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462091.672119 [0] recvUC: HEARTBEAT(#6:1..1 110d7b4:17c5b8c5:94bd0ff4:5603 -> 110cd0d:e3b81b8d:1ccb65b1:1404: 110cd0d:e3b81b8d:1ccb65b1:1404@0(sync)) -1711462091.672133 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462091.672149 [0] recvUC: HEARTBEAT(#7:1..1 110d7b4:17c5b8c5:94bd0ff4:5603 -> 110cd0d:79d6eeac:ea4a8907:1404: 110cd0d:79d6eeac:ea4a8907:1404@0(sync)) -1711462091.672175 [0] tev: thread_cputime 0.015848000 -1711462091.672233 [0] tev: acknack 110cd0d:56a27910:57318171:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#3:1/0: + nackfrag #1:1/2/16:1111111111111111 -1711462091.672250 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.672266 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 100 -1711462091.672305 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#11:1/0: + nackfrag #2:1/2/16:1111111111111111 -1711462091.672319 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.672331 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 100 => now niov 4 sz 188 -1711462091.672354 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#7:1/0: + nackfrag #3:1/2/16:1111111111111111 -1711462091.672395 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.672411 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 188 => now niov 6 sz 276 -1711462091.672473 [0] tev: nn_xpack_send 276: 0x7408c000139c:20 0x7408bc009968:80 0x7408bc0097d0:24 0x7408bc0097f8:64 0x7408bc0094d0:24 0x7408bc0094f8:64 [ udp/10.101.12.71:7415@2 ] -1711462091.672490 [0] tev: traffic-xmit (1) 276 -1711462091.673548 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.673594 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[5..5]/[82000..102500) of 340680 L(:1c1 16939.333082)) -1711462091.673797 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.673840 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[6..6]/[102500..123000) of 340680 L(:1c1 16939.333329)) -1711462091.673894 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.673916 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[7..7]/[123000..143500) of 340680 L(:1c1 16939.333412)) -1711462091.674072 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674110 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[8..8]/[143500..164000) of 340680 L(:1c1 16939.333601)) -1711462091.674163 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674182 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[9..9]/[164000..184500) of 340680 L(:1c1 16939.333678)) -1711462091.674371 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674426 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[10..10]/[184500..205000) of 340680 L(:1c1 16939.333916)) -1711462091.674488 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674512 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[11..11]/[205000..225500) of 340680 L(:1c1 16939.334008)) -1711462091.674529 [0] recvMC: thread_cputime 0.010752000 -1711462091.674584 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.674603 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[12..12]/[225500..246000) of 340680 L(:1c1 16939.334100)) -1711462091.676607 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.676641 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[13..13]/[246000..266500) of 340680 L(:1c1 16939.336135)) -1711462091.676757 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.676823 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[14..14]/[266500..287000) of 340680 L(:1c1 16939.336312)) -1711462091.676868 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.676878 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[15..15]/[287000..307500) of 340680 L(:1c1 16939.336378)) -1711462091.676922 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.676931 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[16..16]/[307500..328000) of 340680 L(:1c1 16939.336432)) -1711462091.680238 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 12736 from udp/10.101.12.71:40473 -1711462091.680305 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[17..17]/[328000..340680) of 340680 L(:1c1 16939.339794)) -1711462091.680354 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.680383 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[2..2]/[20500..41000) of 340680 L(:1c1 16939.339881)) -1711462091.680426 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.680436 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[3..3]/[41000..61500) of 340680 L(:1c1 16939.339937)) -1711462091.680565 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.680590 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[4..4]/[61500..82000) of 340680 L(:1c1 16939.340080) => EVERYONE -1711462091.680980 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 164 from udp/10.101.12.71:40473 -1711462091.681034 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462091.682899 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:5603 #1: ST0 rt/topological_map_2/std_msgs::msg::dds_::String_:{"{"meta": {"last_updated": "2022-06-23_13-10-11"}, "metric_map": "riseholme", "name": "strawberry_polytunnel", "nodes": [{"meta": {"map": "riseholme", "node": "WayPoint140", "pointset": "riseholme_restrict"}, "node": {"edges": [{"action": "NavigateToPose", "action_type": "geometry_msgs/PoseStamped", "config": [], "edge_id": "WayPoint140_WayPoint74", "fail_policy": "fail", "fluid_navigation": true, "goal": {"target_pose": {"header": {"frame_id": "$node.parent_frame"}, "pose": "$node.pose"}}, "node": "WayPoint74", "recovery_behaviours_config": "", "restrictions_planning": "True", "restrictions_runtime": "obstacleFree_1"}, {"action": "NavigateToPose", "action_type": "geometry_msgs/PoseStamped", "config": [], "edge_id": "WayPoint140_WayPoint141", "fail_policy": "fail", "fluid_navigation": true, "goal": {"target_pose": {"header": {"frame_id": "$node.parent_frame"}, "pose": "$node.pose"}}, "node": "WayPoint141", "recovery_behaviours_config": "", "restrictions_planning": "True", "restrictions_runtime": "obstacleFr) -1711462091.682925 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.682935 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[5..5]/[82000..102500) of 340680 L(:1c1 16939.342433)) -1711462091.682965 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 20556 from udp/10.101.12.71:40473 -1711462091.682974 [0] recvMC: DATAFRAG(110d7b4:17c5b8c5:94bd0ff4:5603 -> 0:0:0:0 #1/[6..6]/[102500..123000) of 340680 L(:1c1 16939.342474)) -1711462091.683005 [0] recvUC: HEARTBEAT(#9:1..1 L(:1c1 16939.340531)110d7b4:17c5b8c5:94bd0ff4:5603 -> 110cd0d:56a27910:57318171:1404: 110cd0d:56a27910:57318171:1404@1(sync)) -1711462091.683048 [0] recvUC: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462091.683067 [0] tev: acknack 110cd0d:56a27910:57318171:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#4:2/0: -1711462091.683093 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.683108 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.683164 [0] tev: nn_xpack_send 64: 0x7408c000139c:20 0x7408bc009968:44 [ udp/10.101.12.71:7415@2 ] -1711462091.683175 [0] tev: traffic-xmit (1) 64 -1711462091.683193 [0] recvUC: HEARTBEAT(#10:1..1 110d7b4:17c5b8c5:94bd0ff4:5603 -> 110cd0d:e3b81b8d:1ccb65b1:1404: 110cd0d:e3b81b8d:1ccb65b1:1404@1(sync)) -1711462091.683209 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#12:2/0: -1711462091.683237 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.683249 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.683223 [0] recvUC: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462091.683276 [0] tev: nn_xpack_send 64: 0x7408c000139c:20 0x7408bc009968:44 [ udp/10.101.12.71:7415@2 ] -1711462091.683290 [0] tev: traffic-xmit (1) 64 -1711462091.683282 [0] recvUC: HEARTBEAT(#11:1..1 110d7b4:17c5b8c5:94bd0ff4:5603 -> 110cd0d:79d6eeac:ea4a8907:1404: 110cd0d:79d6eeac:ea4a8907:1404@1(sync)) -1711462091.683302 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:1404 -> 110d7b4:17c5b8c5:94bd0ff4:5603: F#8:2/0: -1711462091.683332 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:1404 -> pwr 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462091.683346 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462091.683368 [0] tev: nn_xpack_send 64: 0x7408c000139c:20 0x7408bc009968:44 [ udp/10.101.12.71:7415@2 ] -1711462091.683379 [0] tev: traffic-xmit (1) 64 -1711462091.683398 [0] recvUC: thread_cputime 0.007174000 -1711462091.905837 [0] tev: write_sample 110cd0d:e3b81b8d:1ccb65b1:200c2 #4: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:e3b81b8d:1ccb65b1:1}:1<0> -1711462091.905916 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(110cd0d:e3b81b8d:1ccb65b1:200c2:#4/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462091.905933 [0] tev: writer_hbcontrol: wr 110cd0d:e3b81b8d:1ccb65b1:200c2 multicasting (rel-prd 4 seq-eq-max 4 seq 4 maxseq 3) -1711462091.905951 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:200c2) piggybacked, resched in 0.1 s (min-ack 3, avail-seq 4, xmit 3) -1711462091.905963 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462091.906092 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc009978:36 0x7408c8038f6c:28 0x7408bc0097f8:32 [ udp/239.255.0.1:7400@2 ] -1711462091.906107 [0] tev: traffic-xmit (1) 116 -1711462091.906121 [0] tev: resched pmd(110cd0d:e3b81b8d:1ccb65b1:1c1): 8s -1711462091.906145 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462091.906197 [0] recv: INFOTS(1711462091.905809560) -1711462091.906222 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0 #4 110cd0d:e3b81b8d:1ccb65b1:200c2? -> 0:0:0:0) -1711462091.906231 [0] recv: HEARTBEAT(#29:4..4 110cd0d:e3b81b8d:1ccb65b1:200c2? -> 0:0:0:0) -1711462091.906254 [0] recv: thread_cputime 0.048331000 -1711462091.906574 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462091.906615 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462091.906639 [0] recv: ACKNACK(F#3:5/0: L(:1c1 16939.566114) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462091.906657 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462091.906662 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462091.906672 [0] recv: ACKNACK(F#3:5/0: L(:1c1 16939.566165) 110e21c:d0762c48:a9f0fb28:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462091.906683 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462091.906688 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462091.906697 [0] recv: ACKNACK(F#3:5/0: L(:1c1 16939.566191) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462091.906707 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462091.906712 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462091.906722 [0] recv: ACKNACK(F#3:5/0: L(:1c1 16939.566216) 110443d:bb7f10a5:18901533:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462092.006014 [0] tev: non-timed queue now has 1 items -1711462092.006092 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 0:0:0:100c7 (resched 8s) -1711462092.006115 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:200c2) suppressed, resched in inf s (min-ack 4, avail-seq 4, xmit 4) -1711462092.006132 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462092.006440 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462092.006504 [0] recv: INFOTS(1711462067.905323275) -1711462092.006592 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462092.006610 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462092.006670 [0] recv: INFOTS(1711462067.905323275) -1711462092.006681 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462092.006728 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.006796 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (local) -1711462092.006834 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462092.006846 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (local) -1711462092.007971 [0] tev: nn_xpack_send 444: 0x7408c000139c:20 0x7408bc009978:36 0x7408d401eea4:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462092.007993 [0] tev: traffic-xmit (101) 444 -1711462092.521510 [0] tev: non-timed queue now has 1 items -1711462092.521568 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110e21c:d0762c48:a9f0fb28:100c7 (resched 1s) -1711462092.521587 [0] tev: non-timed queue now has 2 items -1711462092.521597 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110e21c:d0762c48:a9f0fb28:100c7 (resched 1s) -1711462092.521613 [0] tev: non-timed queue now has 3 items -1711462092.521622 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110e21c:d0762c48:a9f0fb28:100c7 (resched 1s) -1711462092.521634 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462092.521644 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462092.521652 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462092.521717 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7412@2 ] -1711462092.521726 [0] tev: traffic-xmit (1) 1356 -1711462092.543382 [0] tev: non-timed queue now has 1 items -1711462092.543421 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110d7b4:17c5b8c5:94bd0ff4:100c7 (resched 1s) -1711462092.543440 [0] tev: non-timed queue now has 2 items -1711462092.543449 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110d7b4:17c5b8c5:94bd0ff4:100c7 (resched 1s) -1711462092.543464 [0] tev: non-timed queue now has 3 items -1711462092.543473 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110d7b4:17c5b8c5:94bd0ff4:100c7 (resched 1s) -1711462092.543484 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462092.543493 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462092.543501 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462092.543556 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7414@2 ] -1711462092.543566 [0] tev: traffic-xmit (1) 1356 -1711462092.550845 [0] tev: non-timed queue now has 1 items -1711462092.550923 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110443d:bb7f10a5:18901533:100c7 (resched 1s) -1711462092.550955 [0] tev: non-timed queue now has 2 items -1711462092.550972 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110443d:bb7f10a5:18901533:100c7 (resched 1s) -1711462092.550997 [0] tev: non-timed queue now has 3 items -1711462092.551012 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110443d:bb7f10a5:18901533:100c7 (resched 1s) -1711462092.551030 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462092.551055 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462092.551070 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462092.551164 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7416@2 ] -1711462092.551185 [0] tev: traffic-xmit (1) 1356 -1711462092.572987 [0] tev: non-timed queue now has 1 items -1711462092.573068 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462092.573102 [0] tev: non-timed queue now has 2 items -1711462092.573121 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462092.573189 [0] tev: non-timed queue now has 3 items -1711462092.573207 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 110aba1:7b19badd:ce0adb73:100c7 (resched 1s) -1711462092.573229 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462092.573246 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462092.573262 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462092.573367 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7418@2 ] -1711462092.573386 [0] tev: traffic-xmit (1) 1356 -1711462093.521681 [0] tev: thread_cputime 0.020693000 -1711462093.521827 [0] tev: non-timed queue now has 1 items -1711462093.521867 [0] tev: non-timed queue now has 2 items -1711462093.521897 [0] tev: non-timed queue now has 3 items -1711462093.521926 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462093.521944 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462093.521960 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462093.522086 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7412@2 ] -1711462093.522104 [0] tev: traffic-xmit (1) 1356 -1711462093.543589 [0] tev: non-timed queue now has 1 items -1711462093.543640 [0] tev: non-timed queue now has 2 items -1711462093.543659 [0] tev: non-timed queue now has 3 items -1711462093.543674 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462093.543684 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462093.543694 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462093.543761 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7414@2 ] -1711462093.543772 [0] tev: traffic-xmit (1) 1356 -1711462093.551059 [0] tev: non-timed queue now has 1 items -1711462093.551144 [0] tev: non-timed queue now has 2 items -1711462093.551176 [0] tev: non-timed queue now has 3 items -1711462093.551200 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462093.551217 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462093.551232 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462093.551328 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7416@2 ] -1711462093.551345 [0] tev: traffic-xmit (1) 1356 -1711462093.573072 [0] tev: non-timed queue now has 1 items -1711462093.573141 [0] tev: non-timed queue now has 2 items -1711462093.573172 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 460 -1711462093.573201 [0] tev: non-timed queue now has 2 items -1711462093.573254 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(data(0:0:0:0:#0/1)): niov 3 sz 460 => now niov 6 sz 908 -1711462093.573272 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(data(0:0:0:0:#0/1)): niov 6 sz 908 => now niov 9 sz 1356 -1711462093.573376 [0] tev: nn_xpack_send 1356: 0x7408c000139c:20 0x7408bc009968:52 0x7408ac009c94:388 0x7408bc0097d0:24 0x7408bc0097f8:36 0x7408d401eea4:388 0x7408bc0094d0:24 0x7408bc0094f8:36 0x7408e000a504:388 [ udp/10.101.12.71:7418@2 ] -1711462093.573394 [0] tev: traffic-xmit (1) 1356 -1711462093.654337 [0] tev: write_sample 110cd0d:79d6eeac:ea4a8907:200c2 #4: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:79d6eeac:ea4a8907:1}:1<0> -1711462093.654441 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(110cd0d:79d6eeac:ea4a8907:200c2:#4/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462093.654465 [0] tev: writer_hbcontrol: wr 110cd0d:79d6eeac:ea4a8907:200c2 multicasting (rel-prd 4 seq-eq-max 4 seq 4 maxseq 3) -1711462093.654492 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:200c2) piggybacked, resched in 0.1 s (min-ack 3, avail-seq 4, xmit 3) -1711462093.654509 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462093.654709 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc009978:36 0x7408c804e65c:28 0x7408bc0097f8:32 [ udp/239.255.0.1:7400@2 ] -1711462093.654728 [0] tev: traffic-xmit (1) 116 -1711462093.654766 [0] tev: resched pmd(110cd0d:79d6eeac:ea4a8907:1c1): 8s -1711462093.654843 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462093.654910 [0] recv: INFOTS(1711462093.654299867) -1711462093.654937 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0 #4 110cd0d:79d6eeac:ea4a8907:200c2? -> 0:0:0:0) -1711462093.654957 [0] recv: HEARTBEAT(#21:4..4 110cd0d:79d6eeac:ea4a8907:200c2? -> 0:0:0:0) -1711462093.654989 [0] recv: thread_cputime 0.049123000 -1711462093.655401 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462093.655450 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462093.655498 [0] recv: ACKNACK(F#3:5/0: L(:1c1 16941.314947) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462093.655563 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462093.655582 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462093.655610 [0] recv: ACKNACK(F#3:5/0: L(:1c1 16941.315083) 110e21c:d0762c48:a9f0fb28:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462093.655757 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462093.655794 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462093.655823 [0] recv: ACKNACK(F#3:5/0: L(:1c1 16941.315293) 110443d:bb7f10a5:18901533:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462093.655854 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462093.655867 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462093.655889 [0] recv: ACKNACK(F#3:5/0: L(:1c1 16941.315368) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462093.754040 [0] tev: non-timed queue now has 1 items -1711462093.754105 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 0:0:0:100c7 (resched 8s) -1711462093.754124 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462093.754293 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462093.754337 [0] recv: INFOTS(1711462069.653526636) -1711462093.754359 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462093.754379 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462093.754385 [0] recv: INFOTS(1711462069.653526636) -1711462093.754414 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462093.754449 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.754484 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (local) -1711462093.754495 [0] dq.builtin: thread_cputime 0.018726000 -1711462093.754527 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462093.754538 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (local) -1711462093.755517 [0] tev: nn_xpack_send 444: 0x7408c000139c:20 0x7408bc009978:36 0x7408e000a504:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462093.755536 [0] tev: traffic-xmit (101) 444 -1711462093.755552 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:200c2) suppressed, resched in inf s (min-ack 4, avail-seq 4, xmit 4) -1711462098.143916 [0] tev: thread_cputime 0.022373000 -1711462098.144091 [0] tev: write_sample 110cd0d:56a27910:57318171:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:56a27910:57318171:1}:1<0> -1711462098.144165 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(110cd0d:56a27910:57318171:200c2:#2/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462098.144189 [0] tev: writer_hbcontrol: wr 110cd0d:56a27910:57318171:200c2 multicasting (rel-prd 4 seq-eq-max 4 seq 2 maxseq 1) -1711462098.144216 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:200c2) piggybacked, resched in 0.1 s (min-ack 1, avail-seq 2, xmit 1) -1711462098.144234 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462098.144435 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc009978:36 0x7408c800d1dc:28 0x7408bc0097f8:32 [ udp/239.255.0.1:7400@2 ] -1711462098.144457 [0] tev: traffic-xmit (1) 116 -1711462098.144477 [0] tev: resched pmd(110cd0d:56a27910:57318171:1c1): 8s -1711462098.144605 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462098.144722 [0] recv: INFOTS(1711462098.144063519) -1711462098.144773 [0] recv: DATA(110cd0d:56a27910:57318171:200c2 -> 0:0:0:0 #2 110cd0d:56a27910:57318171:200c2? -> 0:0:0:0) -1711462098.144792 [0] recv: HEARTBEAT(#9:2..2 110cd0d:56a27910:57318171:200c2? -> 0:0:0:0) -1711462098.144814 [0] recv: thread_cputime 0.050257000 -1711462098.145525 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462098.145586 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462098.145636 [0] recv: ACKNACK(F#3:3/0: L(:1c1 16945.805082) 110443d:bb7f10a5:18901533:200c7 -> 110cd0d:56a27910:57318171:200c2 ACK1 RM0) -1711462098.145665 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462098.145674 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462098.145690 [0] recv: ACKNACK(F#3:3/0: L(:1c1 16945.805176) 110e21c:d0762c48:a9f0fb28:200c7 -> 110cd0d:56a27910:57318171:200c2 ACK1 RM0) -1711462098.145706 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462098.145714 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462098.145729 [0] recv: ACKNACK(F#3:3/0: L(:1c1 16945.805217) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110cd0d:56a27910:57318171:200c2 ACK1 RM0) -1711462098.145742 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462098.145750 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462098.145762 [0] recv: ACKNACK(F#3:3/0: L(:1c1 16945.805252) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2 ACK1 RM0) -1711462098.244012 [0] tev: non-timed queue now has 1 items -1711462098.244120 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 0:0:0:100c7 (resched 8s) -1711462098.244150 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:200c2) suppressed, resched in inf s (min-ack 2, avail-seq 2, xmit 2) -1711462098.244174 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462098.244538 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462098.244608 [0] recv: INFOTS(1711462090.143481252) -1711462098.244649 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 0:0:0:0 #1) -1711462098.244677 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462098.244690 [0] recv: INFOTS(1711462090.143481252) -1711462098.244705 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 0:0:0:0 #1) -1711462098.244830 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.244930 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (local) -1711462098.244995 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.245021 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (local) -1711462098.245043 [0] dq.builtin: thread_cputime 0.018775000 -1711462098.246115 [0] tev: nn_xpack_send 444: 0x7408c000139c:20 0x7408bc009978:36 0x7408ac009c94:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462098.246139 [0] tev: traffic-xmit (101) 444 -1711462098.520149 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 116 from udp/10.101.12.71:58212 -1711462098.520218 [0] recv: INFOTS(1711462098.519615324) -1711462098.520261 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:200c2 -> 0:0:0:0 #2 L(:1c1 16946.179717)) -1711462098.520300 [0] recv: HEARTBEAT(#10:2..2 110e21c:d0762c48:a9f0fb28:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@2(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@2(sync) 110cd0d:79d6eeac:ea4a8907:200c7@2(sync)) -1711462098.520431 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#3:3/0: -1711462098.520468 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110e21c:d0762c48:a9f0fb28:1}:1<0> -1711462098.520601 [0] dq.builtin: PMD ST0 pp 110e21c:d0762c48:a9f0fb28 kind 1 data 1 -1711462098.520526 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462098.520721 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462098.520754 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#8:3/0: -1711462098.520771 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462098.520786 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462098.520808 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#7:3/0: -1711462098.520823 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462098.520838 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462098.520927 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7412@2 ] -1711462098.520946 [0] tev: traffic-xmit (1) 168 -1711462098.542401 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 116 from udp/10.101.12.71:40473 -1711462098.542466 [0] recv: INFOTS(1711462098.541939305) -1711462098.542537 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:200c2 -> 0:0:0:0 #2 L(:1c1 16946.201965)) -1711462098.542592 [0] recv: HEARTBEAT(#8:2..2 110d7b4:17c5b8c5:94bd0ff4:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@2(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@2(sync) 110cd0d:79d6eeac:ea4a8907:200c7@2(sync)) -1711462098.542699 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#3:3/0: -1711462098.542757 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462098.542735 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110d7b4:17c5b8c5:94bd0ff4:1}:1<0> -1711462098.542846 [0] dq.builtin: PMD ST0 pp 110d7b4:17c5b8c5:94bd0ff4 kind 1 data 1 -1711462098.542784 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462098.542963 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#8:3/0: -1711462098.542985 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462098.543002 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462098.543021 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#6:3/0: -1711462098.543034 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462098.543047 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462098.543112 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7414@2 ] -1711462098.543130 [0] tev: traffic-xmit (1) 168 -1711462098.549783 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 116 from udp/10.101.12.71:44991 -1711462098.549839 [0] recv: INFOTS(1711462098.549450527) -1711462098.549895 [0] recv: DATA(110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0 #2 L(:1c1 16946.209338)) -1711462098.549937 [0] recv: HEARTBEAT(#8:2..2 110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@2(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@2(sync) 110cd0d:79d6eeac:ea4a8907:200c7@2(sync)) -1711462098.550039 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#4:3/0: -1711462098.550120 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462098.550165 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462098.550189 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#9:3/0: -1711462098.550204 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462098.550220 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462098.550257 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#7:3/0: -1711462098.550074 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110443d:bb7f10a5:18901533:1}:1<0> -1711462098.550548 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462098.550566 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462098.550632 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7416@2 ] -1711462098.550649 [0] tev: traffic-xmit (1) 168 -1711462098.550695 [0] dq.builtin: PMD ST0 pp 110443d:bb7f10a5:18901533 kind 1 data 1 -1711462098.570830 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 116 from udp/10.101.12.71:52025 -1711462098.570866 [0] recv: INFOTS(1711462098.570608394) -1711462098.570880 [0] recv: DATA(110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0 #2 L(:1c1 16946.230367)) -1711462098.570895 [0] recv: HEARTBEAT(#6:2..2 110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@2(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@2(sync) 110cd0d:79d6eeac:ea4a8907:200c7@2(sync)) -1711462098.570930 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:200c2 #2: ST0 DCPSParticipantMessage/ParticipantMessageData:{110aba1:7b19badd:ce0adb73:1}:1<0> -1711462098.570963 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#3:3/0: -1711462098.570994 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462098.570966 [0] dq.builtin: PMD ST0 pp 110aba1:7b19badd:ce0adb73 kind 1 data 1 -1711462098.571008 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462098.571046 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#9:3/0: -1711462098.571054 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462098.571061 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462098.571074 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#7:3/0: -1711462098.571082 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462098.571090 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462098.571135 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7418@2 ] -1711462098.571148 [0] tev: traffic-xmit (1) 168 -1711462098.620188 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462098.620217 [0] recv: INFOTS(1711462090.519442682) -1711462098.620239 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462098.620261 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462098.620273 [0] recv: INFOTS(1711462090.519442682) -1711462098.620329 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462098.620488 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.620611 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16946.280106) -1711462098.620676 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.620702 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16946.280201) -1711462098.642287 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462098.642318 [0] recv: INFOTS(1711462090.541682763) -1711462098.642345 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462098.642374 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462098.642387 [0] recv: INFOTS(1711462090.541682763) -1711462098.642417 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462098.642606 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.642696 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16946.302191) -1711462098.642760 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.642786 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16946.302285) -1711462098.649984 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462098.650030 [0] recv: INFOTS(1711462090.549308855) -1711462098.650100 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462098.650114 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462098.650126 [0] recv: INFOTS(1711462090.549308855) -1711462098.650133 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462098.650331 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.650393 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16946.309891) -1711462098.650440 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.650459 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16946.309959) -1711462098.670809 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462098.670838 [0] recv: INFOTS(1711462090.570393375) -1711462098.670853 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462098.670866 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462098.670870 [0] recv: INFOTS(1711462090.570393375) -1711462098.670875 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462098.670981 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.671012 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (known) L(:1c1 16946.330512) -1711462098.671048 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462098.671077 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (known) L(:1c1 16946.330578) -1711462099.906039 [0] tev: thread_cputime 0.027066000 -1711462099.906177 [0] tev: write_sample 110cd0d:e3b81b8d:1ccb65b1:200c2 #5: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:e3b81b8d:1ccb65b1:1}:1<0> -1711462099.906479 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(110cd0d:e3b81b8d:1ccb65b1:200c2:#5/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462099.906502 [0] tev: writer_hbcontrol: wr 110cd0d:e3b81b8d:1ccb65b1:200c2 multicasting (rel-prd 4 seq-eq-max 4 seq 5 maxseq 4) -1711462099.906522 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:200c2) piggybacked, resched in 0.1 s (min-ack 4, avail-seq 5, xmit 4) -1711462099.906535 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462099.906689 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc009978:36 0x7408c800f16c:28 0x7408bc0097f8:32 [ udp/239.255.0.1:7400@2 ] -1711462099.906706 [0] tev: traffic-xmit (1) 116 -1711462099.906716 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462099.906779 [0] recv: INFOTS(1711462099.906152938) -1711462099.906817 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0 #5 110cd0d:e3b81b8d:1ccb65b1:200c2? -> 0:0:0:0) -1711462099.906833 [0] recv: HEARTBEAT(#30:5..5 110cd0d:e3b81b8d:1ccb65b1:200c2? -> 0:0:0:0) -1711462099.906849 [0] recv: thread_cputime 0.050832000 -1711462099.906722 [0] tev: resched pmd(110cd0d:e3b81b8d:1ccb65b1:1c1): 8s -1711462099.907036 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462099.907051 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462099.907090 [0] recv: ACKNACK(F#4:6/0: L(:1c1 16947.566551) 110443d:bb7f10a5:18901533:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462099.907112 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462099.907139 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462099.907159 [0] recv: ACKNACK(F#4:6/0: L(:1c1 16947.566642) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462099.907175 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462099.907182 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462099.907193 [0] recv: ACKNACK(F#4:6/0: L(:1c1 16947.566686) 110e21c:d0762c48:a9f0fb28:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462099.907356 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462099.907364 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462099.907375 [0] recv: ACKNACK(F#4:6/0: L(:1c1 16947.566867) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462100.006242 [0] tev: non-timed queue now has 1 items -1711462100.006339 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 0:0:0:100c7 (resched 8s) -1711462100.006364 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:200c2) suppressed, resched in inf s (min-ack 5, avail-seq 5, xmit 5) -1711462100.006383 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462100.006577 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462100.006641 [0] recv: INFOTS(1711462067.905323275) -1711462100.006667 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462100.006866 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462100.006876 [0] recv: INFOTS(1711462067.905323275) -1711462100.006885 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462100.006918 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462100.007009 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (local) -1711462100.007029 [0] dq.builtin: thread_cputime 0.020966000 -1711462100.007074 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462100.007099 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (local) -1711462100.008351 [0] tev: nn_xpack_send 444: 0x7408c000139c:20 0x7408bc009978:36 0x7408d401eea4:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462100.008374 [0] tev: traffic-xmit (101) 444 -1711462100.520365 [0] gc: thread_cputime 0.022248000 -1711462101.654569 [0] tev: thread_cputime 0.029555000 -1711462101.654722 [0] tev: write_sample 110cd0d:79d6eeac:ea4a8907:200c2 #5: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:79d6eeac:ea4a8907:1}:1<0> -1711462101.654772 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(110cd0d:79d6eeac:ea4a8907:200c2:#5/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462101.654793 [0] tev: writer_hbcontrol: wr 110cd0d:79d6eeac:ea4a8907:200c2 multicasting (rel-prd 4 seq-eq-max 4 seq 5 maxseq 4) -1711462101.654853 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:200c2) piggybacked, resched in 0.1 s (min-ack 4, avail-seq 5, xmit 4) -1711462101.654871 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462101.655067 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc009978:36 0x7408c8038f6c:28 0x7408bc0097f8:32 [ udp/239.255.0.1:7400@2 ] -1711462101.655090 [0] tev: traffic-xmit (1) 116 -1711462101.655110 [0] tev: resched pmd(110cd0d:79d6eeac:ea4a8907:1c1): 8s -1711462101.655193 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462101.655283 [0] recv: INFOTS(1711462101.654695551) -1711462101.655309 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0 #5 110cd0d:79d6eeac:ea4a8907:200c2? -> 0:0:0:0) -1711462101.655322 [0] recv: HEARTBEAT(#22:5..5 110cd0d:79d6eeac:ea4a8907:200c2? -> 0:0:0:0) -1711462101.655345 [0] recv: thread_cputime 0.051418000 -1711462101.655644 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462101.655688 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462101.655725 [0] recv: ACKNACK(F#4:6/0: L(:1c1 16949.315187) 110443d:bb7f10a5:18901533:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462101.655753 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462101.655781 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462101.655806 [0] recv: ACKNACK(F#4:6/0: L(:1c1 16949.315283) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462101.655832 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462101.655840 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462101.655859 [0] recv: ACKNACK(F#4:6/0: L(:1c1 16949.315343) 110e21c:d0762c48:a9f0fb28:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462101.655922 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462101.655935 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462101.655950 [0] recv: ACKNACK(F#4:6/0: L(:1c1 16949.315437) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462101.754274 [0] tev: non-timed queue now has 1 items -1711462101.754368 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 0:0:0:100c7 (resched 8s) -1711462101.754394 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462101.754614 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462101.754683 [0] recv: INFOTS(1711462069.653526636) -1711462101.754735 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462101.754761 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462101.754778 [0] recv: INFOTS(1711462069.653526636) -1711462101.754792 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462101.754985 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462101.755063 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (local) -1711462101.755170 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462101.755223 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (local) -1711462101.755245 [0] dq.builtin: thread_cputime 0.021307000 -1711462101.756482 [0] tev: nn_xpack_send 444: 0x7408c000139c:20 0x7408bc009978:36 0x7408e000a504:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462101.756537 [0] tev: traffic-xmit (101) 444 -1711462101.756562 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:200c2) suppressed, resched in inf s (min-ack 5, avail-seq 5, xmit 5) -1711462106.144086 [0] tev: thread_cputime 0.032640000 -1711462106.144192 [0] tev: write_sample 110cd0d:56a27910:57318171:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:56a27910:57318171:1}:1<0> -1711462106.144229 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(110cd0d:56a27910:57318171:200c2:#3/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462106.144244 [0] tev: writer_hbcontrol: wr 110cd0d:56a27910:57318171:200c2 multicasting (rel-prd 4 seq-eq-max 4 seq 3 maxseq 2) -1711462106.144261 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:200c2) piggybacked, resched in 0.1 s (min-ack 2, avail-seq 3, xmit 2) -1711462106.144273 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462106.144408 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc009978:36 0x7408c804e65c:28 0x7408bc0097f8:32 [ udp/239.255.0.1:7400@2 ] -1711462106.144422 [0] tev: traffic-xmit (1) 116 -1711462106.144447 [0] tev: resched pmd(110cd0d:56a27910:57318171:1c1): 8s -1711462106.144549 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462106.144618 [0] recv: INFOTS(1711462106.144172669) -1711462106.144645 [0] recv: DATA(110cd0d:56a27910:57318171:200c2 -> 0:0:0:0 #3 110cd0d:56a27910:57318171:200c2? -> 0:0:0:0) -1711462106.144659 [0] recv: HEARTBEAT(#10:3..3 110cd0d:56a27910:57318171:200c2? -> 0:0:0:0) -1711462106.144674 [0] recv: thread_cputime 0.052353000 -1711462106.144945 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462106.144974 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462106.145003 [0] recv: ACKNACK(F#4:4/0: L(:1c1 16953.804473) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110cd0d:56a27910:57318171:200c2 ACK1 RM0) -1711462106.145028 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462106.145046 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462106.145062 [0] recv: ACKNACK(F#4:4/0: L(:1c1 16953.804548) 110e21c:d0762c48:a9f0fb28:200c7 -> 110cd0d:56a27910:57318171:200c2 ACK1 RM0) -1711462106.145081 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462106.145091 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462106.145105 [0] recv: ACKNACK(F#4:4/0: L(:1c1 16953.804593) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:56a27910:57318171:200c2 ACK1 RM0) -1711462106.145122 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462106.145133 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462106.145147 [0] recv: ACKNACK(F#4:4/0: L(:1c1 16953.804635) 110443d:bb7f10a5:18901533:200c7 -> 110cd0d:56a27910:57318171:200c2 ACK1 RM0) -1711462106.244177 [0] tev: non-timed queue now has 1 items -1711462106.244239 [0] tev: xmit spdp 110cd0d:56a27910:57318171:1c1 to 0:0:0:100c7 (resched 8s) -1711462106.244257 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:200c2) suppressed, resched in inf s (min-ack 3, avail-seq 3, xmit 3) -1711462106.244271 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462106.244457 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462106.244488 [0] recv: INFOTS(1711462090.143481252) -1711462106.244537 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 0:0:0:0 #1) -1711462106.244560 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462106.244581 [0] recv: INFOTS(1711462090.143481252) -1711462106.244595 [0] recv: DATA(110cd0d:56a27910:57318171:100c2 -> 0:0:0:0 #1) -1711462106.244661 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.244711 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (local) -1711462106.244736 [0] dq.builtin: thread_cputime 0.021725000 -1711462106.244775 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:56a27910:57318171:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.244811 [0] dq.builtin: SPDP ST0 110cd0d:56a27910:57318171:1c1 (local) -1711462106.245770 [0] tev: nn_xpack_send 444: 0x7408c000139c:20 0x7408bc009978:36 0x7408ac009c94:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462106.245790 [0] tev: traffic-xmit (101) 444 -1711462106.520045 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 116 from udp/10.101.12.71:58212 -1711462106.520082 [0] recv: INFOTS(1711462106.519773993) -1711462106.520113 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:200c2 -> 0:0:0:0 #3 L(:1c1 16954.179582)) -1711462106.520137 [0] recv: HEARTBEAT(#11:3..3 110e21c:d0762c48:a9f0fb28:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@3(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@3(sync) 110cd0d:79d6eeac:ea4a8907:200c7@3(sync)) -1711462106.520229 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110e21c:d0762c48:a9f0fb28:1}:1<0> -1711462106.520260 [0] dq.builtin: PMD ST0 pp 110e21c:d0762c48:a9f0fb28 kind 1 data 1 -1711462106.520230 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#4:4/0: -1711462106.520344 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462106.520357 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462106.520369 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#9:4/0: -1711462106.520378 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462106.520385 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462106.520395 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110e21c:d0762c48:a9f0fb28:200c2: F#8:4/0: -1711462106.520416 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110e21c:d0762c48:a9f0fb28:200c2) -1711462106.520422 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462106.520457 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7412@2 ] -1711462106.520465 [0] tev: traffic-xmit (1) 168 -1711462106.542274 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 116 from udp/10.101.12.71:40473 -1711462106.542310 [0] recv: INFOTS(1711462106.542026862) -1711462106.542327 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:200c2 -> 0:0:0:0 #3 L(:1c1 16954.201811)) -1711462106.542350 [0] recv: HEARTBEAT(#9:3..3 110d7b4:17c5b8c5:94bd0ff4:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@3(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@3(sync) 110cd0d:79d6eeac:ea4a8907:200c7@3(sync)) -1711462106.542391 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110d7b4:17c5b8c5:94bd0ff4:1}:1<0> -1711462106.542421 [0] dq.builtin: PMD ST0 pp 110d7b4:17c5b8c5:94bd0ff4 kind 1 data 1 -1711462106.542494 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#4:4/0: -1711462106.542535 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462106.542551 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462106.542565 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#9:4/0: -1711462106.542574 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462106.542581 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462106.542594 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110d7b4:17c5b8c5:94bd0ff4:200c2: F#7:4/0: -1711462106.542602 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462106.542615 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462106.542652 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7414@2 ] -1711462106.542661 [0] tev: traffic-xmit (1) 168 -1711462106.549911 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 116 from udp/10.101.12.71:44991 -1711462106.549989 [0] recv: INFOTS(1711462106.549523877) -1711462106.550019 [0] recv: DATA(110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0 #3 L(:1c1 16954.209488)) -1711462106.550073 [0] recv: HEARTBEAT(#9:3..3 110443d:bb7f10a5:18901533:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@3(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@3(sync) 110cd0d:79d6eeac:ea4a8907:200c7@3(sync)) -1711462106.550131 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110443d:bb7f10a5:18901533:1}:1<0> -1711462106.550155 [0] dq.builtin: PMD ST0 pp 110443d:bb7f10a5:18901533 kind 1 data 1 -1711462106.550187 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#5:4/0: -1711462106.550237 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462106.550259 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462106.550279 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#10:4/0: -1711462106.550293 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462106.550307 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462106.550356 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110443d:bb7f10a5:18901533:200c2: F#8:4/0: -1711462106.550379 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110443d:bb7f10a5:18901533:200c2) -1711462106.550394 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462106.550542 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7416@2 ] -1711462106.550562 [0] tev: traffic-xmit (1) 168 -1711462106.571348 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 116 from udp/10.101.12.71:52025 -1711462106.571441 [0] recv: INFOTS(1711462106.570832443) -1711462106.571474 [0] recv: DATA(110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0 #3 L(:1c1 16954.230938)) -1711462106.571512 [0] recv: HEARTBEAT(#7:3..3 110aba1:7b19badd:ce0adb73:200c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:200c7@3(sync) 110cd0d:e3b81b8d:1ccb65b1:200c7@3(sync) 110cd0d:79d6eeac:ea4a8907:200c7@3(sync)) -1711462106.571701 [0] tev: acknack 110cd0d:56a27910:57318171:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#4:4/0: -1711462106.571783 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462106.571805 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462106.571832 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#10:4/0: -1711462106.571848 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462106.571717 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:200c2 #3: ST0 DCPSParticipantMessage/ParticipantMessageData:{110aba1:7b19badd:ce0adb73:1}:1<0> -1711462106.571913 [0] dq.builtin: PMD ST0 pp 110aba1:7b19badd:ce0adb73 kind 1 data 1 -1711462106.571862 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462106.572041 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:200c7 -> 110aba1:7b19badd:ce0adb73:200c2: F#8:4/0: -1711462106.572064 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:200c7 -> pwr 110aba1:7b19badd:ce0adb73:200c2) -1711462106.572082 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462106.572155 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc009968:44 0x7408bc0097d0:24 0x7408bc0097f8:28 0x7408bc0094d0:24 0x7408bc0094f8:28 [ udp/10.101.12.71:7418@2 ] -1711462106.572173 [0] tev: traffic-xmit (1) 168 -1711462106.620328 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462106.620363 [0] recv: INFOTS(1711462090.519442682) -1711462106.620382 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462106.620396 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 444 from udp/10.101.12.71:58212 -1711462106.620404 [0] recv: INFOTS(1711462090.519442682) -1711462106.620413 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #1) -1711462106.620474 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.620519 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16954.280019) -1711462106.620551 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192695",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110e21c:d0762c48:a9f0fb28:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7413},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7412},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.620575 [0] dq.builtin: SPDP ST0 110e21c:d0762c48:a9f0fb28:1c1 (known) L(:1c1 16954.280077) -1711462106.642571 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462106.642616 [0] recv: INFOTS(1711462090.541682763) -1711462106.642640 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462106.642674 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 444 from udp/10.101.12.71:40473 -1711462106.642685 [0] recv: INFOTS(1711462090.541682763) -1711462106.642695 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #1) -1711462106.642764 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.642799 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16954.302298) -1711462106.642842 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192693",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7415},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7414},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.642860 [0] dq.builtin: SPDP ST0 110d7b4:17c5b8c5:94bd0ff4:1c1 (known) L(:1c1 16954.302362) -1711462106.650211 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462106.650279 [0] recv: INFOTS(1711462090.549308855) -1711462106.650329 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462106.650355 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 444 from udp/10.101.12.71:44991 -1711462106.650370 [0] recv: INFOTS(1711462090.549308855) -1711462106.650397 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #1) -1711462106.650473 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.650564 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16954.310060) -1711462106.650622 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192691",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110443d:bb7f10a5:18901533:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7417},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7416},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.650648 [0] dq.builtin: SPDP ST0 110443d:bb7f10a5:18901533:1c1 (known) L(:1c1 16954.310147) -1711462106.671265 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462106.671327 [0] recv: INFOTS(1711462090.570393375) -1711462106.671384 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462106.671429 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 444 from udp/10.101.12.71:52025 -1711462106.671451 [0] recv: INFOTS(1711462090.570393375) -1711462106.671473 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #1) -1711462106.671536 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.671595 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (known) L(:1c1 16954.331090) -1711462106.671659 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"192697",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110aba1:7b19badd:ce0adb73:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7419},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7418},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462106.671716 [0] dq.builtin: SPDP ST0 110aba1:7b19badd:ce0adb73:1c1 (known) L(:1c1 16954.331214) -1711462107.906312 [0] tev: thread_cputime 0.036129000 -1711462107.906461 [0] tev: write_sample 110cd0d:e3b81b8d:1ccb65b1:200c2 #6: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:e3b81b8d:1ccb65b1:1}:1<0> -1711462107.906511 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(110cd0d:e3b81b8d:1ccb65b1:200c2:#6/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462107.906533 [0] tev: writer_hbcontrol: wr 110cd0d:e3b81b8d:1ccb65b1:200c2 multicasting (rel-prd 4 seq-eq-max 4 seq 6 maxseq 5) -1711462107.906560 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:200c2) piggybacked, resched in 0.1 s (min-ack 5, avail-seq 6, xmit 5) -1711462107.906577 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462107.906764 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc009978:36 0x7408c800d1dc:28 0x7408bc0097f8:32 [ udp/239.255.0.1:7400@2 ] -1711462107.906826 [0] tev: traffic-xmit (1) 116 -1711462107.906848 [0] tev: resched pmd(110cd0d:e3b81b8d:1ccb65b1:1c1): 8s -1711462107.906850 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462107.906935 [0] recv: INFOTS(1711462107.906432793) -1711462107.906980 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:200c2 -> 0:0:0:0 #6 110cd0d:e3b81b8d:1ccb65b1:200c2? -> 0:0:0:0) -1711462107.907018 [0] recv: HEARTBEAT(#31:6..6 110cd0d:e3b81b8d:1ccb65b1:200c2? -> 0:0:0:0) -1711462107.907043 [0] recv: thread_cputime 0.054932000 -1711462107.907514 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462107.907568 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462107.907610 [0] recv: ACKNACK(F#5:7/0: L(:1c1 16955.567064) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462107.907643 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462107.907658 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462107.907681 [0] recv: ACKNACK(F#5:7/0: L(:1c1 16955.567159) 110443d:bb7f10a5:18901533:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462107.907706 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462107.907719 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462107.907739 [0] recv: ACKNACK(F#5:7/0: L(:1c1 16955.567220) 110e21c:d0762c48:a9f0fb28:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462107.907761 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462107.907774 [0] recv: INFODST(110cd0d:e3b81b8d:1ccb65b1) -1711462107.907794 [0] recv: ACKNACK(F#5:7/0: L(:1c1 16955.567275) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110cd0d:e3b81b8d:1ccb65b1:200c2 ACK1 RM0) -1711462108.006514 [0] tev: non-timed queue now has 1 items -1711462108.006629 [0] tev: xmit spdp 110cd0d:e3b81b8d:1ccb65b1:1c1 to 0:0:0:100c7 (resched 8s) -1711462108.006660 [0] tev: heartbeat(wr 110cd0d:e3b81b8d:1ccb65b1:200c2) suppressed, resched in inf s (min-ack 6, avail-seq 6, xmit 6) -1711462108.006683 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462108.007061 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462108.007195 [0] recv: INFOTS(1711462067.905323275) -1711462108.007238 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462108.007270 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462108.007288 [0] recv: INFOTS(1711462067.905323275) -1711462108.007307 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #1) -1711462108.007399 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462108.007460 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (local) -1711462108.007581 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462108.007632 [0] dq.builtin: SPDP ST0 110cd0d:e3b81b8d:1ccb65b1:1c1 (local) -1711462108.007656 [0] dq.builtin: thread_cputime 0.021725000 -1711462108.009175 [0] tev: nn_xpack_send 444: 0x7408c000139c:20 0x7408bc009978:36 0x7408d401eea4:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462108.009236 [0] tev: traffic-xmit (101) 444 -1711462109.654729 [0] tev: thread_cputime 0.039614000 -1711462109.654805 [0] tev: write_sample 110cd0d:79d6eeac:ea4a8907:200c2 #6: ST0 DCPSParticipantMessage/ParticipantMessageData:{110cd0d:79d6eeac:ea4a8907:1}:1<0> -1711462109.654830 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(110cd0d:79d6eeac:ea4a8907:200c2:#6/1)): niov 0 sz 0 => now niov 3 sz 84 -1711462109.654842 [0] tev: writer_hbcontrol: wr 110cd0d:79d6eeac:ea4a8907:200c2 multicasting (rel-prd 4 seq-eq-max 4 seq 6 maxseq 5) -1711462109.654854 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:200c2) piggybacked, resched in 0.1 s (min-ack 5, avail-seq 6, xmit 5) -1711462109.654862 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 3 sz 84 => now niov 4 sz 116 -1711462109.654951 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 116 from udp/10.101.12.71:50807 -1711462109.654977 [0] recv: INFOTS(1711462109.654791672) -1711462109.654958 [0] tev: nn_xpack_send 116: 0x7408c000139c:20 0x7408bc009978:36 0x7408c800f16c:28 0x7408bc0097f8:32 [ udp/239.255.0.1:7400@2 ] -1711462109.654993 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:200c2 -> 0:0:0:0 #6 110cd0d:79d6eeac:ea4a8907:200c2? -> 0:0:0:0) -1711462109.655007 [0] tev: traffic-xmit (1) 116 -1711462109.655028 [0] tev: resched pmd(110cd0d:79d6eeac:ea4a8907:1c1): 8s -1711462109.655029 [0] recv: HEARTBEAT(#23:6..6 110cd0d:79d6eeac:ea4a8907:200c2? -> 0:0:0:0) -1711462109.655066 [0] recv: thread_cputime 0.055966000 -1711462109.655265 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462109.655275 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462109.655292 [0] recv: ACKNACK(F#5:7/0: L(:1c1 16957.314777) 110d7b4:17c5b8c5:94bd0ff4:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462109.655308 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462109.655316 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462109.655327 [0] recv: ACKNACK(F#5:7/0: L(:1c1 16957.314818) 110443d:bb7f10a5:18901533:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462109.655339 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462109.655346 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462109.655357 [0] recv: ACKNACK(F#5:7/0: L(:1c1 16957.314849) 110aba1:7b19badd:ce0adb73:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462109.655369 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462109.655376 [0] recv: INFODST(110cd0d:79d6eeac:ea4a8907) -1711462109.655387 [0] recv: ACKNACK(F#5:7/0: L(:1c1 16957.314879) 110e21c:d0762c48:a9f0fb28:200c7 -> 110cd0d:79d6eeac:ea4a8907:200c2 ACK1 RM0) -1711462109.754465 [0] tev: non-timed queue now has 1 items -1711462109.754538 [0] tev: xmit spdp 110cd0d:79d6eeac:ea4a8907:1c1 to 0:0:0:100c7 (resched 8s) -1711462109.754558 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 444 -1711462109.754753 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462109.754789 [0] recv: INFOTS(1711462069.653526636) -1711462109.754816 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462109.754838 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 444 from udp/10.101.12.71:50807 -1711462109.754850 [0] recv: INFOTS(1711462069.653526636) -1711462109.754865 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #1) -1711462109.754929 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462109.754991 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (local) -1711462109.755009 [0] dq.builtin: thread_cputime 0.021789000 -1711462109.755049 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #1: ST0 /ParticipantBuiltinTopicData:{user_data=10<"enclave=/;">,property_list={1:"__ProcessName":"python3",1:"__Pid":"191354",1:"__Hostname":"op-Alienware-x16-R1"}:{},protocol_version=2:1,vendorid=1:16,participant_lease_duration=10000000000,participant_guid={110cd0d:79d6eeac:ea4a8907:1c1},builtin_endpoint_set=64575,domain_id=0,default_unicast_locator={udp/10.101.12.71:7411},default_multicast_locator={udp/239.255.0.1:7401},metatraffic_unicast_locator={udp/10.101.12.71:7410},metatraffic_multicast_locator={udp/239.255.0.1:7400},adlink_participant_version_info=0:44:0:0:0:"op-Alienware-x16-R1/0.10.4/Linux/Linux",cyclone_receive_buffer_size=2147483647} -1711462109.755083 [0] dq.builtin: SPDP ST0 110cd0d:79d6eeac:ea4a8907:1c1 (local) -1711462109.756234 [0] tev: nn_xpack_send 444: 0x7408c000139c:20 0x7408bc009978:36 0x7408e000a504:388 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.(trunc) -1711462109.756277 [0] tev: traffic-xmit (101) 444 -1711462109.756296 [0] tev: heartbeat(wr 110cd0d:79d6eeac:ea4a8907:200c2) suppressed, resched in inf s (min-ack 6, avail-seq 6, xmit 6) -1711462109.907177 [0] gc: thread_cputime 0.022465000 -1711462110.150810 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162 -1711462110.150862 [0] python3: xpack_addmsg 0x7408c4010670 0x7408bc009880 0(data(110cd0d:56a27910:57318171:403:#13/1)): niov 0 sz 0 => now niov 3 sz 504 -1711462110.150873 [0] python3: writer_hbcontrol: wr 110cd0d:56a27910:57318171:403 multicasting (rel-prd 4 seq-eq-max 4 seq 13 maxseq 12) -1711462110.150885 [0] python3: heartbeat(wr 110cd0d:56a27910:57318171:403) piggybacked, resched in 0.1 s (min-ack 12, avail-seq 13, xmit 12) -1711462110.150891 [0] python3: xpack_addmsg 0x7408c4010670 0x7408bc009700 0(control): niov 3 sz 504 => now niov 4 sz 536 -1711462110.150979 [0] python3: nn_xpack_send 536: 0x7408c401067c:20 0x7408bc009978:36 0x7408ac03d4c0:448 0x7408bc0097f8:32 [ udp/239.255.0.1:7401@2 ] -1711462110.150984 [0] python3: traffic-xmit (1) 536 -1711462110.150991 [0] python3: => EVERYONE -1711462110.151008 [0] recvMC: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 536 from udp/10.101.12.71:50807 -1711462110.151053 [0] recvMC: INFOTS(1711462110.150718264) -1711462110.151114 [0] recvMC: DATA(110cd0d:56a27910:57318171:403 -> 0:0:0:0 #13 110cd0d:56a27910:57318171:403? -> 0:0:0:0) -1711462110.151123 [0] recvMC: HEARTBEAT(#9:13..13 110cd0d:56a27910:57318171:403? -> 0:0:0:0) -1711462110.151038 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:603) ... -1711462110.151187 [0] python3: writer_set_state(110cd0d:56a27910:57318171:603) state transition 0 -> 1 -1711462110.151203 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:603) ... -1711462110.151214 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:603) - no unack'ed samples -1711462110.151224 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:603) ... -1711462110.151135 [0] recvMC: thread_cputime 0.014132000 -1711462110.151242 [0] python3: => EVERYONE -1711462110.151310 [0] python3: writer_set_state(110cd0d:56a27910:57318171:603) state transition 1 -> 3 -1711462110.151387 [0] gc: gc 0x7408ac0322b0: deleting -1711462110.151413 [0] gc: gc_delete_writer(0x7408ac0322b0, 110cd0d:56a27910:57318171:603) -1711462110.151453 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #10: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:603}} -1711462110.151474 [0] gc: non-timed queue now has 1 items -1711462110.151498 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:603 @ 0x7408ac021dc4) user 22 builtin 12 -1711462110.151496 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(data(110cd0d:56a27910:57318171:3c2:#10/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.151507 [0] recvUC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.151511 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.151548 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462110.151579 [0] recvUC: ACKNACK(F#3:14/0: L(:1c1 16957.811048) 110e21c:d0762c48:a9f0fb28:504 -> 110cd0d:56a27910:57318171:403 ACK1 RM0) -1711462110.151591 [0] recvUC: thread_cputime 0.007257000 -1711462110.151604 [0] recvUC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.151612 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462110.151616 [0] tev: nn_xpack_send 96: 0x7408c000139c:20 0x7408c4011ae8:48 0x7408c8073ec4:28 [ udp/239.255.0.1:7400@2 ] -1711462110.151654 [0] tev: traffic-xmit (1) 96 -1711462110.151660 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 96 from udp/10.101.12.71:50807 -1711462110.151686 [0] recv: INFOTS(1711462110.151439386) -1711462110.151700 [0] recv: DATA(110cd0d:56a27910:57318171:3c2 -> 0:0:0:0 #10 110cd0d:56a27910:57318171:3c2? -> 0:0:0:0) -1711462110.151631 [0] recvUC: ACKNACK(F#3:14/0: L(:1c1 16957.811115) 110443d:bb7f10a5:18901533:504 -> 110cd0d:56a27910:57318171:403 ACK1 RM0) -1711462110.151777 [0] recvUC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462110.151784 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462110.151804 [0] recvUC: ACKNACK(F#3:14/0: L(:1c1 16957.811286) 110aba1:7b19badd:ce0adb73:504 -> 110cd0d:56a27910:57318171:403 ACK1 RM0) -1711462110.151812 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.151817 [0] recvUC: INFODST(110cd0d:56a27910:57318171) -1711462110.151831 [0] recvUC: ACKNACK(F#3:14/0: L(:1c1 16957.811321) 110d7b4:17c5b8c5:94bd0ff4:504 -> 110cd0d:56a27910:57318171:403 ACK1 RM0) -1711462110.152565 [0] gc: gc 0x7408c8016150: deleting -1711462110.176530 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 472 from udp/10.101.12.71:44991 -1711462110.176592 [0] recvMC: INFOTS(1711462110.176055959) -1711462110.176622 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #11 L(:1c1 16957.836092) => EVERYONE -1711462110.176813 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #11: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,17,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,16,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.176846 [0] recvMC: HEARTBEAT(#19:11..11 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@11(sync) 110cd0d:e3b81b8d:1ccb65b1:504@11(sync) 110cd0d:79d6eeac:ea4a8907:504@11(sync)) -1711462110.176867 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 600 from udp/10.101.12.71:52025 -1711462110.176879 [0] recvMC: INFOTS(1711462110.176561088) -1711462110.176884 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110443d:bb7f10a5:18901533:403: F#4:12/0: -1711462110.176997 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.177013 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.177033 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110443d:bb7f10a5:18901533:403: F#20:12/0: -1711462110.177239 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.177265 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.176919 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #13 L(:1c1 16957.836380) => EVERYONE -1711462110.177290 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110443d:bb7f10a5:18901533:403: F#11:12/0: -1711462110.177307 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.177314 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.177367 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7417@2 ] -1711462110.177384 [0] tev: traffic-xmit (1) 168 -1711462110.177138 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.177439 [0] recv: INFOTS(1711462110.176811721) -1711462110.177463 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #8 L(:1c1 16957.836934)) -1711462110.177514 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #8: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1304}} -1711462110.177670 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1304 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:1304) => EVERYONE -1711462110.177704 [0] dq.builtin: - deleting -1711462110.177719 [0] dq.builtin: delete -1711462110.177739 [0] gc: gc 0x7408c80951c0: not yet, shortsleep -1711462110.177605 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,) -1711462110.177887 [0] recvMC: HEARTBEAT(#23:13..13 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@13(sync) 110cd0d:e3b81b8d:1ccb65b1:504@13(sync) 110cd0d:79d6eeac:ea4a8907:504@13(sync)) -1711462110.177905 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403: F#4:14/0: -1711462110.177917 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462110.177935 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.177943 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403: F#20:14/0: -1711462110.177958 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462110.177965 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.177971 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403: F#11:14/0: -1711462110.177976 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462110.177984 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.178040 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7419@2 ] -1711462110.178047 [0] tev: traffic-xmit (1) 168 -1711462110.178621 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.178650 [0] recv: INFOTS(1711462110.178392915) -1711462110.178670 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #11 L(:1c1 16957.838150)) -1711462110.178724 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1304}} -1711462110.178742 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1304 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:1304) => EVERYONE -1711462110.178767 [0] dq.builtin: - deleting -1711462110.178780 [0] dq.builtin: delete -1711462110.178860 [0] gc: gc 0x7408c80951c0: deleting -1711462110.178884 [0] gc: gc_delete_proxy_reader (0x7408c80951c0, 110443d:bb7f10a5:18901533:1304) -1711462110.178898 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=28 -1711462110.178916 [0] gc: gc 0x7408c80953f0: deleting -1711462110.178925 [0] gc: gc_delete_proxy_reader (0x7408c80953f0, 110aba1:7b19badd:ce0adb73:1304) -1711462110.178935 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=33 -1711462110.178951 [0] gc: gc 0x7408c8016150: deleting -1711462110.178962 [0] gc: gc 0x7408c80953f0: deleting -1711462110.179459 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.179488 [0] recv: INFOTS(1711462110.179299892) -1711462110.179512 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #11 L(:1c1 16957.838988)) -1711462110.179617 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #11: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1203}} -1711462110.179651 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1203 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:1203) - deleting -1711462110.179667 [0] dq.builtin: => EVERYONE -1711462110.179716 [0] dq.builtin: delete -1711462110.179728 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 424 from udp/10.101.12.71:44991 -1711462110.179733 [0] gc: gc 0x7408c807ae70: deleting -1711462110.179756 [0] recvMC: INFOTS(1711462110.179423769) -1711462110.179781 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807ae70, 110443d:bb7f10a5:18901533:1203) -1711462110.179784 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #12 L(:1c1 16957.839257) => EVERYONE -1711462110.179836 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807ae70, 110443d:bb7f10a5:18901533:1203) -1711462110.179866 [0] dq.user: thread_cputime 0.002164000 -1711462110.179869 [0] gc: gc 0x7408c807ae70: deleting -1711462110.179895 [0] gc: gc_delete_proxy_writer (0x7408c807ae70, 110443d:bb7f10a5:18901533:1203) -1711462110.179914 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=27 -1711462110.179953 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.179990 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,15,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.180001 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.180023 [0] recv: INFOTS(1711462110.179720416) -1711462110.180012 [0] recvMC: HEARTBEAT(F#20:12..12 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@12(sync) 110cd0d:e3b81b8d:1ccb65b1:504@12(sync) 110cd0d:79d6eeac:ea4a8907:504@12(sync)) -1711462110.180063 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #13 L(:1c1 16957.839524)) -1711462110.180134 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1203}} -1711462110.180152 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1203 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:1203) - deleting -1711462110.180164 [0] dq.builtin: => EVERYONE -1711462110.180200 [0] dq.builtin: delete -1711462110.180260 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 552 from udp/10.101.12.71:52025 -1711462110.180288 [0] recvMC: INFOTS(1711462110.179892567) -1711462110.180309 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #14 L(:1c1 16957.839788) => EVERYONE -1711462110.180439 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,) -1711462110.180481 [0] recvMC: HEARTBEAT(F#24:14..14 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@14(sync) 110cd0d:e3b81b8d:1ccb65b1:504@14(sync) 110cd0d:79d6eeac:ea4a8907:504@14(sync)) -1711462110.180794 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.180806 [0] recv: INFOTS(1711462110.180647407) -1711462110.180827 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #9 L(:1c1 16957.840308)) -1711462110.180858 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #9: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1104}} -1711462110.180878 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1104 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:1104) => EVERYONE -1711462110.180897 [0] dq.builtin: - deleting -1711462110.180908 [0] dq.builtin: delete -1711462110.181027 [0] gc: gc 0x7408c8016150: deleting -1711462110.181045 [0] gc: gc 0x7408c8067c90: deleting -1711462110.181054 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8067c90, 110aba1:7b19badd:ce0adb73:1203) -1711462110.181069 [0] gc: gc 0x7408c807ae70: deleting -1711462110.181077 [0] gc: gc_delete_proxy_reader (0x7408c807ae70, 110443d:bb7f10a5:18901533:1104) -1711462110.181091 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=26 -1711462110.181080 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8067c90, 110aba1:7b19badd:ce0adb73:1203) -1711462110.181107 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.181181 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.181208 [0] recv: INFOTS(1711462110.181018038) -1711462110.181226 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #12 L(:1c1 16957.840708)) -1711462110.181258 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1104}} -1711462110.181286 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1104 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:1104) => EVERYONE -1711462110.181306 [0] dq.builtin: - deleting -1711462110.181315 [0] dq.builtin: delete -1711462110.181992 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.182009 [0] recv: INFOTS(1711462110.181868318) -1711462110.182024 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #12 L(:1c1 16957.841510)) -1711462110.182048 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #12: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1003}} -1711462110.182063 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1003 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:1003) - deleting -1711462110.182072 [0] dq.builtin: => EVERYONE -1711462110.182101 [0] dq.builtin: delete -1711462110.182195 [0] gc: gc 0x7408c8016150: deleting -1711462110.182202 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 376 from udp/10.101.12.71:44991 -1711462110.182206 [0] gc: gc 0x7408c8067c90: deleting -1711462110.182262 [0] gc: gc_delete_proxy_writer (0x7408c8067c90, 110aba1:7b19badd:ce0adb73:1203) -1711462110.182280 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=32 -1711462110.182234 [0] recvMC: INFOTS(1711462110.181963719) -1711462110.182293 [0] gc: gc 0x7408c807ea40: deleting -1711462110.182304 [0] gc: gc_delete_proxy_reader (0x7408c807ea40, 110aba1:7b19badd:ce0adb73:1104) -1711462110.182317 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=31 -1711462110.182334 [0] gc: gc 0x7408c8036c70: deleting -1711462110.182347 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8036c70, 110443d:bb7f10a5:18901533:1003) -1711462110.182370 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.182379 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #13 L(:1c1 16957.841735) => EVERYONE -1711462110.182389 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8036c70, 110443d:bb7f10a5:18901533:1003) -1711462110.182456 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.182472 [0] recv: INFOTS(1711462110.182296975) -1711462110.182492 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #14 L(:1c1 16957.841974)) -1711462110.182493 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,13,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.182531 [0] recvMC: HEARTBEAT(F#21:13..13 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@13(sync) 110cd0d:e3b81b8d:1ccb65b1:504@13(sync) 110cd0d:79d6eeac:ea4a8907:504@13(sync)) -1711462110.182536 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1003}} -1711462110.182576 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1003 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:1003) - deleting -1711462110.182591 [0] dq.builtin: => EVERYONE -1711462110.182633 [0] dq.builtin: delete -1711462110.182648 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 504 from udp/10.101.12.71:52025 -1711462110.182663 [0] recvMC: INFOTS(1711462110.182388702) -1711462110.182683 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #15 L(:1c1 16957.842165) => EVERYONE -1711462110.182800 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,) -1711462110.182830 [0] recvMC: HEARTBEAT(F#25:15..15 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@15(sync) 110cd0d:e3b81b8d:1ccb65b1:504@15(sync) 110cd0d:79d6eeac:ea4a8907:504@15(sync)) -1711462110.183201 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.183222 [0] recv: INFOTS(1711462110.183032571) -1711462110.183238 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #10 L(:1c1 16957.842723)) -1711462110.183275 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #10: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:f04}} -1711462110.183303 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:f04 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:f04) => EVERYONE -1711462110.183339 [0] dq.builtin: - deleting -1711462110.183353 [0] dq.builtin: delete -1711462110.183443 [0] gc: gc 0x7408c8016150: deleting -1711462110.183455 [0] gc: gc 0x7408c807ea40: deleting -1711462110.183465 [0] gc: gc 0x7408c8036c70: deleting -1711462110.183472 [0] gc: gc_delete_proxy_writer (0x7408c8036c70, 110443d:bb7f10a5:18901533:1003) -1711462110.183482 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=25 -1711462110.183492 [0] gc: gc 0x7408c8067c90: deleting -1711462110.183503 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8067c90, 110aba1:7b19badd:ce0adb73:1003) -1711462110.183516 [0] gc: gc 0x7408c808c670: deleting -1711462110.183523 [0] gc: gc_delete_proxy_reader (0x7408c808c670, 110443d:bb7f10a5:18901533:f04) -1711462110.183532 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=24 -1711462110.183545 [0] gc: gc 0x7408c8016150: deleting -1711462110.183555 [0] gc: gc 0x7408c808c670: not yet, shortsleep -1711462110.183531 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8067c90, 110aba1:7b19badd:ce0adb73:1003) -1711462110.183800 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.183816 [0] recv: INFOTS(1711462110.183595883) -1711462110.183844 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #13 L(:1c1 16957.843318)) -1711462110.183887 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:f04}} -1711462110.183931 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:f04 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:f04) => EVERYONE -1711462110.183970 [0] dq.builtin: - deleting -1711462110.183986 [0] dq.builtin: delete -1711462110.184379 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.184393 [0] recv: INFOTS(1711462110.184270366) -1711462110.184408 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #13 L(:1c1 16957.843895)) -1711462110.184445 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:e03}} -1711462110.184467 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:e03 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:e03) - deleting -1711462110.184479 [0] dq.builtin: => EVERYONE -1711462110.184539 [0] dq.builtin: delete -1711462110.184591 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 328 from udp/10.101.12.71:44991 -1711462110.184622 [0] gc: gc 0x7408c808c670: deleting -1711462110.184640 [0] gc: gc 0x7408c8067c90: deleting -1711462110.184658 [0] gc: gc_delete_proxy_writer (0x7408c8067c90, 110aba1:7b19badd:ce0adb73:1003) -1711462110.184627 [0] recvMC: INFOTS(1711462110.184354578) -1711462110.184671 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=30 -1711462110.184743 [0] gc: gc 0x7408c8043890: deleting -1711462110.184749 [0] gc: gc_delete_proxy_reader (0x7408c8043890, 110aba1:7b19badd:ce0adb73:f04) -1711462110.184757 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=29 -1711462110.184765 [0] gc: gc 0x7408c808af10: deleting -1711462110.184768 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #14 L(:1c1 16957.844127) => EVERYONE -1711462110.184772 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c808af10, 110443d:bb7f10a5:18901533:e03) -1711462110.184811 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.184828 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c808af10, 110443d:bb7f10a5:18901533:e03) -1711462110.184890 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,11,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.184920 [0] recvMC: HEARTBEAT(F#22:14..14 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@14(sync) 110cd0d:e3b81b8d:1ccb65b1:504@14(sync) 110cd0d:79d6eeac:ea4a8907:504@14(sync)) -1711462110.185199 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.185211 [0] recv: INFOTS(1711462110.185066146) -1711462110.185235 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #15 L(:1c1 16957.844714)) -1711462110.185271 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:e03}} -1711462110.185302 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:e03 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:e03) - deleting -1711462110.185317 [0] dq.builtin: => EVERYONE -1711462110.185356 [0] dq.builtin: delete -1711462110.185435 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 456 from udp/10.101.12.71:52025 -1711462110.185464 [0] recvMC: INFOTS(1711462110.185144558) -1711462110.185487 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #16 L(:1c1 16957.844965) => EVERYONE -1711462110.185649 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.185664 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.185714 [0] recv: INFOTS(1711462110.185500957) -1711462110.185677 [0] recvMC: HEARTBEAT(F#26:16..16 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@16(sync) 110cd0d:e3b81b8d:1ccb65b1:504@16(sync) 110cd0d:79d6eeac:ea4a8907:504@16(sync)) -1711462110.185736 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #11 L(:1c1 16957.845214)) -1711462110.185804 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:d04}} -1711462110.185826 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:d04 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:d04) => EVERYONE -1711462110.185854 [0] dq.builtin: - deleting -1711462110.185862 [0] dq.builtin: delete -1711462110.185879 [0] gc: gc 0x7408c8016150: deleting -1711462110.185895 [0] gc: gc 0x7408c8043890: deleting -1711462110.185937 [0] gc: gc 0x7408c808af10: deleting -1711462110.185946 [0] gc: gc_delete_proxy_writer (0x7408c808af10, 110443d:bb7f10a5:18901533:e03) -1711462110.185953 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=23 -1711462110.185966 [0] gc: gc 0x7408c8067c90: deleting -1711462110.185982 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8067c90, 110aba1:7b19badd:ce0adb73:e03) -1711462110.185993 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.186002 [0] gc: gc_delete_proxy_reader (0x7408c808b3e0, 110443d:bb7f10a5:18901533:d04) -1711462110.186010 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8067c90, 110aba1:7b19badd:ce0adb73:e03) -1711462110.186014 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=22 -1711462110.186047 [0] gc: gc 0x7408c8016150: deleting -1711462110.186055 [0] gc: gc 0x7408c8067c90: deleting -1711462110.186061 [0] gc: gc_delete_proxy_writer (0x7408c8067c90, 110aba1:7b19badd:ce0adb73:e03) -1711462110.186067 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=28 -1711462110.186074 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.186080 [0] gc: gc 0x7408c8016150: deleting -1711462110.186494 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.186511 [0] recv: INFOTS(1711462110.186335683) -1711462110.186549 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #14 L(:1c1 16957.846013)) -1711462110.186594 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:d04}} -1711462110.186624 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:d04 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:d04) => EVERYONE -1711462110.186652 [0] dq.builtin: - deleting -1711462110.186667 [0] dq.builtin: delete -1711462110.186675 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.186983 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.186999 [0] recv: INFOTS(1711462110.186833962) -1711462110.187024 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #14 L(:1c1 16957.846500)) -1711462110.187064 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:c03}} -1711462110.187090 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:c03 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:c03) - deleting -1711462110.187113 [0] dq.builtin: => EVERYONE -1711462110.187158 [0] dq.builtin: delete -1711462110.187196 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 280 from udp/10.101.12.71:44991 -1711462110.187222 [0] recvMC: INFOTS(1711462110.186930064) -1711462110.187240 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #15 L(:1c1 16957.846723) => EVERYONE -1711462110.187363 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,9,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.187416 [0] recvMC: HEARTBEAT(F#23:15..15 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@15(sync) 110cd0d:e3b81b8d:1ccb65b1:504@15(sync) 110cd0d:79d6eeac:ea4a8907:504@15(sync)) -1711462110.187745 [0] gc: gc 0x7408c8016150: deleting -1711462110.187780 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110aba1:7b19badd:ce0adb73:d04) -1711462110.187791 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=27 -1711462110.187804 [0] gc: gc 0x7408c8043890: deleting -1711462110.187813 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8043890, 110443d:bb7f10a5:18901533:c03) -1711462110.187824 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.187838 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.187869 [0] recv: INFOTS(1711462110.187668713) -1711462110.187839 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8043890, 110443d:bb7f10a5:18901533:c03) -1711462110.187890 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #16 L(:1c1 16957.847370)) -1711462110.187916 [0] gc: gc 0x7408c8043890: deleting -1711462110.187943 [0] gc: gc_delete_proxy_writer (0x7408c8043890, 110443d:bb7f10a5:18901533:c03) -1711462110.187957 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=21 -1711462110.187944 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:c03}} -1711462110.187974 [0] gc: gc 0x7408c808b3e0: not yet, shortsleep -1711462110.188006 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:c03 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:c03) - deleting -1711462110.188023 [0] dq.builtin: => EVERYONE -1711462110.188064 [0] dq.builtin: delete -1711462110.188074 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 408 from udp/10.101.12.71:52025 -1711462110.188106 [0] recvMC: INFOTS(1711462110.187809186) -1711462110.188128 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #17 L(:1c1 16957.847606) => EVERYONE -1711462110.188245 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.188266 [0] recvMC: HEARTBEAT(F#27:17..17 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@17(sync) 110cd0d:e3b81b8d:1ccb65b1:504@17(sync) 110cd0d:79d6eeac:ea4a8907:504@17(sync)) -1711462110.188356 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.188376 [0] recv: INFOTS(1711462110.188204641) -1711462110.188412 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #12 L(:1c1 16957.847878)) -1711462110.188588 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:b04}} -1711462110.188638 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:b04 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:b04) => EVERYONE -1711462110.188674 [0] dq.builtin: - deleting -1711462110.188682 [0] dq.builtin: delete -1711462110.189075 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.189094 [0] gc: gc 0x7408c8016150: deleting -1711462110.189103 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110aba1:7b19badd:ce0adb73:c03) -1711462110.189115 [0] gc: gc 0x7408c8013c30: deleting -1711462110.189141 [0] gc: gc_delete_proxy_reader (0x7408c8013c30, 110443d:bb7f10a5:18901533:b04) -1711462110.189157 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=20 -1711462110.189119 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.189175 [0] gc: gc 0x7408c808b3e0: not yet, shortsleep -1711462110.189202 [0] recv: INFOTS(1711462110.188954699) -1711462110.189131 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110aba1:7b19badd:ce0adb73:c03) -1711462110.189227 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #15 L(:1c1 16957.848704)) -1711462110.189321 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #15: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:b04}} -1711462110.189359 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:b04 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:b04) => EVERYONE -1711462110.189385 [0] dq.builtin: - deleting -1711462110.189395 [0] dq.builtin: delete -1711462110.189881 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.189905 [0] recv: INFOTS(1711462110.189703464) -1711462110.189926 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #15 L(:1c1 16957.849406)) -1711462110.190080 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:a03}} -1711462110.190110 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 232 from udp/10.101.12.71:44991 -1711462110.190160 [0] recvMC: INFOTS(1711462110.189872726) -1711462110.190184 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #16 L(:1c1 16957.849660) => EVERYONE -1711462110.190132 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:a03 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:a03) - deleting -1711462110.190228 [0] dq.builtin: => EVERYONE -1711462110.190258 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.190286 [0] recvMC: HEARTBEAT(F#24:16..16 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@16(sync) 110cd0d:e3b81b8d:1ccb65b1:504@16(sync) 110cd0d:79d6eeac:ea4a8907:504@16(sync)) -1711462110.190263 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.190262 [0] dq.builtin: delete -1711462110.190367 [0] gc: gc 0x7408c8016150: deleting -1711462110.190418 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110aba1:7b19badd:ce0adb73:c03) -1711462110.190433 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=26 -1711462110.190454 [0] gc: gc 0x7408c8067c90: deleting -1711462110.190474 [0] gc: gc_delete_proxy_reader (0x7408c8067c90, 110aba1:7b19badd:ce0adb73:b04) -1711462110.190510 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=25 -1711462110.190457 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.190537 [0] gc: gc 0x7408c8043890: deleting -1711462110.190552 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8043890, 110443d:bb7f10a5:18901533:a03) -1711462110.190578 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.190593 [0] gc: gc 0x7408c8067c90: deleting -1711462110.190596 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8043890, 110443d:bb7f10a5:18901533:a03) -1711462110.190624 [0] recv: INFOTS(1711462110.190259026) -1711462110.190697 [0] gc: gc 0x7408c8043890: deleting -1711462110.190731 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #17 L(:1c1 16957.850121)) -1711462110.190747 [0] gc: gc_delete_proxy_writer (0x7408c8043890, 110443d:bb7f10a5:18901533:a03) -1711462110.190780 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=19 -1711462110.190788 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:a03}} -1711462110.190806 [0] gc: gc 0x7408c808b3e0: not yet, shortsleep -1711462110.190813 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:a03 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:a03) - deleting -1711462110.190835 [0] dq.builtin: => EVERYONE -1711462110.190867 [0] dq.builtin: delete -1711462110.190908 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 360 from udp/10.101.12.71:52025 -1711462110.190928 [0] recvMC: INFOTS(1711462110.190464105) -1711462110.190954 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #18 L(:1c1 16957.850429) => EVERYONE -1711462110.191054 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.191079 [0] recvMC: HEARTBEAT(F#28:18..18 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@18(sync) 110cd0d:e3b81b8d:1ccb65b1:504@18(sync) 110cd0d:79d6eeac:ea4a8907:504@18(sync)) -1711462110.191119 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.191129 [0] recv: INFOTS(1711462110.190989734) -1711462110.191141 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #13 L(:1c1 16957.850630)) -1711462110.191162 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:904}} -1711462110.191174 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:904 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:904) => EVERYONE -1711462110.191202 [0] dq.builtin: - deleting -1711462110.191219 [0] dq.builtin: delete -1711462110.191657 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 508 from udp/10.101.12.71:58212 -1711462110.191679 [0] recvMC: INFOTS(1711462110.191353654) -1711462110.191701 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.191722 [0] recv: INFOTS(1711462110.191549194) -1711462110.191769 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #16 L(:1c1 16957.851225)) -1711462110.191705 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #12 L(:1c1 16957.851180) => EVERYONE -1711462110.191797 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #16: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:904}} -1711462110.191826 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:904 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:904) => EVERYONE -1711462110.191857 [0] dq.builtin: - deleting -1711462110.191921 [0] dq.builtin: delete -1711462110.191901 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.191971 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #12: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,16,3,0,0,) -1711462110.191978 [0] gc: gc 0x7408c8016150: deleting -1711462110.192056 [0] recvMC: HEARTBEAT(#19:12..12 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@12(sync) 110cd0d:e3b81b8d:1ccb65b1:504@12(sync) 110cd0d:79d6eeac:ea4a8907:504@12(sync)) -1711462110.192078 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110aba1:7b19badd:ce0adb73:a03) -1711462110.192085 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110e21c:d0762c48:a9f0fb28:403: F#5:13/0: -1711462110.192134 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110aba1:7b19badd:ce0adb73:a03) -1711462110.192141 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462110.192122 [0] gc: gc 0x7408c8040ad0: deleting -1711462110.192194 [0] gc: gc_delete_proxy_reader (0x7408c8040ad0, 110443d:bb7f10a5:18901533:904) -1711462110.192181 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.192209 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=18 -1711462110.192231 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110e21c:d0762c48:a9f0fb28:403: F#19:13/0: -1711462110.192241 [0] gc: gc 0x7408c8013c30: deleting -1711462110.192257 [0] gc: gc_delete_proxy_reader (0x7408c8013c30, 110aba1:7b19badd:ce0adb73:904) -1711462110.192270 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=24 -1711462110.192242 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462110.192302 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.192289 [0] gc: gc 0x7408c8016150: deleting -1711462110.192338 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110aba1:7b19badd:ce0adb73:a03) -1711462110.192351 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=23 -1711462110.192324 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110e21c:d0762c48:a9f0fb28:403: F#10:13/0: -1711462110.192379 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462110.192369 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.192412 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.192432 [0] gc: gc 0x7408c8013c30: deleting -1711462110.192446 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.192465 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.192482 [0] recv: INFOTS(1711462110.192364136) -1711462110.192515 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #16 L(:1c1 16957.851983)) -1711462110.192526 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7413@2 ] -1711462110.192543 [0] tev: traffic-xmit (1) 168 -1711462110.192557 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:803}} -1711462110.192578 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:803 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:803) - deleting -1711462110.192590 [0] dq.builtin: => EVERYONE -1711462110.192623 [0] dq.builtin: delete -1711462110.192733 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 208 from udp/10.101.12.71:44991 -1711462110.192755 [0] recvMC: INFOTS(1711462110.192525975) -1711462110.192773 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #17 L(:1c1 16957.852255) => EVERYONE -1711462110.192841 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.192862 [0] recvMC: HEARTBEAT(F#25:17..17 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@17(sync) 110cd0d:e3b81b8d:1ccb65b1:504@17(sync) 110cd0d:79d6eeac:ea4a8907:504@17(sync)) -1711462110.193048 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.193080 [0] recv: INFOTS(1711462110.192899351) -1711462110.193097 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #18 L(:1c1 16957.852566)) -1711462110.193152 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:803}} -1711462110.193165 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:803 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:803) - deleting -1711462110.193178 [0] dq.builtin: => EVERYONE -1711462110.193206 [0] dq.builtin: delete -1711462110.193304 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 336 from udp/10.101.12.71:52025 -1711462110.193329 [0] recvMC: INFOTS(1711462110.193029153) -1711462110.193347 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #19 L(:1c1 16957.852830) => EVERYONE -1711462110.193440 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,20,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.193481 [0] recvMC: HEARTBEAT(F#29:19..19 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@19(sync) 110cd0d:e3b81b8d:1ccb65b1:504@19(sync) 110cd0d:79d6eeac:ea4a8907:504@19(sync)) -1711462110.193527 [0] gc: gc 0x7408c8016150: deleting -1711462110.193547 [0] gc: gc 0x7408c8067c90: deleting -1711462110.193557 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8067c90, 110443d:bb7f10a5:18901533:803) -1711462110.193570 [0] gc: gc 0x7408c8043890: deleting -1711462110.193580 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8043890, 110aba1:7b19badd:ce0adb73:803) -1711462110.193585 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8067c90, 110443d:bb7f10a5:18901533:803) -1711462110.193617 [0] gc: gc 0x7408c8067c90: deleting -1711462110.193630 [0] gc: gc_delete_proxy_writer (0x7408c8067c90, 110443d:bb7f10a5:18901533:803) -1711462110.193644 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=17 -1711462110.193631 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8043890, 110aba1:7b19badd:ce0adb73:803) -1711462110.193668 [0] gc: gc 0x7408c808b3e0: not yet, shortsleep -1711462110.193815 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.193831 [0] recv: INFOTS(1711462110.193730576) -1711462110.193850 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #17 L(:1c1 16957.853332)) -1711462110.193903 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:703}} -1711462110.193945 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:703 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:703) - deleting -1711462110.193952 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.193964 [0] dq.builtin: => EVERYONE -1711462110.193986 [0] recv: INFOTS(1711462110.193391175) -1711462110.194006 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #9 L(:1c1 16957.853487)) -1711462110.194174 [0] dq.builtin: delete -1711462110.194212 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 184 from udp/10.101.12.71:44991 -1711462110.194556 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #9: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1304}} -1711462110.194578 [0] recvMC: INFOTS(1711462110.193879178) -1711462110.194608 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1304 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:1304) => EVERYONE -1711462110.194657 [0] dq.builtin: - deleting -1711462110.194614 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.194674 [0] dq.builtin: delete -1711462110.194616 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #18 L(:1c1 16957.854077) => EVERYONE -1711462110.194720 [0] recv: INFOTS(1711462110.194357465) -1711462110.194781 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.194782 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.194826 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #19 L(:1c1 16957.854219)) -1711462110.194841 [0] gc: gc 0x7408c8043890: deleting -1711462110.194896 [0] gc: gc_delete_proxy_writer (0x7408c8043890, 110aba1:7b19badd:ce0adb73:803) -1711462110.194912 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=22 -1711462110.194930 [0] gc: gc 0x7408c8016150: deleting -1711462110.194941 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110443d:bb7f10a5:18901533:703) -1711462110.194955 [0] gc: gc 0x7408c8040ad0: deleting -1711462110.194988 [0] gc: gc_delete_proxy_reader (0x7408c8040ad0, 110e21c:d0762c48:a9f0fb28:1304) -1711462110.195005 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=29 -1711462110.194879 [0] recvMC: HEARTBEAT(F#26:18..18 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@18(sync) 110cd0d:e3b81b8d:1ccb65b1:504@18(sync) 110cd0d:79d6eeac:ea4a8907:504@18(sync)) -1711462110.195005 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110443d:bb7f10a5:18901533:703) -1711462110.195062 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 288 from udp/10.101.12.71:52025 -1711462110.194899 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #19: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:703}} -1711462110.195080 [0] recvMC: INFOTS(1711462110.194629401) -1711462110.195108 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:703 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:703) - deleting -1711462110.195032 [0] gc: gc 0x7408c808b3e0: not yet, shortsleep -1711462110.195150 [0] dq.builtin: => EVERYONE -1711462110.195135 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #20 L(:1c1 16957.854581) => EVERYONE -1711462110.195189 [0] dq.builtin: delete -1711462110.195257 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,22,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.195280 [0] recvMC: HEARTBEAT(F#30:20..20 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@20(sync) 110cd0d:e3b81b8d:1ccb65b1:504@20(sync) 110cd0d:79d6eeac:ea4a8907:504@20(sync)) -1711462110.195406 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.195428 [0] recv: INFOTS(1711462110.195259141) -1711462110.195452 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #11 L(:1c1 16957.854929)) -1711462110.195482 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #11: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1203}} -1711462110.195499 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1203 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:1203) - deleting -1711462110.195525 [0] dq.builtin: => EVERYONE -1711462110.195537 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.195554 [0] recv: INFOTS(1711462110.195373351) -1711462110.195568 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #18 L(:1c1 16957.855056)) -1711462110.195568 [0] dq.builtin: delete -1711462110.195616 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:1403}} -1711462110.195630 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:1403 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:1403) - deleting -1711462110.195646 [0] dq.builtin: => EVERYONE -1711462110.195680 [0] dq.builtin: delete -1711462110.195821 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 460 from udp/10.101.12.71:58212 -1711462110.195841 [0] recvMC: INFOTS(1711462110.195421660) -1711462110.195865 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #13 L(:1c1 16957.855342) => EVERYONE -1711462110.196004 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #13: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.196040 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.196060 [0] recv: INFOTS(1711462110.195845630) -1711462110.196047 [0] recvMC: HEARTBEAT(F#20:13..13 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@13(sync) 110cd0d:e3b81b8d:1ccb65b1:504@13(sync) 110cd0d:79d6eeac:ea4a8907:504@13(sync)) -1711462110.196085 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #17 L(:1c1 16957.855561)) -1711462110.196116 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 160 from udp/10.101.12.71:44991 -1711462110.196145 [0] recvMC: INFOTS(1711462110.195554831) -1711462110.196160 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #19 L(:1c1 16957.855646) => EVERYONE -1711462110.196179 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #17: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1504}} -1711462110.196210 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","static_transformer",{},{}}}}) -1711462110.196212 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1504 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:1504) => EVERYONE -1711462110.196237 [0] recvMC: HEARTBEAT(F#27:19..19 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@19(sync) 110cd0d:e3b81b8d:1ccb65b1:504@19(sync) 110cd0d:79d6eeac:ea4a8907:504@19(sync)) -1711462110.196271 [0] dq.builtin: - deleting -1711462110.196287 [0] dq.builtin: delete -1711462110.196258 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.196390 [0] gc: gc 0x7408c8040ad0: deleting -1711462110.196409 [0] gc: gc 0x7408c8016150: deleting -1711462110.196427 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110443d:bb7f10a5:18901533:703) -1711462110.196440 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=16 -1711462110.196459 [0] gc: gc 0x7408c8013c30: deleting -1711462110.196472 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8013c30, 110aba1:7b19badd:ce0adb73:703) -1711462110.196484 [0] gc: gc 0x7408c803bcd0: deleting -1711462110.196492 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c803bcd0, 110e21c:d0762c48:a9f0fb28:1203) -1711462110.196511 [0] gc: gc 0x7408c8067c90: deleting -1711462110.196517 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8013c30, 110aba1:7b19badd:ce0adb73:703) -1711462110.196521 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8067c90, 110443d:bb7f10a5:18901533:1403) -1711462110.196543 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c803bcd0, 110e21c:d0762c48:a9f0fb28:1203) -1711462110.196563 [0] gc: gc 0x7408c8066850: deleting -1711462110.196573 [0] gc: gc_delete_proxy_reader (0x7408c8066850, 110aba1:7b19badd:ce0adb73:1504) -1711462110.196585 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=21 -1711462110.196614 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.196628 [0] gc: gc 0x7408c8013c30: deleting -1711462110.196639 [0] gc: gc_delete_proxy_writer (0x7408c8013c30, 110aba1:7b19badd:ce0adb73:703) -1711462110.196650 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=20 -1711462110.196617 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8067c90, 110443d:bb7f10a5:18901533:1403) -1711462110.196669 [0] gc: gc 0x7408c8066850: deleting -1711462110.196699 [0] gc: gc 0x7408c803bcd0: deleting -1711462110.196707 [0] gc: gc_delete_proxy_writer (0x7408c803bcd0, 110e21c:d0762c48:a9f0fb28:1203) -1711462110.196716 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=28 -1711462110.196731 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.196741 [0] gc: gc 0x7408c8067c90: deleting -1711462110.196750 [0] gc: gc_delete_proxy_writer (0x7408c8067c90, 110443d:bb7f10a5:18901533:1403) -1711462110.196758 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=15 -1711462110.196771 [0] gc: gc 0x7408c8066850: deleting -1711462110.196780 [0] gc: gc 0x7408c808b3e0: deleting -1711462110.197066 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.197078 [0] recv: INFOTS(1711462110.196935375) -1711462110.197090 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #10 L(:1c1 16957.856581)) -1711462110.197124 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #10: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1104}} -1711462110.197149 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1104 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:1104) => EVERYONE -1711462110.197178 [0] dq.builtin: - deleting -1711462110.197196 [0] dq.builtin: delete -1711462110.197202 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.197213 [0] gc: gc 0x7408c8016150: deleting -1711462110.197243 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:1104) -1711462110.197262 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=27 -1711462110.197216 [0] recv: INFOTS(1711462110.197064444) -1711462110.197289 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.197304 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #19 L(:1c1 16957.856718)) -1711462110.197349 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #19: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:603}} -1711462110.197363 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:603 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:603) - deleting -1711462110.197374 [0] dq.builtin: => EVERYONE -1711462110.197408 [0] dq.builtin: delete -1711462110.197511 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 120 from udp/10.101.12.71:44991 -1711462110.197526 [0] recvMC: INFOTS(1711462110.197216804) -1711462110.197542 [0] recvMC: DATA(110443d:bb7f10a5:18901533:403 -> 0:0:0:0 #20 L(:1c1 16957.857028) => EVERYONE -1711462110.197596 [0] recvMC: data(application, vendor 1.16): 110443d:bb7f10a5:18901533:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,68,61,187,127,16,165,24,144,21,51,0,0,1,193,0,0,0,0,0,0,0,0}}},{}}) -1711462110.197622 [0] recvMC: HEARTBEAT(F#28:20..20 110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@20(sync) 110cd0d:e3b81b8d:1ccb65b1:504@20(sync) 110cd0d:79d6eeac:ea4a8907:504@20(sync)) -1711462110.197673 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.197713 [0] recv: INFOTS(1711462110.197489510) -1711462110.197724 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #20 L(:1c1 16957.857216)) -1711462110.197749 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #20: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1403}} -1711462110.197787 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1403 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:1403) - deleting -1711462110.197798 [0] dq.builtin: => EVERYONE -1711462110.197834 [0] dq.builtin: delete -1711462110.198021 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 240 from udp/10.101.12.71:52025 -1711462110.198040 [0] recvMC: INFOTS(1711462110.197789884) -1711462110.198059 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #21 L(:1c1 16957.857541) => EVERYONE -1711462110.198116 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #21: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,25,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,24,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.198131 [0] recvMC: HEARTBEAT(F#31:21..21 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@21(sync) 110cd0d:e3b81b8d:1ccb65b1:504@21(sync) 110cd0d:79d6eeac:ea4a8907:504@21(sync)) -1711462110.198400 [0] gc: gc 0x7408c8066850: deleting -1711462110.198431 [0] gc: gc 0x7408c8016150: deleting -1711462110.198442 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110443d:bb7f10a5:18901533:603) -1711462110.198460 [0] gc: gc 0x7408c805eed0: deleting -1711462110.198470 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c805eed0, 110aba1:7b19badd:ce0adb73:1403) -1711462110.198474 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110443d:bb7f10a5:18901533:603) -1711462110.198508 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c805eed0, 110aba1:7b19badd:ce0adb73:1403) -1711462110.198514 [0] gc: gc 0x7408c8016150: deleting -1711462110.198549 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110443d:bb7f10a5:18901533:603) -1711462110.198550 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.198588 [0] recv: INFOTS(1711462110.198317479) -1711462110.198570 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=14 -1711462110.198604 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #12 L(:1c1 16957.858089)) -1711462110.198631 [0] gc: gc 0x7408c805eed0: deleting -1711462110.198658 [0] gc: gc_delete_proxy_writer (0x7408c805eed0, 110aba1:7b19badd:ce0adb73:1403) -1711462110.198647 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #12: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1003}} -1711462110.198674 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=19 -1711462110.198691 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1003 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:1003) - deleting -1711462110.198707 [0] dq.builtin: => EVERYONE -1711462110.198732 [0] dq.builtin: delete -1711462110.198731 [0] gc: gc 0x7408c8066850: deleting -1711462110.198764 [0] gc: gc 0x7408c805eed0: deleting -1711462110.198779 [0] gc: gc 0x7408c8016150: deleting -1711462110.198787 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 412 from udp/10.101.12.71:58212 -1711462110.198804 [0] recvMC: INFOTS(1711462110.198438587) -1711462110.198819 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #14 L(:1c1 16957.858306) => EVERYONE -1711462110.198790 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:1003) -1711462110.198871 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:1003) -1711462110.198901 [0] gc: gc 0x7408c8016150: deleting -1711462110.198944 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:1003) -1711462110.198902 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.198976 [0] recvMC: HEARTBEAT(F#21:14..14 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@14(sync) 110cd0d:e3b81b8d:1ccb65b1:504@14(sync) 110cd0d:79d6eeac:ea4a8907:504@14(sync)) -1711462110.198961 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=26 -1711462110.199014 [0] gc: gc 0x7408c8066850: deleting -1711462110.199249 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.199263 [0] recv: INFOTS(1711462110.199105790) -1711462110.199274 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #18 L(:1c1 16957.858766)) -1711462110.199325 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #18: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1704}} -1711462110.199346 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1704 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:1704) => EVERYONE -1711462110.199372 [0] dq.builtin: - deleting -1711462110.199389 [0] dq.builtin: delete -1711462110.199399 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.199960 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.199969 [0] recv: INFOTS(1711462110.199842623) -1711462110.199981 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #11 L(:1c1 16957.859472)) -1711462110.200018 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:f04}} -1711462110.200042 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:f04 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:f04) => EVERYONE -1711462110.200064 [0] dq.builtin: - deleting -1711462110.200074 [0] dq.builtin: delete -1711462110.200315 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.200323 [0] recv: INFOTS(1711462110.200208970) -1711462110.200337 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #13 L(:1c1 16957.859826)) -1711462110.200359 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:e03}} -1711462110.200379 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:e03 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:e03) - deleting -1711462110.200399 [0] dq.builtin: => EVERYONE -1711462110.200392 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.200429 [0] recv: INFOTS(1711462110.200281996) -1711462110.200433 [0] dq.builtin: delete -1711462110.200438 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #21 L(:1c1 16957.859932)) -1711462110.200456 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #21: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1603}} -1711462110.200934 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1603 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:1603) - deleting -1711462110.200954 [0] dq.builtin: => EVERYONE -1711462110.200479 [0] gc: gc 0x7408c8016150: deleting -1711462110.200991 [0] dq.builtin: delete -1711462110.201209 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110aba1:7b19badd:ce0adb73:1704) -1711462110.201159 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 364 from udp/10.101.12.71:58212 -1711462110.201281 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=18 -1711462110.201336 [0] gc: gc 0x7408c8050f20: deleting -1711462110.201354 [0] gc: gc_delete_proxy_reader (0x7408c8050f20, 110e21c:d0762c48:a9f0fb28:f04) -1711462110.201367 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=25 -1711462110.201380 [0] gc: gc 0x7408c805eed0: deleting -1711462110.201390 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c805eed0, 110e21c:d0762c48:a9f0fb28:e03) -1711462110.201317 [0] recvMC: INFOTS(1711462110.200343585) -1711462110.201493 [0] gc: gc 0x7408c8043890: deleting -1711462110.201547 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8043890, 110aba1:7b19badd:ce0adb73:1603) -1711462110.201562 [0] gc: gc 0x7408c8066850: deleting -1711462110.201575 [0] gc: gc 0x7408c8050f20: deleting -1711462110.201506 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c805eed0, 110e21c:d0762c48:a9f0fb28:e03) -1711462110.201531 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #15 L(:1c1 16957.860816) => EVERYONE -1711462110.201621 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8043890, 110aba1:7b19badd:ce0adb73:1603) -1711462110.201624 [0] gc: gc 0x7408c805eed0: deleting -1711462110.201668 [0] gc: gc_delete_proxy_writer (0x7408c805eed0, 110e21c:d0762c48:a9f0fb28:e03) -1711462110.201681 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=24 -1711462110.201696 [0] gc: gc 0x7408c8043890: deleting -1711462110.201702 [0] gc: gc_delete_proxy_writer (0x7408c8043890, 110aba1:7b19badd:ce0adb73:1603) -1711462110.201710 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=17 -1711462110.201721 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.201729 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.201754 [0] recvMC: HEARTBEAT(F#22:15..15 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@15(sync) 110cd0d:e3b81b8d:1ccb65b1:504@15(sync) 110cd0d:79d6eeac:ea4a8907:504@15(sync)) -1711462110.201779 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 192 from udp/10.101.12.71:52025 -1711462110.201791 [0] recvMC: INFOTS(1711462110.200384071) -1711462110.201808 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #22 L(:1c1 16957.861293) => EVERYONE -1711462110.201838 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.201850 [0] recv: INFOTS(1711462110.201520038) -1711462110.201871 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #12 L(:1c1 16957.861352)) -1711462110.201915 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:d04}} -1711462110.201940 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:d04 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:d04) => EVERYONE -1711462110.201856 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #22: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{},{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.201970 [0] dq.builtin: - deleting -1711462110.202031 [0] recvMC: HEARTBEAT(F#32:22..22 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@22(sync) 110cd0d:e3b81b8d:1ccb65b1:504@22(sync) 110cd0d:79d6eeac:ea4a8907:504@22(sync)) -1711462110.202033 [0] dq.builtin: delete -1711462110.202730 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.202739 [0] recv: INFOTS(1711462110.202594273) -1711462110.202749 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #19 L(:1c1 16957.862242)) -1711462110.202791 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #19: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1904}} -1711462110.202812 [0] gc: gc 0x7408c8066850: deleting -1711462110.202825 [0] gc: gc 0x7408c8043890: deleting -1711462110.202846 [0] gc: gc 0x7408c8016150: deleting -1711462110.202862 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:d04) -1711462110.202881 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=23 -1711462110.202817 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1904 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:1904) => EVERYONE -1711462110.202897 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.202921 [0] dq.builtin: - deleting -1711462110.202935 [0] dq.builtin: delete -1711462110.203026 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.203035 [0] recv: INFOTS(1711462110.202893761) -1711462110.203048 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #14 L(:1c1 16957.862538)) -1711462110.203076 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:c03}} -1711462110.203087 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:c03 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:c03) - deleting -1711462110.203094 [0] dq.builtin: => EVERYONE -1711462110.203118 [0] dq.builtin: delete -1711462110.203229 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 316 from udp/10.101.12.71:58212 -1711462110.203253 [0] recvMC: INFOTS(1711462110.202997343) -1711462110.203274 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #16 L(:1c1 16957.862753) => EVERYONE -1711462110.203366 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.203387 [0] recvMC: HEARTBEAT(F#23:16..16 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@16(sync) 110cd0d:e3b81b8d:1ccb65b1:504@16(sync) 110cd0d:79d6eeac:ea4a8907:504@16(sync)) -1711462110.203999 [0] gc: gc 0x7408c8066850: deleting -1711462110.204015 [0] gc: gc 0x7408c8016150: deleting -1711462110.204021 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110aba1:7b19badd:ce0adb73:1904) -1711462110.204033 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=16 -1711462110.204042 [0] gc: gc 0x7408c8050f20: deleting -1711462110.204049 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8050f20, 110e21c:d0762c48:a9f0fb28:c03) -1711462110.204057 [0] gc: gc 0x7408c8066850: deleting -1711462110.204074 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8050f20, 110e21c:d0762c48:a9f0fb28:c03) -1711462110.204106 [0] gc: gc 0x7408c8050f20: deleting -1711462110.204117 [0] gc: gc_delete_proxy_writer (0x7408c8050f20, 110e21c:d0762c48:a9f0fb28:c03) -1711462110.204128 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=22 -1711462110.204138 [0] gc: gc 0x7408c8066850: deleting -1711462110.204139 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.204175 [0] recv: INFOTS(1711462110.204013066) -1711462110.204191 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #22 L(:1c1 16957.863677)) -1711462110.204221 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #22: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:1803}} -1711462110.204236 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:1803 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:1803) - deleting -1711462110.204247 [0] dq.builtin: => EVERYONE -1711462110.204282 [0] dq.builtin: delete -1711462110.204303 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.204372 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 168 from udp/10.101.12.71:52025 -1711462110.204394 [0] recvMC: INFOTS(1711462110.204092708) -1711462110.204413 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #23 L(:1c1 16957.863895) => EVERYONE -1711462110.204469 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #23: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_navigation_core",{},{}}}}) -1711462110.204483 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.204525 [0] recv: INFOTS(1711462110.204117111) -1711462110.204546 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #13 L(:1c1 16957.864025)) -1711462110.204488 [0] recvMC: HEARTBEAT(F#33:23..23 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@23(sync) 110cd0d:e3b81b8d:1ccb65b1:504@23(sync) 110cd0d:79d6eeac:ea4a8907:504@23(sync)) -1711462110.204605 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:b04}} -1711462110.204628 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:b04 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:b04) => EVERYONE -1711462110.204659 [0] dq.builtin: - deleting -1711462110.204670 [0] dq.builtin: delete -1711462110.205394 [0] gc: gc 0x7408c8016150: deleting -1711462110.205413 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110aba1:7b19badd:ce0adb73:1803) -1711462110.205432 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.205460 [0] recv: INFOTS(1711462110.205296205) -1711462110.205438 [0] gc: gc 0x7408c805eed0: deleting -1711462110.205486 [0] gc: gc_delete_proxy_reader (0x7408c805eed0, 110e21c:d0762c48:a9f0fb28:b04) -1711462110.205503 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=21 -1711462110.205462 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110aba1:7b19badd:ce0adb73:1803) -1711462110.205531 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.205486 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #23 L(:1c1 16957.864960)) -1711462110.205672 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #23: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:603}} -1711462110.205694 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:603 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:603) - deleting -1711462110.205724 [0] dq.builtin: => EVERYONE -1711462110.205674 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.205738 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 120 from udp/10.101.12.71:52025 -1711462110.205776 [0] recv: INFOTS(1711462110.205333502) -1711462110.205827 [0] recvMC: INFOTS(1711462110.205378348) -1711462110.205758 [0] dq.builtin: delete -1711462110.205854 [0] recvMC: DATA(110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0 #24 L(:1c1 16957.865322) => EVERYONE -1711462110.205893 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #15 L(:1c1 16957.865272)) -1711462110.205929 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:a03}} -1711462110.205947 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:a03 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:a03) - deleting -1711462110.205961 [0] dq.builtin: => EVERYONE -1711462110.205933 [0] recvMC: data(application, vendor 1.16): 110aba1:7b19badd:ce0adb73:403 #24: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,171,161,123,25,186,221,206,10,219,115,0,0,1,193,0,0,0,0,0,0,0,0}}},{}}) -1711462110.205994 [0] dq.builtin: delete -1711462110.205998 [0] recvMC: HEARTBEAT(F#34:24..24 110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@24(sync) 110cd0d:e3b81b8d:1ccb65b1:504@24(sync) 110cd0d:79d6eeac:ea4a8907:504@24(sync)) -1711462110.206038 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 268 from udp/10.101.12.71:58212 -1711462110.206052 [0] recvMC: INFOTS(1711462110.205461976) -1711462110.206073 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #17 L(:1c1 16957.865554) => EVERYONE -1711462110.206150 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,21,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.206168 [0] recvMC: HEARTBEAT(F#24:17..17 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@17(sync) 110cd0d:e3b81b8d:1ccb65b1:504@17(sync) 110cd0d:79d6eeac:ea4a8907:504@17(sync)) -1711462110.206703 [0] gc: gc 0x7408c8066850: deleting -1711462110.206736 [0] gc: gc 0x7408c8016150: deleting -1711462110.206747 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110aba1:7b19badd:ce0adb73:1803) -1711462110.206759 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=15 -1711462110.206778 [0] gc: gc 0x7408c8050f20: deleting -1711462110.206792 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8050f20, 110aba1:7b19badd:ce0adb73:603) -1711462110.206814 [0] gc: gc 0x7408c805eed0: deleting -1711462110.206824 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c805eed0, 110e21c:d0762c48:a9f0fb28:a03) -1711462110.206845 [0] gc: gc 0x7408c8066850: deleting -1711462110.206823 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.206864 [0] recv: INFOTS(1711462110.206657137) -1711462110.206832 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8050f20, 110aba1:7b19badd:ce0adb73:603) -1711462110.206885 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #14 L(:1c1 16957.866364)) -1711462110.206911 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c805eed0, 110e21c:d0762c48:a9f0fb28:a03) -1711462110.206952 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:904}} -1711462110.206989 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:904 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:904) => EVERYONE -1711462110.206972 [0] gc: gc 0x7408c8050f20: deleting -1711462110.207019 [0] gc: gc_delete_proxy_writer (0x7408c8050f20, 110aba1:7b19badd:ce0adb73:603) -1711462110.207010 [0] dq.builtin: - deleting -1711462110.207036 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=14 -1711462110.207042 [0] dq.builtin: delete -1711462110.207052 [0] gc: gc 0x7408c805eed0: deleting -1711462110.207063 [0] gc: gc_delete_proxy_writer (0x7408c805eed0, 110e21c:d0762c48:a9f0fb28:a03) -1711462110.207072 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=20 -1711462110.207087 [0] gc: gc 0x7408c8016150: deleting -1711462110.207095 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:904) -1711462110.207103 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=19 -1711462110.207116 [0] gc: gc 0x7408c8066850: deleting -1711462110.207126 [0] gc: gc 0x7408c805eed0: deleting -1711462110.207135 [0] gc: gc 0x7408c8016150: deleting -1711462110.208199 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.208213 [0] recv: INFOTS(1711462110.208064226) -1711462110.208227 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #16 L(:1c1 16957.867715)) -1711462110.208245 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:803}} -1711462110.208255 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:803 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:803) - deleting -1711462110.208265 [0] dq.builtin: => EVERYONE -1711462110.208289 [0] dq.builtin: delete -1711462110.208308 [0] gc: gc 0x7408c8016150: deleting -1711462110.208333 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:803) -1711462110.208359 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:803) -1711462110.208380 [0] gc: gc 0x7408c8016150: deleting -1711462110.208393 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:803) -1711462110.208406 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=18 -1711462110.208423 [0] gc: gc 0x7408c8066850: deleting -1711462110.208439 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 244 from udp/10.101.12.71:58212 -1711462110.208453 [0] recvMC: INFOTS(1711462110.208197991) -1711462110.208476 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #18 L(:1c1 16957.867955) => EVERYONE -1711462110.208595 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.208613 [0] recvMC: HEARTBEAT(F#25:18..18 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@18(sync) 110cd0d:e3b81b8d:1ccb65b1:504@18(sync) 110cd0d:79d6eeac:ea4a8907:504@18(sync)) -1711462110.209394 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.209413 [0] recv: INFOTS(1711462110.209265145) -1711462110.209423 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #15 L(:1c1 16957.868914)) -1711462110.209449 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #15: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1504}} -1711462110.209465 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1504 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:1504) => EVERYONE -1711462110.209492 [0] dq.builtin: - deleting -1711462110.209507 [0] dq.builtin: delete -1711462110.209518 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.209652 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 220 from udp/10.101.12.71:58212 -1711462110.209675 [0] recvMC: INFOTS(1711462110.209425688) -1711462110.209702 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #19 L(:1c1 16957.869175) => EVERYONE -1711462110.209768 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,20,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.209790 [0] recvMC: HEARTBEAT(F#26:19..19 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@19(sync) 110cd0d:e3b81b8d:1ccb65b1:504@19(sync) 110cd0d:79d6eeac:ea4a8907:504@19(sync)) -1711462110.210608 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.210631 [0] recv: INFOTS(1711462110.210493781) -1711462110.210618 [0] gc: gc 0x7408c8016150: deleting -1711462110.210650 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #17 L(:1c1 16957.870133)) -1711462110.210673 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:1504) -1711462110.210689 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=17 -1711462110.210708 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:703}} -1711462110.210728 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:703 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:703) - deleting -1711462110.210745 [0] dq.builtin: => EVERYONE -1711462110.210709 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.210778 [0] dq.builtin: delete -1711462110.210865 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 196 from udp/10.101.12.71:58212 -1711462110.210886 [0] recvMC: INFOTS(1711462110.210679198) -1711462110.210943 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #20 L(:1c1 16957.870387) => EVERYONE -1711462110.210996 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{},{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.211016 [0] recvMC: HEARTBEAT(F#27:20..20 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@20(sync) 110cd0d:e3b81b8d:1ccb65b1:504@20(sync) 110cd0d:79d6eeac:ea4a8907:504@20(sync)) -1711462110.211784 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.211804 [0] recv: INFOTS(1711462110.211688403) -1711462110.211819 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #18 L(:1c1 16957.871304)) -1711462110.211845 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:1403}} -1711462110.211871 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:1403 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:1403) - deleting -1711462110.211881 [0] dq.builtin: => EVERYONE -1711462110.211850 [0] gc: gc 0x7408c8066850: deleting -1711462110.211911 [0] dq.builtin: delete -1711462110.211912 [0] gc: gc 0x7408c8016150: deleting -1711462110.211945 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:703) -1711462110.211959 [0] gc: gc 0x7408c8006f50: deleting -1711462110.211966 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:703) -1711462110.211969 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8006f50, 110e21c:d0762c48:a9f0fb28:1403) -1711462110.212008 [0] gc: gc 0x7408c8016150: deleting -1711462110.212020 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:703) -1711462110.212039 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=16 -1711462110.212012 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8006f50, 110e21c:d0762c48:a9f0fb28:1403) -1711462110.211989 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 172 from udp/10.101.12.71:58212 -1711462110.212059 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.212095 [0] recvMC: INFOTS(1711462110.211821961) -1711462110.212148 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #21 L(:1c1 16957.871594) => EVERYONE -1711462110.212194 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #21: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","topological_transform_publisher",{},{}}}}) -1711462110.212231 [0] recvMC: HEARTBEAT(F#28:21..21 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@21(sync) 110cd0d:e3b81b8d:1ccb65b1:504@21(sync) 110cd0d:79d6eeac:ea4a8907:504@21(sync)) -1711462110.213023 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.213043 [0] recv: INFOTS(1711462110.212917857) -1711462110.213052 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #19 L(:1c1 16957.872546)) -1711462110.213076 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #19: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:603}} -1711462110.213090 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:603 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:603) - deleting -1711462110.213098 [0] dq.builtin: => EVERYONE -1711462110.213121 [0] dq.builtin: delete -1711462110.213171 [0] gc: gc 0x7408c8066850: deleting -1711462110.213191 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 120 from udp/10.101.12.71:58212 -1711462110.213215 [0] recvMC: INFOTS(1711462110.213044269) -1711462110.213195 [0] gc: gc 0x7408c8006f50: deleting -1711462110.213247 [0] gc: gc_delete_proxy_writer (0x7408c8006f50, 110e21c:d0762c48:a9f0fb28:1403) -1711462110.213258 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=15 -1711462110.213239 [0] recvMC: DATA(110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0 #22 L(:1c1 16957.872717) => EVERYONE -1711462110.213275 [0] gc: gc 0x7408c8016150: deleting -1711462110.213302 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:603) -1711462110.213314 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.213315 [0] recvMC: data(application, vendor 1.16): 110e21c:d0762c48:a9f0fb28:403 #22: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,226,28,208,118,44,72,169,240,251,40,0,0,1,193,0,0,0,0,0,0,0,0}}},{}}) -1711462110.213339 [0] recvMC: HEARTBEAT(F#29:22..22 110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@22(sync) 110cd0d:e3b81b8d:1ccb65b1:504@22(sync) 110cd0d:79d6eeac:ea4a8907:504@22(sync)) -1711462110.213351 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:603) -1711462110.213444 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2096 from udp/10.101.12.71:40473 -1711462110.213457 [0] recvMC: INFOTS(1711462110.213069639) -1711462110.213476 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #46 L(:1c1 16957.872959) => EVERYONE -1711462110.213590 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #46: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,19,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462110.213605 [0] recvMC: HEARTBEAT(#55:46..46 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@46(sync) 110cd0d:e3b81b8d:1ccb65b1:504@46(sync) 110cd0d:79d6eeac:ea4a8907:504@46(sync)) -1711462110.213623 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#6:47/0: -1711462110.213634 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.213642 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.213651 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#22:47/0: -1711462110.213656 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.213663 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.213670 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#13:47/0: -1711462110.213676 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.213683 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.213718 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7415@2 ] -1711462110.213724 [0] tev: traffic-xmit (1) 168 -1711462110.214399 [0] gc: gc 0x7408c8066850: deleting -1711462110.214417 [0] gc: gc 0x7408c8016150: deleting -1711462110.214422 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:603) -1711462110.214428 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=14 -1711462110.214436 [0] gc: gc 0x7408c8066850: deleting -1711462110.214546 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.214565 [0] recv: INFOTS(1711462110.214373670) -1711462110.214583 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #41 L(:1c1 16957.874066)) -1711462110.214627 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #41: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2704}} -1711462110.214644 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2704 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2704) => EVERYONE -1711462110.214678 [0] dq.builtin: - deleting -1711462110.214698 [0] dq.builtin: delete -1711462110.214705 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.215786 [0] gc: gc 0x7408c8016150: deleting -1711462110.215807 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2704) -1711462110.215825 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=96 -1711462110.215842 [0] gc: gc 0x7408c8066850: deleting -1711462110.215895 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.215909 [0] recv: INFOTS(1711462110.215754782) -1711462110.215935 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #46 L(:1c1 16957.875412)) -1711462110.215962 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #46: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2603}} -1711462110.215975 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2603) - deleting -1711462110.215985 [0] dq.builtin: => EVERYONE -1711462110.216008 [0] dq.builtin: delete -1711462110.216015 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.216118 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2048 from udp/10.101.12.71:40473 -1711462110.216133 [0] recvMC: INFOTS(1711462110.215865725) -1711462110.216150 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #47 L(:1c1 16957.875636) => EVERYONE -1711462110.216273 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #47: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462110.216285 [0] recvMC: HEARTBEAT(F#56:47..47 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@47(sync) 110cd0d:e3b81b8d:1ccb65b1:504@47(sync) 110cd0d:79d6eeac:ea4a8907:504@47(sync)) -1711462110.217088 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.217106 [0] recv: INFOTS(1711462110.216976537) -1711462110.217128 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #42 L(:1c1 16957.876608)) -1711462110.217105 [0] gc: gc 0x7408c8016150: deleting -1711462110.217166 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2603) -1711462110.217187 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #42: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1304}} -1711462110.217191 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2603) -1711462110.217222 [0] gc: gc 0x7408c8016150: deleting -1711462110.217239 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2603) -1711462110.217263 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=95 -1711462110.217227 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1304) => EVERYONE -1711462110.217299 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.217361 [0] dq.builtin: - deleting -1711462110.217384 [0] dq.builtin: delete -1711462110.218412 [0] gc: gc 0x7408c8066850: deleting -1711462110.218431 [0] gc: gc 0x7408c8016150: deleting -1711462110.218447 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1304) -1711462110.218461 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=94 -1711462110.218490 [0] gc: gc 0x7408c8066850: deleting -1711462110.218565 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.218588 [0] recv: INFOTS(1711462110.218401795) -1711462110.218607 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #47 L(:1c1 16957.878091)) -1711462110.218657 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #47: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1203}} -1711462110.218684 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1203) - deleting -1711462110.218703 [0] dq.builtin: => EVERYONE -1711462110.218748 [0] dq.builtin: delete -1711462110.218759 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.218933 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 2000 from udp/10.101.12.71:40473 -1711462110.218945 [0] recvMC: INFOTS(1711462110.218561783) -1711462110.218958 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #48 L(:1c1 16957.878448) => EVERYONE -1711462110.219087 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #48: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,3) -1711462110.219101 [0] recvMC: HEARTBEAT(F#57:48..48 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@48(sync) 110cd0d:e3b81b8d:1ccb65b1:504@48(sync) 110cd0d:79d6eeac:ea4a8907:504@48(sync)) -1711462110.219798 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.219816 [0] recv: INFOTS(1711462110.219653728) -1711462110.219838 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #43 L(:1c1 16957.879318)) -1711462110.219841 [0] gc: gc 0x7408c8016150: deleting -1711462110.219876 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #43: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1104}} -1711462110.219880 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1203) -1711462110.219917 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1104) => EVERYONE -1711462110.219955 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1203) -1711462110.219986 [0] dq.builtin: - deleting -1711462110.220189 [0] gc: gc 0x7408c8016150: deleting -1711462110.220231 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1203) -1711462110.220209 [0] dq.builtin: delete -1711462110.220250 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=93 -1711462110.220289 [0] gc: gc 0x7408c8006f50: deleting -1711462110.220306 [0] gc: gc_delete_proxy_reader (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:1104) -1711462110.220320 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=92 -1711462110.220340 [0] gc: gc 0x7408c8066850: deleting -1711462110.220352 [0] gc: gc 0x7408c8006f50: deleting -1711462110.221055 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.221079 [0] recv: INFOTS(1711462110.220906100) -1711462110.221101 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #48 L(:1c1 16957.880581)) -1711462110.221135 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #48: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1003}} -1711462110.221162 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1003) - deleting -1711462110.221179 [0] dq.builtin: => EVERYONE -1711462110.221225 [0] dq.builtin: delete -1711462110.221236 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.221390 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1952 from udp/10.101.12.71:40473 -1711462110.221411 [0] recvMC: INFOTS(1711462110.221033854) -1711462110.221437 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #49 L(:1c1 16957.880913) => EVERYONE -1711462110.221589 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #49: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,4) -1711462110.221607 [0] recvMC: HEARTBEAT(F#58:49..49 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@49(sync) 110cd0d:e3b81b8d:1ccb65b1:504@49(sync) 110cd0d:79d6eeac:ea4a8907:504@49(sync)) -1711462110.222300 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.222321 [0] recv: INFOTS(1711462110.222150811) -1711462110.222351 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #44 L(:1c1 16957.881824)) -1711462110.222326 [0] gc: gc 0x7408c8016150: deleting -1711462110.222387 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1003) -1711462110.222387 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #44: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:f04}} -1711462110.222413 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1003) -1711462110.222439 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:f04) => EVERYONE -1711462110.222464 [0] gc: gc 0x7408c8016150: deleting -1711462110.222523 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1003) -1711462110.222534 [0] dq.builtin: - deleting -1711462110.222569 [0] dq.builtin: delete -1711462110.222541 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=91 -1711462110.222643 [0] gc: gc 0x7408c8016150: deleting -1711462110.222652 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:f04) -1711462110.222659 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=90 -1711462110.222671 [0] gc: gc 0x7408c8066850: deleting -1711462110.222678 [0] gc: gc 0x7408c8016150: deleting -1711462110.223611 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.223632 [0] recv: INFOTS(1711462110.223428697) -1711462110.223657 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #49 L(:1c1 16957.883134)) -1711462110.223704 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #49: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:e03}} -1711462110.223727 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:e03) - deleting -1711462110.223740 [0] dq.builtin: => EVERYONE -1711462110.223778 [0] dq.builtin: delete -1711462110.223794 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.223898 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1904 from udp/10.101.12.71:40473 -1711462110.223914 [0] recvMC: INFOTS(1711462110.223556659) -1711462110.223963 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #50 L(:1c1 16957.883417) => EVERYONE -1711462110.224102 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #50: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,4) -1711462110.224117 [0] recvMC: HEARTBEAT(F#59:50..50 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@50(sync) 110cd0d:e3b81b8d:1ccb65b1:504@50(sync) 110cd0d:79d6eeac:ea4a8907:504@50(sync)) -1711462110.224802 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.224819 [0] recv: INFOTS(1711462110.224684003) -1711462110.224853 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #45 L(:1c1 16957.884322)) -1711462110.224887 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #45: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:d04}} -1711462110.224921 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:d04) => EVERYONE -1711462110.224888 [0] gc: gc 0x7408c8016150: deleting -1711462110.224958 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:e03) -1711462110.224964 [0] dq.builtin: - deleting -1711462110.224999 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:e03) -1711462110.225022 [0] dq.builtin: delete -1711462110.225031 [0] gc: gc 0x7408c8006f50: not yet, shortsleep -1711462110.226085 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.226107 [0] recv: INFOTS(1711462110.225949014) -1711462110.226130 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #50 L(:1c1 16957.885610)) -1711462110.226147 [0] gc: gc 0x7408c8006f50: deleting -1711462110.226171 [0] gc: gc_delete_proxy_reader (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:d04) -1711462110.226178 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #50: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:c03}} -1711462110.226200 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:c03) - deleting -1711462110.226215 [0] dq.builtin: => EVERYONE -1711462110.226182 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=89 -1711462110.226244 [0] dq.builtin: delete -1711462110.226249 [0] gc: gc 0x7408c8016150: deleting -1711462110.226314 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:e03) -1711462110.226328 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=88 -1711462110.226339 [0] gc: gc 0x7408c8006f50: deleting -1711462110.226356 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:c03) -1711462110.226368 [0] gc: gc 0x7408c8066850: deleting -1711462110.226376 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:c03) -1711462110.226377 [0] gc: gc 0x7408c8016150: deleting -1711462110.226402 [0] gc: gc 0x7408c8006f50: deleting -1711462110.226413 [0] gc: gc_delete_proxy_writer (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:c03) -1711462110.226418 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1856 from udp/10.101.12.71:40473 -1711462110.226422 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=87 -1711462110.226459 [0] recvMC: INFOTS(1711462110.226054803) -1711462110.226476 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.226518 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #51 L(:1c1 16957.885958) => EVERYONE -1711462110.226725 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #51: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,4) -1711462110.226792 [0] recvMC: HEARTBEAT(F#60:51..51 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@51(sync) 110cd0d:e3b81b8d:1ccb65b1:504@51(sync) 110cd0d:79d6eeac:ea4a8907:504@51(sync)) -1711462110.227307 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.227345 [0] recv: INFOTS(1711462110.227176717) -1711462110.227370 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #46 L(:1c1 16957.886848)) -1711462110.227414 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #46: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:b04}} -1711462110.227444 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:b04) => EVERYONE -1711462110.227477 [0] dq.builtin: - deleting -1711462110.227505 [0] dq.builtin: delete -1711462110.227592 [0] gc: gc 0x7408c8066850: deleting -1711462110.227625 [0] gc: gc 0x7408c8016150: deleting -1711462110.227639 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:b04) -1711462110.227652 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=86 -1711462110.227673 [0] gc: gc 0x7408c8066850: deleting -1711462110.228821 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.228910 [0] recv: INFOTS(1711462110.228522592) -1711462110.228951 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #51 L(:1c1 16957.888411)) -1711462110.228998 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #51: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:a03}} -1711462110.229019 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:a03) - deleting -1711462110.229042 [0] dq.builtin: => EVERYONE -1711462110.229086 [0] dq.builtin: delete -1711462110.229113 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.229235 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1808 from udp/10.101.12.71:40473 -1711462110.229248 [0] recvMC: INFOTS(1711462110.228719825) -1711462110.229262 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #52 L(:1c1 16957.888751) => EVERYONE -1711462110.229400 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #52: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.229415 [0] recvMC: HEARTBEAT(F#61:52..52 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@52(sync) 110cd0d:e3b81b8d:1ccb65b1:504@52(sync) 110cd0d:79d6eeac:ea4a8907:504@52(sync)) -1711462110.229999 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.230014 [0] recv: INFOTS(1711462110.229849806) -1711462110.230029 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #47 L(:1c1 16957.889516)) -1711462110.230053 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #47: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:904}} -1711462110.230072 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:904) => EVERYONE -1711462110.230109 [0] dq.builtin: - deleting -1711462110.230121 [0] dq.builtin: delete -1711462110.230235 [0] gc: gc 0x7408c8016150: deleting -1711462110.230255 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:a03) -1711462110.230272 [0] gc: gc 0x7408c8006f50: deleting -1711462110.230281 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:a03) -1711462110.230285 [0] gc: gc_delete_proxy_reader (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:904) -1711462110.230325 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=85 -1711462110.230351 [0] gc: gc 0x7408c8016150: deleting -1711462110.230362 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:a03) -1711462110.230374 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=84 -1711462110.230394 [0] gc: gc 0x7408c8066850: deleting -1711462110.230408 [0] gc: gc 0x7408c8016150: deleting -1711462110.231107 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.231117 [0] recv: INFOTS(1711462110.231018837) -1711462110.231131 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #52 L(:1c1 16957.890619)) -1711462110.231166 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #52: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:803}} -1711462110.231182 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:803) - deleting -1711462110.231188 [0] dq.builtin: => EVERYONE -1711462110.231210 [0] dq.builtin: delete -1711462110.231230 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.231409 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1784 from udp/10.101.12.71:40473 -1711462110.231419 [0] recvMC: INFOTS(1711462110.231152392) -1711462110.231430 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #53 L(:1c1 16957.890921) => EVERYONE -1711462110.231540 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #53: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.231550 [0] recvMC: HEARTBEAT(F#62:53..53 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@53(sync) 110cd0d:e3b81b8d:1ccb65b1:504@53(sync) 110cd0d:79d6eeac:ea4a8907:504@53(sync)) -1711462110.232296 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.232304 [0] recv: INFOTS(1711462110.232191115) -1711462110.232316 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #53 L(:1c1 16957.891807)) -1711462110.232327 [0] gc: gc 0x7408c8016150: deleting -1711462110.232349 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:803) -1711462110.232361 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #53: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:703}} -1711462110.232404 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:703 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:703) - deleting -1711462110.232420 [0] dq.builtin: => EVERYONE -1711462110.232404 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:803) -1711462110.232454 [0] dq.builtin: delete -1711462110.232476 [0] gc: gc 0x7408c807d440: not yet, shortsleep -1711462110.232716 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1736 from udp/10.101.12.71:40473 -1711462110.232730 [0] recvMC: INFOTS(1711462110.232345337) -1711462110.232748 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #54 L(:1c1 16957.892233) => EVERYONE -1711462110.232850 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #54: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,23,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.232865 [0] recvMC: HEARTBEAT(F#63:54..54 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@54(sync) 110cd0d:e3b81b8d:1ccb65b1:504@54(sync) 110cd0d:79d6eeac:ea4a8907:504@54(sync)) -1711462110.233446 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.233457 [0] recv: INFOTS(1711462110.233337855) -1711462110.233473 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #48 L(:1c1 16957.892960)) -1711462110.233506 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #48: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1504}} -1711462110.233525 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1504) => EVERYONE -1711462110.233548 [0] dq.builtin: - deleting -1711462110.233557 [0] dq.builtin: delete -1711462110.233586 [0] gc: gc 0x7408c807d440: deleting -1711462110.233607 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:703) -1711462110.233624 [0] gc: gc 0x7408c8016150: deleting -1711462110.233635 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:803) -1711462110.233641 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:703) -1711462110.233648 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=83 -1711462110.233697 [0] gc: gc 0x7408c8006f50: deleting -1711462110.233714 [0] gc: gc_delete_proxy_reader (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:1504) -1711462110.233727 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=82 -1711462110.233750 [0] gc: gc 0x7408c807d440: deleting -1711462110.233761 [0] gc: gc_delete_proxy_writer (0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:703) -1711462110.233773 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=81 -1711462110.233789 [0] gc: gc 0x7408c8066850: deleting -1711462110.233824 [0] gc: gc 0x7408c8006f50: deleting -1711462110.233836 [0] gc: gc 0x7408c807d440: deleting -1711462110.234700 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.234727 [0] recv: INFOTS(1711462110.234533470) -1711462110.234757 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #54 L(:1c1 16957.894228)) -1711462110.234794 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #54: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1403}} -1711462110.234815 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1403) - deleting -1711462110.234827 [0] dq.builtin: => EVERYONE -1711462110.234875 [0] dq.builtin: delete -1711462110.234897 [0] gc: gc 0x7408c8016150: deleting -1711462110.234919 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1403) -1711462110.234960 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1403) -1711462110.234997 [0] gc: gc 0x7408c8016150: deleting -1711462110.235016 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1403) -1711462110.235030 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=80 -1711462110.235054 [0] gc: gc 0x7408c8066850: deleting -1711462110.235142 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1688 from udp/10.101.12.71:40473 -1711462110.235152 [0] recvMC: INFOTS(1711462110.234735835) -1711462110.235166 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #55 L(:1c1 16957.894655) => EVERYONE -1711462110.235278 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #55: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,25,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.235296 [0] recvMC: HEARTBEAT(F#64:55..55 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@55(sync) 110cd0d:e3b81b8d:1ccb65b1:504@55(sync) 110cd0d:79d6eeac:ea4a8907:504@55(sync)) -1711462110.235938 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.235951 [0] recv: INFOTS(1711462110.235766500) -1711462110.235972 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #49 L(:1c1 16957.895453)) -1711462110.236013 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #49: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1704}} -1711462110.236041 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1704 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1704) => EVERYONE -1711462110.236063 [0] dq.builtin: - deleting -1711462110.236083 [0] dq.builtin: delete -1711462110.236093 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.237167 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.237254 [0] recv: INFOTS(1711462110.236982638) -1711462110.237180 [0] gc: gc 0x7408c8016150: deleting -1711462110.237277 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #55 L(:1c1 16957.896754)) -1711462110.237301 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1704) -1711462110.237320 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=79 -1711462110.237340 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.237340 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #55: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1603}} -1711462110.237379 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1603) - deleting -1711462110.237387 [0] dq.builtin: => EVERYONE -1711462110.237406 [0] dq.builtin: delete -1711462110.237502 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1640 from udp/10.101.12.71:40473 -1711462110.237521 [0] recvMC: INFOTS(1711462110.237105267) -1711462110.237536 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #56 L(:1c1 16957.897023) => EVERYONE -1711462110.237657 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #56: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,27,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.237669 [0] recvMC: HEARTBEAT(F#65:56..56 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@56(sync) 110cd0d:e3b81b8d:1ccb65b1:504@56(sync) 110cd0d:79d6eeac:ea4a8907:504@56(sync)) -1711462110.238358 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.238386 [0] recv: INFOTS(1711462110.238231579) -1711462110.238407 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #50 L(:1c1 16957.897886)) -1711462110.238430 [0] gc: gc 0x7408c8066850: deleting -1711462110.238433 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #50: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1904}} -1711462110.238451 [0] gc: gc 0x7408c8016150: deleting -1711462110.238470 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1603) -1711462110.238498 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1904) => EVERYONE -1711462110.238515 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1603) -1711462110.238529 [0] dq.builtin: - deleting -1711462110.238558 [0] gc: gc 0x7408c8016150: deleting -1711462110.238577 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1603) -1711462110.238559 [0] dq.builtin: delete -1711462110.238590 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=78 -1711462110.238624 [0] gc: gc 0x7408c8035250: deleting -1711462110.238633 [0] gc: gc_delete_proxy_reader (0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:1904) -1711462110.238658 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=77 -1711462110.238672 [0] gc: gc 0x7408c8066850: deleting -1711462110.238681 [0] gc: gc 0x7408c8035250: deleting -1711462110.239502 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.239517 [0] recv: INFOTS(1711462110.239384619) -1711462110.239532 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #56 L(:1c1 16957.899019)) -1711462110.239557 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #56: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1803}} -1711462110.239566 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1803) - deleting -1711462110.239574 [0] dq.builtin: => EVERYONE -1711462110.239598 [0] dq.builtin: delete -1711462110.239622 [0] gc: gc 0x7408c8016150: deleting -1711462110.239646 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1803) -1711462110.239672 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1803) -1711462110.239696 [0] gc: gc 0x7408c8016150: deleting -1711462110.239707 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1803) -1711462110.239718 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=76 -1711462110.239739 [0] gc: gc 0x7408c8066850: deleting -1711462110.239849 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1592 from udp/10.101.12.71:40473 -1711462110.239857 [0] recvMC: INFOTS(1711462110.239529783) -1711462110.239869 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #57 L(:1c1 16957.899360) => EVERYONE -1711462110.239958 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #57: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,29,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.239972 [0] recvMC: HEARTBEAT(F#66:57..57 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@57(sync) 110cd0d:e3b81b8d:1ccb65b1:504@57(sync) 110cd0d:79d6eeac:ea4a8907:504@57(sync)) -1711462110.240673 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.240693 [0] recv: INFOTS(1711462110.240557023) -1711462110.240719 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #51 L(:1c1 16957.900194)) -1711462110.240743 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #51: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1b04}} -1711462110.240758 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1b04) => EVERYONE -1711462110.240787 [0] dq.builtin: - deleting -1711462110.240806 [0] dq.builtin: delete -1711462110.240823 [0] gc: gc 0x7408c8016150: deleting -1711462110.240863 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1b04) -1711462110.240876 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=75 -1711462110.240892 [0] gc: gc 0x7408c8066850: deleting -1711462110.240938 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.240961 [0] recv: INFOTS(1711462110.240780572) -1711462110.240979 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #57 L(:1c1 16957.900461)) -1711462110.241001 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #57: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1a03}} -1711462110.241011 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1a03) - deleting -1711462110.241017 [0] dq.builtin: => EVERYONE -1711462110.241035 [0] dq.builtin: delete -1711462110.241047 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.241163 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1544 from udp/10.101.12.71:40473 -1711462110.241178 [0] recvMC: INFOTS(1711462110.240869006) -1711462110.241198 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #58 L(:1c1 16957.900680) => EVERYONE -1711462110.241324 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #58: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,31,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.241338 [0] recvMC: HEARTBEAT(F#67:58..58 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@58(sync) 110cd0d:e3b81b8d:1ccb65b1:504@58(sync) 110cd0d:79d6eeac:ea4a8907:504@58(sync)) -1711462110.242041 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.242075 [0] recv: INFOTS(1711462110.241943611) -1711462110.242099 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #52 L(:1c1 16957.901575)) -1711462110.242123 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #52: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1d04}} -1711462110.242132 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1d04) => EVERYONE -1711462110.242141 [0] gc: gc 0x7408c8016150: deleting -1711462110.242166 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1a03) -1711462110.242157 [0] dq.builtin: - deleting -1711462110.242192 [0] dq.builtin: delete -1711462110.242196 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1a03) -1711462110.242201 [0] gc: gc 0x7408c807c290: deleting -1711462110.242230 [0] gc: gc_delete_proxy_reader (0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:1d04) -1711462110.242239 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=74 -1711462110.242253 [0] gc: gc 0x7408c8016150: deleting -1711462110.242272 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1a03) -1711462110.242281 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=73 -1711462110.242294 [0] gc: gc 0x7408c8066850: deleting -1711462110.242303 [0] gc: gc 0x7408c8016150: deleting -1711462110.243206 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.243227 [0] recv: INFOTS(1711462110.243107441) -1711462110.243252 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #58 L(:1c1 16957.902729)) -1711462110.243274 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #58: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1c03}} -1711462110.243284 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1c03) - deleting -1711462110.243290 [0] dq.builtin: => EVERYONE -1711462110.243313 [0] dq.builtin: delete -1711462110.243326 [0] gc: gc 0x7408c8016150: deleting -1711462110.243342 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1c03) -1711462110.243365 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1c03) -1711462110.243390 [0] gc: gc 0x7408c8016150: deleting -1711462110.243397 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1c03) -1711462110.243407 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=72 -1711462110.243420 [0] gc: gc 0x7408c8066850: deleting -1711462110.243505 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1496 from udp/10.101.12.71:40473 -1711462110.243522 [0] recvMC: INFOTS(1711462110.243228417) -1711462110.243538 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #59 L(:1c1 16957.903025) => EVERYONE -1711462110.243635 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #59: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,33,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.243649 [0] recvMC: HEARTBEAT(F#68:59..59 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@59(sync) 110cd0d:e3b81b8d:1ccb65b1:504@59(sync) 110cd0d:79d6eeac:ea4a8907:504@59(sync)) -1711462110.244378 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.244398 [0] recv: INFOTS(1711462110.244270576) -1711462110.244414 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #53 L(:1c1 16957.903899)) -1711462110.244435 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #53: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1f04}} -1711462110.244446 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:1f04) => EVERYONE -1711462110.244463 [0] dq.builtin: - deleting -1711462110.244475 [0] dq.builtin: delete -1711462110.244483 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.245493 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.245513 [0] recv: INFOTS(1711462110.245398636) -1711462110.245529 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #59 L(:1c1 16957.905014)) -1711462110.245553 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #59: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:1e03}} -1711462110.245567 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:1e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:1e03) - deleting -1711462110.245576 [0] dq.builtin: => EVERYONE -1711462110.245585 [0] gc: gc 0x7408c8016150: deleting -1711462110.245598 [0] dq.builtin: delete -1711462110.245602 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:1f04) -1711462110.245621 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=71 -1711462110.245639 [0] gc: gc 0x7408c8006f50: deleting -1711462110.245645 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:1e03) -1711462110.245653 [0] gc: gc 0x7408c8066850: deleting -1711462110.245672 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:1e03) -1711462110.245720 [0] gc: gc 0x7408c8006f50: deleting -1711462110.245727 [0] gc: gc_delete_proxy_writer (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:1e03) -1711462110.245734 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=70 -1711462110.245742 [0] gc: gc 0x7408c8066850: deleting -1711462110.245799 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1448 from udp/10.101.12.71:40473 -1711462110.245807 [0] recvMC: INFOTS(1711462110.245476405) -1711462110.245819 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #60 L(:1c1 16957.905310) => EVERYONE -1711462110.245908 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #60: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,35,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.245917 [0] recvMC: HEARTBEAT(F#69:60..60 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@60(sync) 110cd0d:e3b81b8d:1ccb65b1:504@60(sync) 110cd0d:79d6eeac:ea4a8907:504@60(sync)) -1711462110.246634 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.246665 [0] recv: INFOTS(1711462110.246534188) -1711462110.246687 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #54 L(:1c1 16957.906165)) -1711462110.246714 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #54: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2104}} -1711462110.246730 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2104) => EVERYONE -1711462110.246752 [0] dq.builtin: - deleting -1711462110.246777 [0] dq.builtin: delete -1711462110.246783 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.246904 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.246928 [0] recv: INFOTS(1711462110.246821907) -1711462110.246943 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #60 L(:1c1 16957.906429)) -1711462110.246967 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #60: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2003}} -1711462110.246981 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2003) - deleting -1711462110.246998 [0] dq.builtin: => EVERYONE -1711462110.247021 [0] dq.builtin: delete -1711462110.247205 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1400 from udp/10.101.12.71:40473 -1711462110.247215 [0] recvMC: INFOTS(1711462110.246924796) -1711462110.247228 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #61 L(:1c1 16957.906718) => EVERYONE -1711462110.247313 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #61: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,37,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.247322 [0] recvMC: HEARTBEAT(F#70:61..61 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@61(sync) 110cd0d:e3b81b8d:1ccb65b1:504@61(sync) 110cd0d:79d6eeac:ea4a8907:504@61(sync)) -1711462110.247873 [0] gc: gc 0x7408c8016150: deleting -1711462110.247897 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2104) -1711462110.247907 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=69 -1711462110.247923 [0] gc: gc 0x7408c807d440: deleting -1711462110.247931 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:2003) -1711462110.247940 [0] gc: gc 0x7408c8066850: deleting -1711462110.247953 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:2003) -1711462110.247982 [0] gc: gc 0x7408c807d440: deleting -1711462110.247993 [0] gc: gc_delete_proxy_writer (0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:2003) -1711462110.248002 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=68 -1711462110.248014 [0] gc: gc 0x7408c8066850: deleting -1711462110.248090 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.248115 [0] recv: INFOTS(1711462110.248003659) -1711462110.248139 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #55 L(:1c1 16957.907615)) -1711462110.248160 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #55: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2304}} -1711462110.248176 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2304) => EVERYONE -1711462110.248200 [0] dq.builtin: - deleting -1711462110.248214 [0] dq.builtin: delete -1711462110.248223 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.249284 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.249326 [0] recv: INFOTS(1711462110.249152398) -1711462110.249303 [0] gc: gc 0x7408c8016150: deleting -1711462110.249366 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2304) -1711462110.249378 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=67 -1711462110.249353 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #61 L(:1c1 16957.908826)) -1711462110.249402 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.249427 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #61: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2203}} -1711462110.249436 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2203) - deleting -1711462110.249447 [0] dq.builtin: => EVERYONE -1711462110.249470 [0] dq.builtin: delete -1711462110.249564 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1352 from udp/10.101.12.71:40473 -1711462110.249574 [0] recvMC: INFOTS(1711462110.249287705) -1711462110.249589 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #62 L(:1c1 16957.909077) => EVERYONE -1711462110.249715 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #62: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.249727 [0] recvMC: HEARTBEAT(F#71:62..62 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@62(sync) 110cd0d:e3b81b8d:1ccb65b1:504@62(sync) 110cd0d:79d6eeac:ea4a8907:504@62(sync)) -1711462110.250399 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.250430 [0] recv: INFOTS(1711462110.250315538) -1711462110.250448 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #56 L(:1c1 16957.909921)) -1711462110.250472 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #56: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2504}} -1711462110.250487 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2504) => EVERYONE -1711462110.250505 [0] gc: gc 0x7408c8066850: deleting -1711462110.250515 [0] dq.builtin: - deleting -1711462110.250525 [0] gc: gc 0x7408c8016150: deleting -1711462110.250539 [0] dq.builtin: delete -1711462110.250553 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2203) -1711462110.250567 [0] gc: gc 0x7408c8035250: deleting -1711462110.250575 [0] gc: gc_delete_proxy_reader (0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:2504) -1711462110.250590 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=66 -1711462110.250580 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2203) -1711462110.250619 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.250676 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.250701 [0] recv: INFOTS(1711462110.250535418) -1711462110.250719 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #62 L(:1c1 16957.910201)) -1711462110.250742 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #62: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2403}} -1711462110.250753 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2403) - deleting -1711462110.250760 [0] dq.builtin: => EVERYONE -1711462110.250778 [0] dq.builtin: delete -1711462110.250812 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:403) suppressed, resched in inf s (min-ack 13, avail-seq 13, xmit 13) -1711462110.250922 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1304 from udp/10.101.12.71:40473 -1711462110.250937 [0] recvMC: INFOTS(1711462110.250646289) -1711462110.250953 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #63 L(:1c1 16957.910439) => EVERYONE -1711462110.251110 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #63: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,41,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.251138 [0] recvMC: HEARTBEAT(F#72:63..63 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@63(sync) 110cd0d:e3b81b8d:1ccb65b1:504@63(sync) 110cd0d:79d6eeac:ea4a8907:504@63(sync)) -1711462110.251522 [0] tev: writer_hbcontrol: wr 110cd0d:56a27910:57318171:3c2 multicasting (rel-prd 4 seq-eq-max 4 seq 10 maxseq 9) -1711462110.251550 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:3c2) sent, resched in 0.1 s (min-ack 9, avail-seq 10, xmit 10) -1711462110.251562 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 52 -1711462110.251623 [0] recv: HDR(110cd0d:56a27910:57318171 vendor 1.16) len 52 from udp/10.101.12.71:50807 -1711462110.251643 [0] recv: HEARTBEAT(#9:1..10 110cd0d:56a27910:57318171:3c2? -> 0:0:0:0) -1711462110.251627 [0] tev: nn_xpack_send 52: 0x7408c000139c:20 0x7408c4011ae8:32 [ udp/239.255.0.1:7400@2 ] -1711462110.251677 [0] tev: traffic-xmit (1) 52 -1711462110.251717 [0] gc: gc 0x7408c8066850: deleting -1711462110.251726 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 64 from udp/10.101.12.71:40473 -1711462110.251748 [0] gc: gc 0x7408c8016150: deleting -1711462110.251762 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462110.251780 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2203) -1711462110.251788 [0] recv: ACKNACK(F#3:11/0: L(:1c1 16957.911263) 110d7b4:17c5b8c5:94bd0ff4:3c7 -> 110cd0d:56a27910:57318171:3c2 ACK1 RM0) -1711462110.251802 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=65 -1711462110.251841 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 64 from udp/10.101.12.71:58212 -1711462110.251855 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462110.251846 [0] gc: gc 0x7408c807c290: deleting -1711462110.251882 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:2403) -1711462110.251872 [0] recv: ACKNACK(F#3:11/0: L(:1c1 16957.911357) 110e21c:d0762c48:a9f0fb28:3c7 -> 110cd0d:56a27910:57318171:3c2 ACK1 RM0) -1711462110.251901 [0] gc: gc 0x7408c8066850: deleting -1711462110.251914 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:2403) -1711462110.251919 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 64 from udp/10.101.12.71:44991 -1711462110.251963 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462110.251979 [0] recv: ACKNACK(F#3:11/0: L(:1c1 16957.911465) 110443d:bb7f10a5:18901533:3c7 -> 110cd0d:56a27910:57318171:3c2 ACK1 RM0) -1711462110.251990 [0] gc: gc 0x7408c807c290: deleting -1711462110.252011 [0] gc: gc_delete_proxy_writer (0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:2403) -1711462110.251994 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.252037 [0] recv: INFOTS(1711462110.251787909) -1711462110.252023 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=64 -1711462110.252053 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #57 L(:1c1 16957.911538)) -1711462110.252071 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.252089 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 64 from udp/10.101.12.71:52025 -1711462110.252114 [0] recv: INFODST(110cd0d:56a27910:57318171) -1711462110.252102 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #57: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5104}} -1711462110.252132 [0] recv: ACKNACK(F#3:11/0: L(:1c1 16957.911615) 110aba1:7b19badd:ce0adb73:3c7 -> 110cd0d:56a27910:57318171:3c2 ACK1 RM1) -1711462110.252151 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:5104) => EVERYONE -1711462110.252188 [0] dq.builtin: - deleting -1711462110.252199 [0] dq.builtin: delete -1711462110.253173 [0] gc: gc 0x7408c8066850: deleting -1711462110.253191 [0] gc: gc 0x7408c8016150: deleting -1711462110.253200 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5104) -1711462110.253209 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=63 -1711462110.253223 [0] gc: gc 0x7408c8066850: deleting -1711462110.253282 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.253300 [0] recv: INFOTS(1711462110.253143913) -1711462110.253324 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #63 L(:1c1 16957.912801)) -1711462110.253346 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #63: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5003}} -1711462110.253360 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5003) - deleting -1711462110.253369 [0] dq.builtin: => EVERYONE -1711462110.253402 [0] dq.builtin: delete -1711462110.253420 [0] gc: gc 0x7408c8016150: deleting -1711462110.253438 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5003) -1711462110.253474 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5003) -1711462110.253510 [0] gc: gc 0x7408c8016150: deleting -1711462110.253520 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5003) -1711462110.253545 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1256 from udp/10.101.12.71:40473 -1711462110.253560 [0] recvMC: INFOTS(1711462110.253252814) -1711462110.253547 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=62 -1711462110.253578 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #64 L(:1c1 16957.913062) => EVERYONE -1711462110.253599 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.253754 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #64: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,43,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.253772 [0] recvMC: HEARTBEAT(F#73:64..64 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@64(sync) 110cd0d:e3b81b8d:1ccb65b1:504@64(sync) 110cd0d:79d6eeac:ea4a8907:504@64(sync)) -1711462110.254430 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.254447 [0] recv: INFOTS(1711462110.254311896) -1711462110.254465 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #58 L(:1c1 16957.913949)) -1711462110.254485 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #58: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2904}} -1711462110.254502 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2904) => EVERYONE -1711462110.254522 [0] dq.builtin: - deleting -1711462110.254530 [0] dq.builtin: delete -1711462110.254685 [0] gc: gc 0x7408c8066850: deleting -1711462110.254701 [0] gc: gc 0x7408c8016150: deleting -1711462110.254709 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2904) -1711462110.254717 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=61 -1711462110.254730 [0] gc: gc 0x7408c8066850: deleting -1711462110.255572 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.255839 [0] recv: INFOTS(1711462110.255450496) -1711462110.255853 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #64 L(:1c1 16957.915341)) -1711462110.255881 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #64: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2803}} -1711462110.255901 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2803) - deleting -1711462110.255916 [0] dq.builtin: => EVERYONE -1711462110.255963 [0] dq.builtin: delete -1711462110.255975 [0] gc: gc 0x7408c8016150: deleting -1711462110.255988 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1208 from udp/10.101.12.71:40473 -1711462110.256000 [0] recvMC: INFOTS(1711462110.255527534) -1711462110.255989 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2803) -1711462110.256020 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #65 L(:1c1 16957.915502) => EVERYONE -1711462110.256048 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2803) -1711462110.256087 [0] gc: gc 0x7408c8016150: deleting -1711462110.256097 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2803) -1711462110.256108 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=60 -1711462110.256123 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.256195 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #65: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,45,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.256212 [0] recvMC: HEARTBEAT(F#74:65..65 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@65(sync) 110cd0d:e3b81b8d:1ccb65b1:504@65(sync) 110cd0d:79d6eeac:ea4a8907:504@65(sync)) -1711462110.256649 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.256668 [0] recv: INFOTS(1711462110.256559919) -1711462110.256701 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #59 L(:1c1 16957.916170)) -1711462110.256741 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #59: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2b04}} -1711462110.256773 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2b04) => EVERYONE -1711462110.256801 [0] dq.builtin: - deleting -1711462110.256809 [0] dq.builtin: delete -1711462110.257196 [0] gc: gc 0x7408c8066850: deleting -1711462110.257212 [0] gc: gc 0x7408c8016150: deleting -1711462110.257219 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2b04) -1711462110.257227 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=59 -1711462110.257240 [0] gc: gc 0x7408c8066850: deleting -1711462110.257806 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.257820 [0] recv: INFOTS(1711462110.257696628) -1711462110.257833 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #65 L(:1c1 16957.917322)) -1711462110.257876 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #65: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2a03}} -1711462110.257895 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2a03) - deleting -1711462110.257901 [0] dq.builtin: => EVERYONE -1711462110.257920 [0] dq.builtin: delete -1711462110.257933 [0] gc: gc 0x7408c8016150: deleting -1711462110.257947 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2a03) -1711462110.257968 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2a03) -1711462110.258005 [0] gc: gc 0x7408c8016150: deleting -1711462110.258016 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2a03) -1711462110.258025 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=58 -1711462110.258039 [0] gc: gc 0x7408c8066850: deleting -1711462110.258063 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1160 from udp/10.101.12.71:40473 -1711462110.258090 [0] recvMC: INFOTS(1711462110.257775243) -1711462110.258109 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #66 L(:1c1 16957.917592) => EVERYONE -1711462110.258254 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #66: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,47,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.258270 [0] recvMC: HEARTBEAT(F#75:66..66 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@66(sync) 110cd0d:e3b81b8d:1ccb65b1:504@66(sync) 110cd0d:79d6eeac:ea4a8907:504@66(sync)) -1711462110.258986 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.259013 [0] recv: INFOTS(1711462110.258846924) -1711462110.259031 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #60 L(:1c1 16957.918515)) -1711462110.259071 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #60: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2d04}} -1711462110.259096 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2d04) => EVERYONE -1711462110.259121 [0] dq.builtin: - deleting -1711462110.259129 [0] dq.builtin: delete -1711462110.259142 [0] gc: gc 0x7408c8016150: deleting -1711462110.259157 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2d04) -1711462110.259168 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=57 -1711462110.259183 [0] gc: gc 0x7408c8066850: deleting -1711462110.260389 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.260404 [0] recv: INFOTS(1711462110.260256234) -1711462110.260427 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #66 L(:1c1 16957.919905)) -1711462110.260467 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #66: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2c03}} -1711462110.260491 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2c03) - deleting -1711462110.260531 [0] dq.builtin: => EVERYONE -1711462110.260564 [0] dq.builtin: delete -1711462110.260581 [0] gc: gc 0x7408c8016150: deleting -1711462110.260613 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2c03) -1711462110.260639 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2c03) -1711462110.260668 [0] gc: gc 0x7408c8016150: deleting -1711462110.260696 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2c03) -1711462110.260707 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=56 -1711462110.260723 [0] gc: gc 0x7408c8066850: deleting -1711462110.260725 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1112 from udp/10.101.12.71:40473 -1711462110.260772 [0] recvMC: INFOTS(1711462110.260382757) -1711462110.260790 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #67 L(:1c1 16957.920273) => EVERYONE -1711462110.260972 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #67: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,49,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.260991 [0] recvMC: HEARTBEAT(F#76:67..67 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@67(sync) 110cd0d:e3b81b8d:1ccb65b1:504@67(sync) 110cd0d:79d6eeac:ea4a8907:504@67(sync)) -1711462110.261641 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.261669 [0] recv: INFOTS(1711462110.261474906) -1711462110.261693 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #61 L(:1c1 16957.921171)) -1711462110.261757 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #61: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2f04}} -1711462110.261782 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:2f04) => EVERYONE -1711462110.261809 [0] dq.builtin: - deleting -1711462110.261824 [0] dq.builtin: delete -1711462110.261843 [0] gc: gc 0x7408c8016150: deleting -1711462110.261864 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2f04) -1711462110.261881 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=55 -1711462110.261901 [0] gc: gc 0x7408c8066850: deleting -1711462110.262875 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.262900 [0] recv: INFOTS(1711462110.262693665) -1711462110.262922 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #67 L(:1c1 16957.922401)) -1711462110.262965 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #67: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:2e03}} -1711462110.262992 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:2e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:2e03) - deleting -1711462110.263001 [0] dq.builtin: => EVERYONE -1711462110.263037 [0] dq.builtin: delete -1711462110.263058 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.263186 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1064 from udp/10.101.12.71:40473 -1711462110.263202 [0] recvMC: INFOTS(1711462110.262818245) -1711462110.263367 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #68 L(:1c1 16957.922703) => EVERYONE -1711462110.263618 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #68: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,51,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.263653 [0] recvMC: HEARTBEAT(F#77:68..68 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@68(sync) 110cd0d:e3b81b8d:1ccb65b1:504@68(sync) 110cd0d:79d6eeac:ea4a8907:504@68(sync)) -1711462110.264166 [0] gc: gc 0x7408c8016150: deleting -1711462110.264199 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2e03) -1711462110.264173 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.264244 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2e03) -1711462110.264275 [0] recv: INFOTS(1711462110.263960068) -1711462110.264319 [0] gc: gc 0x7408c8016150: deleting -1711462110.264367 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:2e03) -1711462110.264349 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #62 L(:1c1 16957.923773)) -1711462110.264395 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=54 -1711462110.264444 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #62: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3104}} -1711462110.264445 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.264466 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3104) => EVERYONE -1711462110.264520 [0] dq.builtin: - deleting -1711462110.264535 [0] dq.builtin: delete -1711462110.265429 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.265459 [0] recv: INFOTS(1711462110.265251831) -1711462110.265480 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #68 L(:1c1 16957.924959)) -1711462110.265527 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #68: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3003}} -1711462110.265546 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3003) - deleting -1711462110.265561 [0] dq.builtin: => EVERYONE -1711462110.265564 [0] gc: gc 0x7408c8066850: deleting -1711462110.265597 [0] dq.builtin: delete -1711462110.265635 [0] gc: gc 0x7408c8016150: deleting -1711462110.265680 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3104) -1711462110.265695 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=53 -1711462110.265717 [0] gc: gc 0x7408c8006f50: deleting -1711462110.265727 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:3003) -1711462110.265740 [0] gc: gc 0x7408c8066850: deleting -1711462110.265775 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 1016 from udp/10.101.12.71:40473 -1711462110.265757 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:3003) -1711462110.265808 [0] recvMC: INFOTS(1711462110.265383925) -1711462110.265855 [0] gc: gc 0x7408c8006f50: deleting -1711462110.265876 [0] gc: gc_delete_proxy_writer (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:3003) -1711462110.265861 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #69 L(:1c1 16957.925308) => EVERYONE -1711462110.265893 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=52 -1711462110.265916 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.266080 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #69: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,53,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.266109 [0] recvMC: HEARTBEAT(F#78:69..69 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@69(sync) 110cd0d:e3b81b8d:1ccb65b1:504@69(sync) 110cd0d:79d6eeac:ea4a8907:504@69(sync)) -1711462110.266663 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.266687 [0] recv: INFOTS(1711462110.266492303) -1711462110.266710 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #63 L(:1c1 16957.926188)) -1711462110.266752 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #63: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3304}} -1711462110.266778 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3304) => EVERYONE -1711462110.266808 [0] dq.builtin: - deleting -1711462110.266823 [0] dq.builtin: delete -1711462110.267005 [0] gc: gc 0x7408c8066850: deleting -1711462110.267031 [0] gc: gc 0x7408c8016150: deleting -1711462110.267040 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3304) -1711462110.267051 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=51 -1711462110.267069 [0] gc: gc 0x7408c8066850: deleting -1711462110.268335 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.268349 [0] recv: INFOTS(1711462110.268216434) -1711462110.268368 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #69 L(:1c1 16957.927850)) -1711462110.268417 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #69: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3203}} -1711462110.268437 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3203) - deleting -1711462110.268451 [0] dq.builtin: => EVERYONE -1711462110.268473 [0] dq.builtin: delete -1711462110.268489 [0] gc: gc 0x7408c8016150: deleting -1711462110.268550 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3203) -1711462110.268604 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3203) -1711462110.268635 [0] gc: gc 0x7408c8016150: deleting -1711462110.268643 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 968 from udp/10.101.12.71:40473 -1711462110.268644 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3203) -1711462110.268671 [0] recvMC: INFOTS(1711462110.268318557) -1711462110.268676 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=50 -1711462110.268693 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #70 L(:1c1 16957.928172) => EVERYONE -1711462110.268703 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.268839 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #70: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,55,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.268861 [0] recvMC: HEARTBEAT(F#79:70..70 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@70(sync) 110cd0d:e3b81b8d:1ccb65b1:504@70(sync) 110cd0d:79d6eeac:ea4a8907:504@70(sync)) -1711462110.269576 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.269601 [0] recv: INFOTS(1711462110.269427108) -1711462110.269625 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #64 L(:1c1 16957.929102)) -1711462110.269665 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #64: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3504}} -1711462110.269684 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3504) => EVERYONE -1711462110.269718 [0] dq.builtin: - deleting -1711462110.269731 [0] dq.builtin: delete -1711462110.269791 [0] gc: gc 0x7408c8066850: deleting -1711462110.269816 [0] gc: gc 0x7408c8016150: deleting -1711462110.269826 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3504) -1711462110.269836 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=49 -1711462110.269854 [0] gc: gc 0x7408c8066850: deleting -1711462110.270719 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.270736 [0] recv: INFOTS(1711462110.270594373) -1711462110.270768 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #70 L(:1c1 16957.930236)) -1711462110.270805 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #70: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3403}} -1711462110.270830 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3403) - deleting -1711462110.270837 [0] dq.builtin: => EVERYONE -1711462110.270865 [0] dq.builtin: delete -1711462110.270888 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.271030 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 920 from udp/10.101.12.71:40473 -1711462110.271052 [0] recvMC: INFOTS(1711462110.270694611) -1711462110.271074 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #71 L(:1c1 16957.930553) => EVERYONE -1711462110.271211 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #71: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,57,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.271232 [0] recvMC: HEARTBEAT(F#80:71..71 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@71(sync) 110cd0d:e3b81b8d:1ccb65b1:504@71(sync) 110cd0d:79d6eeac:ea4a8907:504@71(sync)) -1711462110.271908 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.271923 [0] recv: INFOTS(1711462110.271774725) -1711462110.271944 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #65 L(:1c1 16957.931424)) -1711462110.271989 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #65: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3704}} -1711462110.272013 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3704 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3704) => EVERYONE -1711462110.271999 [0] gc: gc 0x7408c8016150: deleting -1711462110.272031 [0] dq.builtin: - deleting -1711462110.272057 [0] dq.builtin: delete -1711462110.272045 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3403) -1711462110.272090 [0] gc: gc 0x7408c807d440: deleting -1711462110.272100 [0] gc: gc_delete_proxy_reader (0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:3704) -1711462110.272116 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=48 -1711462110.272103 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3403) -1711462110.272135 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.273239 [0] gc: gc 0x7408c8066850: deleting -1711462110.273262 [0] gc: gc 0x7408c8016150: deleting -1711462110.273272 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3403) -1711462110.273281 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=47 -1711462110.273298 [0] gc: gc 0x7408c8066850: deleting -1711462110.273327 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.273344 [0] recv: INFOTS(1711462110.273181976) -1711462110.273364 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #71 L(:1c1 16957.932846)) -1711462110.273402 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #71: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3603}} -1711462110.273425 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3603) - deleting -1711462110.273451 [0] dq.builtin: => EVERYONE -1711462110.273478 [0] dq.builtin: delete -1711462110.273488 [0] gc: gc 0x7408c8016150: deleting -1711462110.273502 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3603) -1711462110.273526 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3603) -1711462110.273553 [0] gc: gc 0x7408c8016150: deleting -1711462110.273564 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3603) -1711462110.273574 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=46 -1711462110.273581 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 872 from udp/10.101.12.71:40473 -1711462110.273596 [0] gc: gc 0x7408c8066850: deleting -1711462110.273618 [0] recvMC: INFOTS(1711462110.273304522) -1711462110.273655 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #72 L(:1c1 16957.933119) => EVERYONE -1711462110.273809 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #72: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,59,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,) -1711462110.273834 [0] recvMC: HEARTBEAT(F#81:72..72 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@72(sync) 110cd0d:e3b81b8d:1ccb65b1:504@72(sync) 110cd0d:79d6eeac:ea4a8907:504@72(sync)) -1711462110.274653 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.274688 [0] recv: INFOTS(1711462110.274472079) -1711462110.274715 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #66 L(:1c1 16957.934187)) -1711462110.274761 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #66: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3904}} -1711462110.274783 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3904) => EVERYONE -1711462110.274804 [0] dq.builtin: - deleting -1711462110.274816 [0] dq.builtin: delete -1711462110.274827 [0] gc: gc 0x7408c8016150: deleting -1711462110.274842 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3904) -1711462110.274854 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=45 -1711462110.274872 [0] gc: gc 0x7408c8066850: deleting -1711462110.275981 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.276010 [0] recv: INFOTS(1711462110.275834015) -1711462110.276041 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #72 L(:1c1 16957.935510)) -1711462110.276081 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #72: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3803}} -1711462110.276105 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3803) - deleting -1711462110.276126 [0] dq.builtin: => EVERYONE -1711462110.276159 [0] dq.builtin: delete -1711462110.276173 [0] gc: gc 0x7408c8016150: deleting -1711462110.276199 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3803) -1711462110.276231 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3803) -1711462110.276261 [0] gc: gc 0x7408c8016150: deleting -1711462110.276275 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3803) -1711462110.276286 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=44 -1711462110.276297 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 824 from udp/10.101.12.71:40473 -1711462110.276349 [0] recvMC: INFOTS(1711462110.275936014) -1711462110.276322 [0] gc: gc 0x7408c8066850: deleting -1711462110.276373 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #73 L(:1c1 16957.935849) => EVERYONE -1711462110.276573 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #73: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,61,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0) -1711462110.276600 [0] recvMC: HEARTBEAT(F#82:73..73 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@73(sync) 110cd0d:e3b81b8d:1ccb65b1:504@73(sync) 110cd0d:79d6eeac:ea4a8907:504@73(sync)) -1711462110.277013 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.277060 [0] recv: HEARTBEAT(#8:1..13 L(:1c1 16957.936539)110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@13(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@13(sync) 110cd0d:79d6eeac:ea4a8907:4c7@13(sync)) -1711462110.277088 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#4:14/0: -1711462110.277113 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462110.277139 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.277159 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#12:14/0: -1711462110.277174 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462110.277189 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.277206 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110443d:bb7f10a5:18901533:4c2: F#7:14/0: -1711462110.277220 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110443d:bb7f10a5:18901533:4c2) -1711462110.277233 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.277301 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7416@2 ] -1711462110.277319 [0] tev: traffic-xmit (1) 168 -1711462110.277484 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.277526 [0] recv: INFOTS(1711462110.277107873) -1711462110.277554 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #67 L(:1c1 16957.937026)) -1711462110.277597 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #67: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3b04}} -1711462110.277620 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3b04) => EVERYONE -1711462110.277674 [0] dq.builtin: - deleting -1711462110.277688 [0] dq.builtin: delete -1711462110.277708 [0] gc: gc 0x7408c8016150: deleting -1711462110.277735 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3b04) -1711462110.277750 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=43 -1711462110.277773 [0] gc: gc 0x7408c8066850: deleting -1711462110.278616 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.278662 [0] recv: HEARTBEAT(#7:1..19 L(:1c1 16957.938144)110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@19(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@19(sync) 110cd0d:79d6eeac:ea4a8907:4c7@19(sync)) -1711462110.278688 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#4:20/0: -1711462110.278719 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462110.278740 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.278692 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.278796 [0] recv: INFOTS(1711462110.278533644) -1711462110.278765 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#12:20/0: -1711462110.278842 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462110.278861 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.278823 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #73 L(:1c1 16957.938295)) -1711462110.278888 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110aba1:7b19badd:ce0adb73:4c2: F#7:20/0: -1711462110.278940 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110aba1:7b19badd:ce0adb73:4c2) -1711462110.278961 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.278960 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #73: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3a03}} -1711462110.279012 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7418@2 ] -1711462110.279035 [0] tev: traffic-xmit (1) 168 -1711462110.279015 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3a03) - deleting -1711462110.279086 [0] dq.builtin: => EVERYONE -1711462110.279124 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 776 from udp/10.101.12.71:40473 -1711462110.279137 [0] dq.builtin: delete -1711462110.279159 [0] recvMC: INFOTS(1711462110.278642503) -1711462110.279158 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.279189 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #74 L(:1c1 16957.938659) => EVERYONE -1711462110.279391 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #74: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,63,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.279440 [0] recvMC: HEARTBEAT(F#83:74..74 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@74(sync) 110cd0d:e3b81b8d:1ccb65b1:504@74(sync) 110cd0d:79d6eeac:ea4a8907:504@74(sync)) -1711462110.279494 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.279529 [0] recv: HEARTBEAT(#8:1..19 L(:1c1 16957.939014)110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@19(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@19(sync) 110cd0d:79d6eeac:ea4a8907:3c7@19(sync)) -1711462110.279548 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#4:20/0: -1711462110.279572 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.279586 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.279604 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#14:20/0: -1711462110.279616 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.279626 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.279644 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#8:20/0: -1711462110.279656 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.279667 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.279709 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7416@2 ] -1711462110.279723 [0] tev: traffic-xmit (1) 168 -1711462110.279900 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.279956 [0] recv: HEARTBEAT(#7:1..23 L(:1c1 16957.939436)110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@23(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@23(sync) 110cd0d:79d6eeac:ea4a8907:3c7@23(sync)) -1711462110.279969 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#4:24/0: -1711462110.280003 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462110.280018 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.280034 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#14:24/0: -1711462110.280045 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462110.280056 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.280069 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#8:24/0: -1711462110.280083 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462110.280106 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.280085 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.280162 [0] recv: INFOTS(1711462110.279820853) -1711462110.280163 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7418@2 ] -1711462110.280205 [0] tev: traffic-xmit (1) 168 -1711462110.280189 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #68 L(:1c1 16957.939662)) -1711462110.280272 [0] gc: gc 0x7408c8016150: deleting -1711462110.280284 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #68: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3d04}} -1711462110.280294 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3a03) -1711462110.280318 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3d04) => EVERYONE -1711462110.280345 [0] dq.builtin: - deleting -1711462110.280356 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3a03) -1711462110.280360 [0] dq.builtin: delete -1711462110.280370 [0] gc: gc 0x7408c8035250: not yet, shortsleep -1711462110.281318 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.281338 [0] recv: INFOTS(1711462110.281176913) -1711462110.281359 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #74 L(:1c1 16957.940839)) -1711462110.281390 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #74: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3c03}} -1711462110.281410 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3c03) - deleting -1711462110.281419 [0] dq.builtin: => EVERYONE -1711462110.281447 [0] dq.builtin: delete -1711462110.281466 [0] gc: gc 0x7408c8035250: deleting -1711462110.281487 [0] gc: gc_delete_proxy_reader (0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:3d04) -1711462110.281499 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=42 -1711462110.281519 [0] gc: gc 0x7408c8016150: deleting -1711462110.281531 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3a03) -1711462110.281547 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=41 -1711462110.281564 [0] gc: gc 0x7408c807c290: deleting -1711462110.281576 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:3c03) -1711462110.281591 [0] gc: gc 0x7408c8066850: deleting -1711462110.281601 [0] gc: gc 0x7408c8016150: deleting -1711462110.281606 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:3c03) -1711462110.281611 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 728 from udp/10.101.12.71:40473 -1711462110.281663 [0] recvMC: INFOTS(1711462110.281294357) -1711462110.281645 [0] gc: gc 0x7408c807c290: deleting -1711462110.281700 [0] gc: gc_delete_proxy_writer (0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:3c03) -1711462110.281685 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #75 L(:1c1 16957.941163) => EVERYONE -1711462110.281717 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=40 -1711462110.281740 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.281887 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #75: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,65,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,64,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.281927 [0] recvMC: HEARTBEAT(F#84:75..75 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@75(sync) 110cd0d:e3b81b8d:1ccb65b1:504@75(sync) 110cd0d:79d6eeac:ea4a8907:504@75(sync)) -1711462110.282652 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.282676 [0] recv: INFOTS(1711462110.282396477) -1711462110.282701 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #69 L(:1c1 16957.942177)) -1711462110.282733 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #69: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3f04}} -1711462110.282754 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3f04) => EVERYONE -1711462110.282783 [0] dq.builtin: - deleting -1711462110.282797 [0] dq.builtin: delete -1711462110.282833 [0] gc: gc 0x7408c8066850: deleting -1711462110.282855 [0] gc: gc 0x7408c8016150: deleting -1711462110.282871 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3f04) -1711462110.282884 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=39 -1711462110.282903 [0] gc: gc 0x7408c8066850: deleting -1711462110.283784 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.283818 [0] recv: INFOTS(1711462110.283671990) -1711462110.283847 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #75 L(:1c1 16957.943319)) -1711462110.283879 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #75: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:3e03}} -1711462110.283904 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:3e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3e03) - deleting -1711462110.283917 [0] dq.builtin: => EVERYONE -1711462110.283957 [0] dq.builtin: delete -1711462110.283970 [0] gc: gc 0x7408c8016150: deleting -1711462110.284004 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3e03) -1711462110.284054 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3e03) -1711462110.284093 [0] gc: gc 0x7408c8016150: deleting -1711462110.284108 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:3e03) -1711462110.284120 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=38 -1711462110.284138 [0] gc: gc 0x7408c8066850: deleting -1711462110.284164 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 680 from udp/10.101.12.71:40473 -1711462110.284196 [0] recvMC: INFOTS(1711462110.283774544) -1711462110.284220 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #76 L(:1c1 16957.943696) => EVERYONE -1711462110.284387 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #76: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,67,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,66,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,68,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.284431 [0] recvMC: HEARTBEAT(F#85:76..76 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@76(sync) 110cd0d:e3b81b8d:1ccb65b1:504@76(sync) 110cd0d:79d6eeac:ea4a8907:504@76(sync)) -1711462110.285011 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.285042 [0] recv: INFOTS(1711462110.284892655) -1711462110.285072 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #70 L(:1c1 16957.944542)) -1711462110.285107 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #70: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4104}} -1711462110.285138 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4104 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4104) => EVERYONE -1711462110.285178 [0] dq.builtin: - deleting -1711462110.285201 [0] dq.builtin: delete -1711462110.285215 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.286220 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.286248 [0] recv: INFOTS(1711462110.286072873) -1711462110.286272 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #76 L(:1c1 16957.945749)) -1711462110.286308 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #76: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4003}} -1711462110.286317 [0] gc: gc 0x7408c8016150: deleting -1711462110.286331 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4003 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4003) - deleting -1711462110.286359 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4104) -1711462110.286380 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=37 -1711462110.286405 [0] dq.builtin: => EVERYONE -1711462110.286406 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.286442 [0] dq.builtin: delete -1711462110.286568 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 632 from udp/10.101.12.71:40473 -1711462110.286600 [0] recvMC: INFOTS(1711462110.286193571) -1711462110.286627 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #77 L(:1c1 16957.946100) => EVERYONE -1711462110.286802 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #77: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,69,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,68,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,70,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.286843 [0] recvMC: HEARTBEAT(F#86:77..77 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@77(sync) 110cd0d:e3b81b8d:1ccb65b1:504@77(sync) 110cd0d:79d6eeac:ea4a8907:504@77(sync)) -1711462110.287376 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.287405 [0] recv: INFOTS(1711462110.287238788) -1711462110.287428 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #71 L(:1c1 16957.946905)) -1711462110.287463 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #71: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4304}} -1711462110.287487 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4304) => EVERYONE -1711462110.287531 [0] dq.builtin: - deleting -1711462110.287556 [0] dq.builtin: delete -1711462110.287534 [0] gc: gc 0x7408c8066850: deleting -1711462110.287612 [0] gc: gc 0x7408c8016150: deleting -1711462110.287627 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4003) -1711462110.287646 [0] gc: gc 0x7408c8006f50: deleting -1711462110.287661 [0] gc: gc_delete_proxy_reader (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:4304) -1711462110.287679 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=36 -1711462110.287662 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4003) -1711462110.287703 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.288611 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.288721 [0] recv: INFOTS(1711462110.288409621) -1711462110.288747 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #77 L(:1c1 16957.948216)) -1711462110.288777 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #77: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4203}} -1711462110.288802 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4203) - deleting -1711462110.288817 [0] dq.builtin: => EVERYONE -1711462110.288806 [0] gc: gc 0x7408c8066850: deleting -1711462110.288852 [0] dq.builtin: delete -1711462110.288869 [0] gc: gc 0x7408c8016150: deleting -1711462110.288907 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4003) -1711462110.288923 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=35 -1711462110.288946 [0] gc: gc 0x7408c807d440: deleting -1711462110.288959 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:4203) -1711462110.288976 [0] gc: gc 0x7408c8066850: deleting -1711462110.288980 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 584 from udp/10.101.12.71:40473 -1711462110.288987 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:4203) -1711462110.289039 [0] recvMC: INFOTS(1711462110.288540698) -1711462110.289072 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #78 L(:1c1 16957.948538) => EVERYONE -1711462110.289091 [0] gc: gc 0x7408c807d440: deleting -1711462110.289114 [0] gc: gc_delete_proxy_writer (0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:4203) -1711462110.289131 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=34 -1711462110.289154 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.289247 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #78: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,71,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,70,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.289290 [0] recvMC: HEARTBEAT(F#87:78..78 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@78(sync) 110cd0d:e3b81b8d:1ccb65b1:504@78(sync) 110cd0d:79d6eeac:ea4a8907:504@78(sync)) -1711462110.289727 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.289750 [0] recv: INFOTS(1711462110.289583439) -1711462110.289770 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #72 L(:1c1 16957.949252)) -1711462110.289827 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #72: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4504}} -1711462110.289856 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4504) => EVERYONE -1711462110.289890 [0] dq.builtin: - deleting -1711462110.289904 [0] dq.builtin: delete -1711462110.290241 [0] gc: gc 0x7408c8066850: deleting -1711462110.290265 [0] gc: gc 0x7408c8016150: deleting -1711462110.290276 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4504) -1711462110.290288 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=33 -1711462110.290309 [0] gc: gc 0x7408c8066850: deleting -1711462110.290885 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.290910 [0] recv: INFOTS(1711462110.290746464) -1711462110.290930 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #78 L(:1c1 16957.950413)) -1711462110.290966 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #78: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4403}} -1711462110.290983 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4403) - deleting -1711462110.290994 [0] dq.builtin: => EVERYONE -1711462110.291089 [0] dq.builtin: delete -1711462110.291144 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 536 from udp/10.101.12.71:40473 -1711462110.291186 [0] recvMC: INFOTS(1711462110.290874452) -1711462110.291218 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #79 L(:1c1 16957.950686) => EVERYONE -1711462110.291376 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.291420 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #79: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,73,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,72,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.291475 [0] recvMC: HEARTBEAT(F#88:79..79 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@79(sync) 110cd0d:e3b81b8d:1ccb65b1:504@79(sync) 110cd0d:79d6eeac:ea4a8907:504@79(sync)) -1711462110.292054 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.292080 [0] recv: INFOTS(1711462110.291919173) -1711462110.292100 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #73 L(:1c1 16957.951582)) -1711462110.292140 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #73: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4704}} -1711462110.292169 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4704 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4704) => EVERYONE -1711462110.292219 [0] dq.builtin: - deleting -1711462110.292241 [0] dq.builtin: delete -1711462110.292376 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.292392 [0] recv: INFOTS(1711462110.292287600) -1711462110.292416 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #79 L(:1c1 16957.951895)) -1711462110.292454 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #79: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4603}} -1711462110.292471 [0] gc: gc 0x7408c8016150: deleting -1711462110.292488 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4603) - deleting -1711462110.292534 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4403) -1711462110.292557 [0] gc: gc 0x7408c8035250: deleting -1711462110.292574 [0] gc: gc_delete_proxy_reader (0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:4704) -1711462110.292593 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=32 -1711462110.292563 [0] dq.builtin: => EVERYONE -1711462110.292617 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.292574 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4403) -1711462110.292678 [0] dq.builtin: delete -1711462110.292830 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 488 from udp/10.101.12.71:40473 -1711462110.292850 [0] recvMC: INFOTS(1711462110.292409838) -1711462110.292875 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #80 L(:1c1 16957.952351) => EVERYONE -1711462110.293030 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #80: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,75,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,74,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,) -1711462110.293071 [0] recvMC: HEARTBEAT(F#89:80..80 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@80(sync) 110cd0d:e3b81b8d:1ccb65b1:504@80(sync) 110cd0d:79d6eeac:ea4a8907:504@80(sync)) -1711462110.293606 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.293625 [0] recv: INFOTS(1711462110.293464039) -1711462110.293649 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #74 L(:1c1 16957.953127)) -1711462110.293672 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.293705 [0] recv: HEARTBEAT(#10:1..15 L(:1c1 16957.953189)110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@15(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@15(sync) 110cd0d:79d6eeac:ea4a8907:4c7@15(sync)) -1711462110.293712 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #74: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4904}} -1711462110.293751 [0] gc: gc 0x7408c8066850: deleting -1711462110.293788 [0] gc: gc 0x7408c807c290: deleting -1711462110.293744 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#4:16/0: -1711462110.293800 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:4603) -1711462110.293827 [0] gc: gc 0x7408c8016150: deleting -1711462110.293843 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4403) -1711462110.293860 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=31 -1711462110.293869 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.293906 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.293881 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.293882 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:4603) -1711462110.293766 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4904 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4904) => EVERYONE -1711462110.293933 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#12:16/0: -1711462110.294056 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.294072 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.294091 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#7:16/0: -1711462110.294104 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.294119 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.294106 [0] dq.builtin: - deleting -1711462110.294180 [0] dq.builtin: delete -1711462110.294181 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7412@2 ] -1711462110.294224 [0] tev: traffic-xmit (1) 168 -1711462110.294786 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.294808 [0] recv: INFOTS(1711462110.294634250) -1711462110.294834 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #80 L(:1c1 16957.954309)) -1711462110.294900 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #80: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4803}} -1711462110.294917 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4803) - deleting -1711462110.294947 [0] dq.builtin: => EVERYONE -1711462110.294983 [0] dq.builtin: delete -1711462110.295010 [0] gc: gc 0x7408c8066850: deleting -1711462110.295023 [0] gc: gc 0x7408c807c290: deleting -1711462110.295032 [0] gc: gc_delete_proxy_writer (0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:4603) -1711462110.295042 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=30 -1711462110.295056 [0] gc: gc 0x7408c8016150: deleting -1711462110.295064 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4904) -1711462110.295072 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=29 -1711462110.295083 [0] gc: gc 0x7408c8006f50: deleting -1711462110.295093 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:4803) -1711462110.295107 [0] gc: gc 0x7408c8066850: deleting -1711462110.295116 [0] gc: gc 0x7408c8016150: deleting -1711462110.295130 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:4803) -1711462110.295167 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 440 from udp/10.101.12.71:40473 -1711462110.295187 [0] gc: gc 0x7408c8006f50: deleting -1711462110.295227 [0] gc: gc_delete_proxy_writer (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:4803) -1711462110.295197 [0] recvMC: INFOTS(1711462110.294787650) -1711462110.295248 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=28 -1711462110.295280 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #81 L(:1c1 16957.954697) => EVERYONE -1711462110.295308 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.295469 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #81: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,77,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,76,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.295497 [0] recvMC: HEARTBEAT(F#90:81..81 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@81(sync) 110cd0d:e3b81b8d:1ccb65b1:504@81(sync) 110cd0d:79d6eeac:ea4a8907:504@81(sync)) -1711462110.295514 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.295560 [0] recv: HEARTBEAT(#10:1..19 L(:1c1 16957.955042)110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@19(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@19(sync) 110cd0d:79d6eeac:ea4a8907:3c7@19(sync)) -1711462110.295582 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#4:20/0: -1711462110.295598 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.295620 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.295634 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#14:20/0: -1711462110.295660 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.295690 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.295706 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#8:20/0: -1711462110.295716 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.295729 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.295778 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7412@2 ] -1711462110.295787 [0] tev: traffic-xmit (1) 168 -1711462110.295935 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.295959 [0] recv: INFOTS(1711462110.295791082) -1711462110.295982 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #75 L(:1c1 16957.955459)) -1711462110.296032 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #75: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4b04}} -1711462110.296052 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4b04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4b04) => EVERYONE -1711462110.296079 [0] dq.builtin: - deleting -1711462110.296096 [0] dq.builtin: delete -1711462110.296406 [0] gc: gc 0x7408c8066850: deleting -1711462110.296431 [0] gc: gc 0x7408c8016150: deleting -1711462110.296451 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4b04) -1711462110.296461 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=27 -1711462110.296477 [0] gc: gc 0x7408c8066850: deleting -1711462110.297080 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.297109 [0] recv: INFOTS(1711462110.296972092) -1711462110.297145 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #81 L(:1c1 16957.956609)) -1711462110.297197 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #81: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4a03}} -1711462110.297288 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4a03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4a03) - deleting -1711462110.297305 [0] dq.builtin: => EVERYONE -1711462110.297354 [0] dq.builtin: delete -1711462110.297368 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.297404 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 392 from udp/10.101.12.71:40473 -1711462110.297421 [0] recvMC: INFOTS(1711462110.297082585) -1711462110.297439 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #82 L(:1c1 16957.956922) => EVERYONE -1711462110.297569 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #82: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,79,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,78,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.297589 [0] recvMC: HEARTBEAT(F#91:82..82 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@82(sync) 110cd0d:e3b81b8d:1ccb65b1:504@82(sync) 110cd0d:79d6eeac:ea4a8907:504@82(sync)) -1711462110.298246 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.298267 [0] recv: INFOTS(1711462110.298143983) -1711462110.298292 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #76 L(:1c1 16957.957767)) -1711462110.298323 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #76: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4d04}} -1711462110.298347 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4d04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4d04) => EVERYONE -1711462110.298381 [0] dq.builtin: - deleting -1711462110.298395 [0] dq.builtin: delete -1711462110.298502 [0] gc: gc 0x7408c8016150: deleting -1711462110.298512 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4a03) -1711462110.298520 [0] gc: gc 0x7408c807d440: deleting -1711462110.298527 [0] gc: gc_delete_proxy_reader (0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:4d04) -1711462110.298534 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=26 -1711462110.298535 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4a03) -1711462110.298549 [0] gc: gc 0x7408c8066850: not yet, shortsleep -1711462110.298578 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.298616 [0] recv: INFOTS(1711462110.298471822) -1711462110.298637 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #82 L(:1c1 16957.958117)) -1711462110.298666 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #82: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4c03}} -1711462110.298687 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4c03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4c03) - deleting -1711462110.298699 [0] dq.builtin: => EVERYONE -1711462110.298731 [0] dq.builtin: delete -1711462110.298843 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 344 from udp/10.101.12.71:40473 -1711462110.298857 [0] recvMC: INFOTS(1711462110.298593713) -1711462110.298879 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #83 L(:1c1 16957.958359) => EVERYONE -1711462110.298975 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #83: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,83,4,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,82,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.298993 [0] recvMC: HEARTBEAT(F#92:83..83 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@83(sync) 110cd0d:e3b81b8d:1ccb65b1:504@83(sync) 110cd0d:79d6eeac:ea4a8907:504@83(sync)) -1711462110.299671 [0] gc: gc 0x7408c8066850: deleting -1711462110.299690 [0] gc: gc 0x7408c8016150: deleting -1711462110.299695 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4a03) -1711462110.299703 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=25 -1711462110.299714 [0] gc: gc 0x7408c8035250: deleting -1711462110.299720 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:4c03) -1711462110.299729 [0] gc: gc 0x7408c8066850: deleting -1711462110.299742 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:4c03) -1711462110.299780 [0] gc: gc 0x7408c8035250: deleting -1711462110.299787 [0] gc: gc_delete_proxy_writer (0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:4c03) -1711462110.299794 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=24 -1711462110.299802 [0] gc: gc 0x7408c8066850: deleting -1711462110.299853 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.299868 [0] recv: INFOTS(1711462110.299733268) -1711462110.299888 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #77 L(:1c1 16957.959370)) -1711462110.299915 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #77: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4f04}} -1711462110.299935 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4f04 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4f04) => EVERYONE -1711462110.299958 [0] dq.builtin: - deleting -1711462110.299971 [0] dq.builtin: delete -1711462110.299976 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.301064 [0] gc: gc 0x7408c8016150: deleting -1711462110.301081 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4f04) -1711462110.301087 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=23 -1711462110.301101 [0] gc: gc 0x7408c8066850: deleting -1711462110.301109 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.301132 [0] recv: INFOTS(1711462110.301005028) -1711462110.301151 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #83 L(:1c1 16957.960633)) -1711462110.301240 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #83: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:4e03}} -1711462110.301251 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:4e03 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4e03) - deleting -1711462110.301265 [0] dq.builtin: => EVERYONE -1711462110.301288 [0] dq.builtin: delete -1711462110.301294 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.301367 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 296 from udp/10.101.12.71:40473 -1711462110.301390 [0] recvMC: INFOTS(1711462110.301158893) -1711462110.301420 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #84 L(:1c1 16957.960891) => EVERYONE -1711462110.301503 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #84: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,85,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,84,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.301522 [0] recvMC: HEARTBEAT(F#93:84..84 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@84(sync) 110cd0d:e3b81b8d:1ccb65b1:504@84(sync) 110cd0d:79d6eeac:ea4a8907:504@84(sync)) -1711462110.302352 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.302369 [0] recv: INFOTS(1711462110.302235006) -1711462110.302371 [0] gc: gc 0x7408c8016150: deleting -1711462110.302396 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #78 L(:1c1 16957.961870)) -1711462110.302448 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4e03) -1711462110.302494 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #78: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5304}} -1711462110.302525 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5304 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:5304) => EVERYONE -1711462110.302504 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4e03) -1711462110.302615 [0] dq.builtin: - deleting -1711462110.302630 [0] gc: gc 0x7408c8016150: deleting -1711462110.302659 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4e03) -1711462110.302638 [0] dq.builtin: delete -1711462110.302677 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=22 -1711462110.302719 [0] gc: gc 0x7408c807c290: deleting -1711462110.302729 [0] gc: gc_delete_proxy_reader (0x7408c807c290, 110d7b4:17c5b8c5:94bd0ff4:5304) -1711462110.302738 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=21 -1711462110.302753 [0] gc: gc 0x7408c8066850: deleting -1711462110.302762 [0] gc: gc 0x7408c807c290: deleting -1711462110.303572 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.303593 [0] recv: INFOTS(1711462110.303466791) -1711462110.303618 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #84 L(:1c1 16957.963094)) -1711462110.303655 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #84: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5203}} -1711462110.303675 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5203 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5203) - deleting -1711462110.303690 [0] dq.builtin: => EVERYONE -1711462110.303748 [0] dq.builtin: delete -1711462110.303758 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.303848 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 248 from udp/10.101.12.71:40473 -1711462110.303869 [0] recvMC: INFOTS(1711462110.303608296) -1711462110.303893 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #85 L(:1c1 16957.963371) => EVERYONE -1711462110.303962 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #85: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,86,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.303982 [0] recvMC: HEARTBEAT(F#94:85..85 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@85(sync) 110cd0d:e3b81b8d:1ccb65b1:504@85(sync) 110cd0d:79d6eeac:ea4a8907:504@85(sync)) -1711462110.304785 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.304809 [0] recv: INFOTS(1711462110.304665223) -1711462110.304831 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #79 L(:1c1 16957.964310)) -1711462110.304855 [0] gc: gc 0x7408c8016150: deleting -1711462110.304937 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5203) -1711462110.304919 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #79: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5504}} -1711462110.304980 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:5504) => EVERYONE -1711462110.304987 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5203) -1711462110.305014 [0] dq.builtin: - deleting -1711462110.305026 [0] dq.builtin: delete -1711462110.305029 [0] gc: gc 0x7408c8016150: deleting -1711462110.305061 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5203) -1711462110.305092 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=20 -1711462110.305115 [0] gc: gc 0x7408c8006f50: deleting -1711462110.305129 [0] gc: gc_delete_proxy_reader (0x7408c8006f50, 110d7b4:17c5b8c5:94bd0ff4:5504) -1711462110.305139 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=19 -1711462110.305156 [0] gc: gc 0x7408c8066850: deleting -1711462110.305168 [0] gc: gc 0x7408c8006f50: deleting -1711462110.306098 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.306122 [0] recv: INFOTS(1711462110.305944071) -1711462110.306150 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #85 L(:1c1 16957.965621)) -1711462110.306180 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #85: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5403}} -1711462110.306194 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5403) - deleting -1711462110.306202 [0] dq.builtin: => EVERYONE -1711462110.306235 [0] dq.builtin: delete -1711462110.306258 [0] gc: gc 0x7408c8016150: deleting -1711462110.306285 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5403) -1711462110.306319 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5403) -1711462110.306355 [0] gc: gc 0x7408c8016150: deleting -1711462110.306376 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5403) -1711462110.306390 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=18 -1711462110.306415 [0] gc: gc 0x7408c8066850: deleting -1711462110.307436 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 224 from udp/10.101.12.71:40473 -1711462110.307464 [0] recvMC: INFOTS(1711462110.307152640) -1711462110.307493 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #86 L(:1c1 16957.966964) => EVERYONE -1711462110.307688 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #86: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,87,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.307708 [0] recvMC: HEARTBEAT(F#95:86..86 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@86(sync) 110cd0d:e3b81b8d:1ccb65b1:504@86(sync) 110cd0d:79d6eeac:ea4a8907:504@86(sync)) -1711462110.307891 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.307913 [0] recv: INFOTS(1711462110.307705837) -1711462110.307939 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #86 L(:1c1 16957.967414)) -1711462110.307992 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #86: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5603}} -1711462110.308021 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5603) - deleting -1711462110.308045 [0] dq.builtin: => EVERYONE -1711462110.308103 [0] dq.builtin: delete -1711462110.308118 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.308342 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 200 from udp/10.101.12.71:40473 -1711462110.308358 [0] recvMC: INFOTS(1711462110.308113998) -1711462110.308374 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #87 L(:1c1 16957.967860) => EVERYONE -1711462110.308433 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #87: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,88,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.308465 [0] recvMC: HEARTBEAT(F#96:87..87 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@87(sync) 110cd0d:e3b81b8d:1ccb65b1:504@87(sync) 110cd0d:79d6eeac:ea4a8907:504@87(sync)) -1711462110.308848 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.308867 [0] recv: INFOTS(1711462110.308696854) -1711462110.308906 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #87 L(:1c1 16957.968369)) -1711462110.308973 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #87: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5703}} -1711462110.309002 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5703 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5703) - deleting -1711462110.309020 [0] dq.builtin: => EVERYONE -1711462110.309065 [0] dq.builtin: delete -1711462110.309121 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 176 from udp/10.101.12.71:40473 -1711462110.309154 [0] recvMC: INFOTS(1711462110.308810509) -1711462110.309180 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #88 L(:1c1 16957.968654) => EVERYONE -1711462110.309216 [0] gc: gc 0x7408c8016150: deleting -1711462110.309250 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462110.309256 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #88: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,6,3,0,0,0,0,0,0,0,0}}}}}}}) -1711462110.309304 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462110.309282 [0] gc: gc 0x7408c807d440: deleting -1711462110.309320 [0] recvMC: HEARTBEAT(F#97:88..88 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@88(sync) 110cd0d:e3b81b8d:1ccb65b1:504@88(sync) 110cd0d:79d6eeac:ea4a8907:504@88(sync)) -1711462110.309365 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:5703) -1711462110.309410 [0] gc: gc 0x7408c8016150: deleting -1711462110.309430 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5603) -1711462110.309417 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:5703) -1711462110.309463 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:1404, 13): 9 -> 13 -1711462110.309496 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:1404, 5): 1 -> 5 -1711462110.309522 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:1404, 9): 5 -> 9 -1711462110.309543 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=17 -1711462110.309568 [0] gc: gc 0x7408c807d440: deleting -1711462110.309579 [0] gc: gc_delete_proxy_writer (0x7408c807d440, 110d7b4:17c5b8c5:94bd0ff4:5703) -1711462110.309590 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=16 -1711462110.309606 [0] gc: gc 0x7408c8066850: deleting -1711462110.309617 [0] gc: gc 0x7408c807d440: deleting -1711462110.310027 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.310052 [0] recv: INFOTS(1711462110.309878806) -1711462110.310076 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #88 L(:1c1 16957.969554)) -1711462110.310123 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #88: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:5803}} -1711462110.310177 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:5803 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:5803) - deleting -1711462110.310199 [0] dq.builtin: => EVERYONE -1711462110.310248 [0] dq.builtin: delete -1711462110.310266 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.310293 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 152 from udp/10.101.12.71:40473 -1711462110.310317 [0] recvMC: INFOTS(1711462110.310031935) -1711462110.310345 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #89 L(:1c1 16957.969817) => EVERYONE -1711462110.310404 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #89: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","map_manager",{},{}}}}) -1711462110.310430 [0] recvMC: HEARTBEAT(F#98:89..89 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@89(sync) 110cd0d:e3b81b8d:1ccb65b1:504@89(sync) 110cd0d:79d6eeac:ea4a8907:504@89(sync)) -1711462110.311197 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.311219 [0] recv: INFOTS(1711462110.311071865) -1711462110.311233 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #89 L(:1c1 16957.970722)) -1711462110.311281 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #89: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:603}} -1711462110.311311 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:603 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:603) - deleting -1711462110.311323 [0] dq.builtin: => EVERYONE -1711462110.311331 [0] recvMC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 120 from udp/10.101.12.71:40473 -1711462110.311359 [0] gc: gc 0x7408c8016150: deleting -1711462110.311386 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5803) -1711462110.311407 [0] gc: gc 0x7408c8035250: not yet, shortsleep -1711462110.311417 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5803) -1711462110.311367 [0] recvMC: INFOTS(1711462110.311185386) -1711462110.311369 [0] dq.builtin: delete -1711462110.311494 [0] recvMC: DATA(110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0 #90 L(:1c1 16957.970868) => EVERYONE -1711462110.311544 [0] recvMC: data(application, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:403 #90: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,215,180,23,197,184,197,148,189,15,244,0,0,1,193,0,0,0,0,0,0,0,0}}},{}}) -1711462110.311565 [0] recvMC: HEARTBEAT(F#99:90..90 110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@90(sync) 110cd0d:e3b81b8d:1ccb65b1:504@90(sync) 110cd0d:79d6eeac:ea4a8907:504@90(sync)) -1711462110.312519 [0] gc: gc 0x7408c8035250: deleting -1711462110.312540 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:603) -1711462110.312552 [0] gc: gc 0x7408c8016150: deleting -1711462110.312568 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:5803) -1711462110.312559 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:603) -1711462110.312584 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=15 -1711462110.312626 [0] gc: gc 0x7408c8035250: deleting -1711462110.312638 [0] gc: gc_delete_proxy_writer (0x7408c8035250, 110d7b4:17c5b8c5:94bd0ff4:603) -1711462110.312649 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=14 -1711462110.312665 [0] gc: gc 0x7408c8066850: deleting -1711462110.312674 [0] gc: gc 0x7408c8035250: deleting -1711462110.314605 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.314644 [0] recv: HEARTBEAT(#9:1..79 L(:1c1 16957.974125)110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@79(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@79(sync) 110cd0d:79d6eeac:ea4a8907:4c7@79(sync)) -1711462110.314685 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#4:80/0: -1711462110.314712 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.314731 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.314742 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#12:80/0: -1711462110.314749 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.314757 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.314765 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#7:80/0: -1711462110.314772 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.314779 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.314839 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7414@2 ] -1711462110.314848 [0] tev: traffic-xmit (1) 168 -1711462110.315992 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.316028 [0] recv: HEARTBEAT(#10:1..89 L(:1c1 16957.975517)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@89(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@89(sync) 110cd0d:79d6eeac:ea4a8907:3c7@89(sync)) -1711462110.316053 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#5:90/0: -1711462110.316074 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.316089 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.316109 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#15:90/0: -1711462110.316121 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.316133 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.316142 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#9:90/0: -1711462110.316148 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.316156 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.316177 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7414@2 ] -1711462110.316190 [0] tev: traffic-xmit (1) 168 -1711462110.351587 [0] tev: heartbeat(wr 110cd0d:56a27910:57318171:3c2) suppressed, resched in inf s (min-ack 10, avail-seq 9, xmit 10) -1711462110.376661 [0] recvMC: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.376766 [0] recvMC: HEARTBEAT(#29:20..20 L(:1c1 16958.036225)110443d:bb7f10a5:18901533:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@20(sync) 110cd0d:e3b81b8d:1ccb65b1:504@20(sync) 110cd0d:79d6eeac:ea4a8907:504@20(sync)) -1711462110.376867 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110443d:bb7f10a5:18901533:403: F#5:21/0: -1711462110.376914 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.376941 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.376965 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110443d:bb7f10a5:18901533:403: F#21:21/0: -1711462110.377007 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.377021 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.377038 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110443d:bb7f10a5:18901533:403: F#12:21/0: -1711462110.377048 [0] recvMC: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.377051 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110443d:bb7f10a5:18901533:403) -1711462110.377113 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.377086 [0] recvMC: HEARTBEAT(#35:24..24 L(:1c1 16958.036571)110aba1:7b19badd:ce0adb73:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@24(sync) 110cd0d:e3b81b8d:1ccb65b1:504@24(sync) 110cd0d:79d6eeac:ea4a8907:504@24(sync)) -1711462110.377138 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110aba1:7b19badd:ce0adb73:403: F#5:25/0: -1711462110.377203 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462110.377278 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408c4011ad8:44 0x7408bc009950:24 0x7408bc009978:28 0x7408bc0097d0:24 0x7408bc0097f8:28 [ udp/10.101.12.71:7417@2 ] -1711462110.377312 [0] tev: traffic-xmit (1) 168 -1711462110.377330 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.377352 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110aba1:7b19badd:ce0adb73:403: F#21:25/0: -1711462110.377377 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462110.377391 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.377412 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110aba1:7b19badd:ce0adb73:403: F#12:25/0: -1711462110.377506 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110aba1:7b19badd:ce0adb73:403) -1711462110.377522 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.377564 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0094e8:44 0x7408c4011ac0:24 0x7408c4011ae8:28 0x7408bc009950:24 0x7408bc009978:28 [ udp/10.101.12.71:7419@2 ] -1711462110.377581 [0] tev: traffic-xmit (1) 168 -1711462110.378904 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.378972 [0] recv: INFOTS(1711462110.378453601) -1711462110.379036 [0] recv: DATA(110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0 #20 L(:1c1 16958.038470)) -1711462110.379077 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.379091 [0] recv: INFOTS(1711462110.378380676) -1711462110.379111 [0] recv: DATA(110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0 #24 L(:1c1 16958.038592)) -1711462110.379153 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:3c2 #20: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:403}} -1711462110.379206 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:403 ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:403) - deleting -1711462110.379227 [0] dq.builtin: => EVERYONE -1711462110.379295 [0] dq.builtin: delete -1711462110.379322 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:3c2 #24: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:403}} -1711462110.379340 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:403 ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:403) - deleting -1711462110.379371 [0] dq.builtin: => EVERYONE -1711462110.379377 [0] gc: gc 0x7408c8016150: deleting -1711462110.379443 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110443d:bb7f10a5:18901533:403) -1711462110.379466 [0] gc: gc 0x7408c807c290: not yet, shortsleep -1711462110.379423 [0] dq.builtin: delete -1711462110.379483 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110443d:bb7f10a5:18901533:403) -1711462110.379699 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 52 from udp/10.101.12.71:44991 -1711462110.379772 [0] recv: HEARTBEAT(#9:20..20 L(:1c1 16958.039247)110443d:bb7f10a5:18901533:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@20(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@20(sync) 110cd0d:79d6eeac:ea4a8907:3c7@20(sync)) -1711462110.379804 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.379821 [0] recv: INFOTS(1711462110.379581375) -1711462110.379845 [0] recv: DATA(110443d:bb7f10a5:18901533:4c2 -> 0:0:0:0 #14 L(:1c1 16958.039322)) -1711462110.379803 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#5:21/0: -1711462110.379917 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.379932 [0] dq.builtin: data(builtin, vendor 1.16): 110443d:bb7f10a5:18901533:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110443d:bb7f10a5:18901533:504}} -1711462110.379937 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.380075 [0] dq.builtin: SEDP ST3 110443d:bb7f10a5:18901533:504 ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:504) => EVERYONE -1711462110.380022 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 52 from udp/10.101.12.71:52025 -1711462110.380130 [0] recv: HEARTBEAT(#8:24..24 L(:1c1 16958.039619)110aba1:7b19badd:ce0adb73:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@24(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@24(sync) 110cd0d:79d6eeac:ea4a8907:3c7@24(sync)) -1711462110.380136 [0] dq.builtin: - deleting -1711462110.380308 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#15:21/0: -1711462110.380341 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.380331 [0] dq.builtin: delete -1711462110.380355 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.380434 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110443d:bb7f10a5:18901533:3c2: F#9:21/0: -1711462110.380457 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110443d:bb7f10a5:18901533:3c2) -1711462110.380473 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009880 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.380490 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#9:25/0: -1711462110.380550 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462110.380587 [0] gc: gc 0x7408c807c290: deleting -1711462110.380616 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0094e8:44 0x7408c4011ac0:24 0x7408c4011ae8:28 0x7408bc009950:24 0x7408bc009978:28 [ udp/10.101.12.71:7416@2 ] -1711462110.380638 [0] tev: traffic-xmit (1) 168 -1711462110.380617 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807c290, 110aba1:7b19badd:ce0adb73:403) -1711462110.380660 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.380691 [0] gc: gc 0x7408c8016150: deleting -1711462110.380707 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c807c290, 110aba1:7b19badd:ce0adb73:403) -1711462110.380713 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110443d:bb7f10a5:18901533:403) -1711462110.380780 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:504, 22): 17 -> 22 -1711462110.380791 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.380817 [0] recv: INFOTS(1711462110.380434543) -1711462110.380831 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#5:25/0: -1711462110.380902 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462110.380934 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.380804 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:504, 6): 1 -> 6 -1711462110.380954 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110aba1:7b19badd:ce0adb73:3c2: F#15:25/0: -1711462110.381024 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110aba1:7b19badd:ce0adb73:3c2) -1711462110.381039 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.380845 [0] recv: DATA(110aba1:7b19badd:ce0adb73:4c2 -> 0:0:0:0 #20 L(:1c1 16958.040317)) -1711462110.381009 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:504, 13): 8 -> 13 -1711462110.381096 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0097e8:44 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408c4011ac0:24 0x7408c4011ae8:28 [ udp/10.101.12.71:7418@2 ] -1711462110.381113 [0] dq.builtin: data(builtin, vendor 1.16): 110aba1:7b19badd:ce0adb73:4c2 #20: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110aba1:7b19badd:ce0adb73:504}} -1711462110.381141 [0] tev: traffic-xmit (1) 168 -1711462110.381115 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=13 -1711462110.381206 [0] gc: gc 0x7408c8006f50: deleting -1711462110.381228 [0] gc: gc_delete_proxy_reader (0x7408c8006f50, 110443d:bb7f10a5:18901533:504) -1711462110.381210 [0] dq.builtin: SEDP ST3 110aba1:7b19badd:ce0adb73:504 ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:504) => EVERYONE -1711462110.381269 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 udp/10.101.12.71:7419@2 -1711462110.381289 [0] dq.builtin: - deleting -1711462110.381289 [0] gc: reduced nlocs=4 -1711462110.381341 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381357 [0] gc: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462110.381370 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462110.381384 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381397 [0] gc: rdidx 1 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462110.381410 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 3 -> 8 -1711462110.381427 [0] gc: a b -1711462110.381443 [0] gc: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. } -1711462110.381463 [0] gc: loc 1 = udp/10.101.12.71:7415@2 1 { .. +u } -1711462110.381479 [0] gc: loc 2 = udp/10.101.12.71:7419@2 2147483647 { .. .. } -1711462110.381496 [0] gc: loc 3 = udp/239.255.0.1:7401@2 1 { +1 +1 } -1711462110.381510 [0] gc: best = 3 -1711462110.381527 [0] gc: simple udp/239.255.0.1:7401@2 -1711462110.381548 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462110.381582 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 -1711462110.381601 [0] gc: reduced nlocs=3 -1711462110.381619 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381638 [0] gc: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462110.381654 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.381669 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381685 [0] gc: rdidx 1 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462110.381699 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.381714 [0] gc: a b -1711462110.381584 [0] dq.builtin: delete -1711462110.381757 [0] gc: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. } -1711462110.381789 [0] gc: loc 1 = udp/10.101.12.71:7415@2 1 { .. +u } -1711462110.381807 [0] gc: loc 2 = udp/239.255.0.1:7401@2 1 { +1 +1 } -1711462110.381815 [0] gc: best = 2 -1711462110.381824 [0] gc: simple udp/239.255.0.1:7401@2 -1711462110.381839 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462110.381863 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 -1711462110.381873 [0] gc: reduced nlocs=3 -1711462110.381885 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381895 [0] gc: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462110.381904 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.381912 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.381921 [0] gc: rdidx 1 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462110.381930 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.381939 [0] gc: a b -1711462110.381949 [0] gc: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. } -1711462110.381958 [0] gc: loc 1 = udp/10.101.12.71:7415@2 1 { .. +u } -1711462110.381968 [0] gc: loc 2 = udp/239.255.0.1:7401@2 1 { +1 +1 } -1711462110.381975 [0] gc: best = 2 -1711462110.381983 [0] gc: simple udp/239.255.0.1:7401@2 -1711462110.381995 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462110.382007 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=12 -1711462110.382023 [0] gc: gc 0x7408c807c290: deleting -1711462110.382032 [0] gc: gc_delete_proxy_writer (0x7408c807c290, 110aba1:7b19badd:ce0adb73:403) -1711462110.382048 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:504, 22): 22 -> 22 -1711462110.382067 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:504, 6): 6 -> 6 -1711462110.382081 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:504, 13): 13 -> 13 -1711462110.382093 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=13 -1711462110.382106 [0] gc: gc 0x7408c8066850: deleting -1711462110.382117 [0] gc: gc 0x7408c80057c0: deleting -1711462110.382124 [0] gc: gc_delete_proxy_reader (0x7408c80057c0, 110aba1:7b19badd:ce0adb73:504) -1711462110.382141 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 -1711462110.382150 [0] gc: reduced nlocs=3 -1711462110.382160 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.382169 [0] gc: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462110.382178 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.382186 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.382195 [0] gc: rdidx 1 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462110.382204 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.382213 [0] gc: a b -1711462110.382223 [0] gc: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. } -1711462110.382234 [0] gc: loc 1 = udp/10.101.12.71:7415@2 1 { .. +u } -1711462110.382246 [0] gc: loc 2 = udp/239.255.0.1:7401@2 1 { +1 +1 } -1711462110.382252 [0] gc: best = 2 -1711462110.382271 [0] gc: simple udp/239.255.0.1:7401@2 -1711462110.382286 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462110.382306 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 -1711462110.382315 [0] gc: reduced nlocs=3 -1711462110.382325 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.382334 [0] gc: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462110.382342 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.382351 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.382360 [0] gc: rdidx 1 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462110.382368 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.382377 [0] gc: a b -1711462110.382387 [0] gc: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. } -1711462110.382398 [0] gc: loc 1 = udp/10.101.12.71:7415@2 1 { .. +u } -1711462110.382412 [0] gc: loc 2 = udp/239.255.0.1:7401@2 1 { +1 +1 } -1711462110.382418 [0] gc: best = 2 -1711462110.382426 [0] gc: simple udp/239.255.0.1:7401@2 -1711462110.382438 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462110.382455 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7413@2 udp/10.101.12.71:7415@2 -1711462110.382464 [0] gc: reduced nlocs=3 -1711462110.382474 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.382485 [0] gc: rdidx 0 lidx udp/10.101.12.71:7413@2 0 -> 0 -1711462110.382495 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.382504 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.382513 [0] gc: rdidx 1 lidx udp/10.101.12.71:7415@2 1 -> 0 -1711462110.382521 [0] gc: rdidx 1 lidx udp/239.255.0.1:7401@2 2 -> 8 -1711462110.382530 [0] gc: a b -1711462110.382539 [0] gc: loc 0 = udp/10.101.12.71:7413@2 1 { +u .. } -1711462110.382548 [0] gc: loc 1 = udp/10.101.12.71:7415@2 1 { .. +u } -1711462110.382558 [0] gc: loc 2 = udp/239.255.0.1:7401@2 1 { +1 +1 } -1711462110.382564 [0] gc: best = 2 -1711462110.382572 [0] gc: simple udp/239.255.0.1:7401@2 -1711462110.382583 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:403): udp/239.255.0.1:7401@2 (burst size 4294901760 rexmit 1048576) -1711462110.382593 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=12 -1711462110.382607 [0] gc: gc 0x7408c8006f50: deleting -1711462110.382616 [0] gc: gc 0x7408c807c290: deleting -1711462110.382626 [0] gc: gc 0x7408c8066850: deleting -1711462110.385423 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.385435 [0] recv: INFOTS(1711462110.385271352) -1711462110.385449 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #2) -1711462110.385465 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110aba1:7b19badd:ce0adb73:1c1}} -1711462110.385641 [0] recv: HDR(110aba1:7b19badd:ce0adb73 vendor 1.16) len 96 from udp/10.101.12.71:52025 -1711462110.385667 [0] recv: INFOTS(1711462110.385271352) -1711462110.385678 [0] recv: DATA(110aba1:7b19badd:ce0adb73:100c2 -> 0:0:0:0 #2) -1711462110.385653 [0] dq.builtin: SPDP ST3 110aba1:7b19badd:ce0adb73:1c1ddsi_delete_proxy_participant_by_guid(110aba1:7b19badd:ce0adb73:1c1) - deleting -1711462110.385700 [0] dq.builtin: => EVERYONE -1711462110.385737 [0] dq.builtin: delete_ppt(110aba1:7b19badd:ce0adb73:1c1) - deleting dependent proxy participants -1711462110.385753 [0] dq.builtin: delete_ppt(110aba1:7b19badd:ce0adb73:1c1) - deleting endpoints -1711462110.385763 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:301c4) - deleting -1711462110.385782 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:301c3) - deleting -1711462110.385793 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.385797 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:300c4) - deleting -1711462110.385830 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:300c3) - deleting -1711462110.385850 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:200c7) - deleting -1711462110.385868 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:200c2) - deleting -1711462110.385881 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:4c7) - deleting -1711462110.385898 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:4c2) - deleting -1711462110.385909 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:3c7) - deleting -1711462110.385925 [0] dq.builtin: ddsi_delete_proxy_writer (110aba1:7b19badd:ce0adb73:3c2) - deleting -1711462110.385933 [0] dq.builtin: ddsi_delete_proxy_reader (110aba1:7b19badd:ce0adb73:100c7) - deleting -1711462110.385939 [0] dq.builtin: delete -1711462110.385948 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110aba1:7b19badd:ce0adb73:1c1}} -1711462110.385955 [0] dq.builtin: SPDP ST3 110aba1:7b19badd:ce0adb73:1c1 unknown not allowed -1711462110.386873 [0] gc: gc 0x7408c8016150: deleting -1711462110.386885 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110aba1:7b19badd:ce0adb73:301c4) -1711462110.386910 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.386920 [0] gc: reduced nlocs=4 -1711462110.386931 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.386949 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.386958 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.386968 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.386978 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.386989 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.386999 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387010 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.387021 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387033 [0] gc: a b c -1711462110.387044 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.387057 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.387069 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.387081 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.387090 [0] gc: best = 3 -1711462110.387103 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.387115 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.387133 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.387145 [0] gc: reduced nlocs=4 -1711462110.387154 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387163 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.387173 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387196 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387206 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.387214 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387224 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387233 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.387245 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387255 [0] gc: a b c -1711462110.387268 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.387279 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.387301 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.387312 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.387321 [0] gc: best = 3 -1711462110.387333 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.387347 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.387366 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.387379 [0] gc: reduced nlocs=4 -1711462110.387389 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387398 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.387410 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387423 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387435 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.387444 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387456 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387464 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.387474 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387486 [0] gc: a b c -1711462110.387499 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.387511 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.387523 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.387535 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.387542 [0] gc: best = 3 -1711462110.387552 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.387562 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.387572 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=11 -1711462110.387587 [0] gc: gc 0x7408c80929f0: deleting -1711462110.387599 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c80929f0, 110aba1:7b19badd:ce0adb73:301c3) -1711462110.387612 [0] gc: gc 0x7408c80057c0: deleting -1711462110.387627 [0] gc: gc_delete_proxy_reader (0x7408c80057c0, 110aba1:7b19badd:ce0adb73:300c4) -1711462110.387618 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c80929f0, 110aba1:7b19badd:ce0adb73:301c3) -1711462110.387651 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.387679 [0] gc: reduced nlocs=4 -1711462110.387691 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387702 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.387713 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387721 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387739 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.387755 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387764 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387773 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.387784 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387795 [0] gc: a b c -1711462110.387807 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.387819 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.387831 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.387842 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.387849 [0] gc: best = 3 -1711462110.387863 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.387878 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.387897 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.387906 [0] gc: reduced nlocs=4 -1711462110.387918 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387932 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.387944 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387951 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387962 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.387971 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.387979 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.387989 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.387997 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388008 [0] gc: a b c -1711462110.388019 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.388028 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.388040 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.388053 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.388062 [0] gc: best = 3 -1711462110.388074 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.388085 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.388377 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.388392 [0] gc: reduced nlocs=4 -1711462110.388404 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388416 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.388424 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388431 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388439 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.388447 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388452 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388461 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.388467 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388476 [0] gc: a b c -1711462110.388484 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.388509 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.388526 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.388535 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.388540 [0] gc: best = 3 -1711462110.388549 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.388562 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.388571 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=10 -1711462110.388581 [0] gc: gc 0x7408c807d440: deleting -1711462110.388600 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c807d440, 110aba1:7b19badd:ce0adb73:300c3) -1711462110.388609 [0] gc: gc 0x7408c8007880: deleting -1711462110.388616 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c807d440, 110aba1:7b19badd:ce0adb73:300c3) -1711462110.388617 [0] gc: gc_delete_proxy_reader (0x7408c8007880, 110aba1:7b19badd:ce0adb73:200c7) -1711462110.388652 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.388661 [0] gc: reduced nlocs=4 -1711462110.388666 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388676 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.388686 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388694 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388703 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.388708 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388714 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388720 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.388726 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388733 [0] gc: a b c -1711462110.388744 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.388750 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.388756 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.388766 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.388772 [0] gc: best = 3 -1711462110.388777 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.388785 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.388799 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.388804 [0] gc: reduced nlocs=4 -1711462110.388809 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388815 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.388820 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388825 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388830 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.388836 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388840 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388845 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.388852 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388858 [0] gc: a b c -1711462110.388864 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.388875 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.388881 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.388887 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.388893 [0] gc: best = 3 -1711462110.388898 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.388905 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.388915 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.388920 [0] gc: reduced nlocs=4 -1711462110.388925 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388931 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.388935 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388941 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388946 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.388951 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388956 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.388963 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.388967 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.388974 [0] gc: a b c -1711462110.388980 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.388985 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.388991 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.388997 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.389001 [0] gc: best = 3 -1711462110.389007 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.389015 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.389019 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=9 -1711462110.389026 [0] gc: gc 0x7408c8035250: deleting -1711462110.389030 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8035250, 110aba1:7b19badd:ce0adb73:200c2) -1711462110.389037 [0] gc: gc 0x7408c8041ae0: deleting -1711462110.389041 [0] gc: gc_delete_proxy_reader (0x7408c8041ae0, 110aba1:7b19badd:ce0adb73:4c7) -1711462110.389043 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8035250, 110aba1:7b19badd:ce0adb73:200c2) -1711462110.389054 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.389070 [0] gc: reduced nlocs=4 -1711462110.389075 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389080 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.389086 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389090 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389097 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.389102 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389108 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389113 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.389120 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389125 [0] gc: a b c -1711462110.389131 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.389140 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.389148 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.389156 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.389160 [0] gc: best = 3 -1711462110.389166 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.389174 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.389185 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.389190 [0] gc: reduced nlocs=4 -1711462110.389195 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389202 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.389207 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389212 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389221 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.389228 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389233 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389239 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.389244 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389251 [0] gc: a b c -1711462110.389257 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.389263 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.389269 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.389275 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.389279 [0] gc: best = 3 -1711462110.389285 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.389293 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.389303 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.389307 [0] gc: reduced nlocs=4 -1711462110.389312 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389319 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.389325 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389329 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389335 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.389341 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389346 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389352 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.389356 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389361 [0] gc: a b c -1711462110.389367 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.389373 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.389378 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.389385 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.389390 [0] gc: best = 3 -1711462110.389395 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.389401 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.389411 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=8 -1711462110.389417 [0] gc: gc 0x7408c800b910: deleting -1711462110.389422 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c800b910, 110aba1:7b19badd:ce0adb73:4c2) -1711462110.389428 [0] gc: gc 0x7408c807c290: deleting -1711462110.389433 [0] gc: gc_delete_proxy_reader (0x7408c807c290, 110aba1:7b19badd:ce0adb73:3c7) -1711462110.389434 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c800b910, 110aba1:7b19badd:ce0adb73:4c2) -1711462110.389444 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.389462 [0] gc: reduced nlocs=4 -1711462110.389467 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389472 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.389477 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389482 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389487 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.389492 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389497 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389502 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.389506 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389512 [0] gc: a b c -1711462110.389517 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.389523 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.389529 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.389536 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.389541 [0] gc: best = 3 -1711462110.389546 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.389552 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.389563 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.389568 [0] gc: reduced nlocs=4 -1711462110.389572 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389577 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.389583 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389588 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389593 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.389599 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389603 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389608 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.389614 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389619 [0] gc: a b c -1711462110.389624 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.389631 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.389636 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.389643 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.389648 [0] gc: best = 3 -1711462110.389653 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.389660 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.389671 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 udp/10.101.12.71:7416@2 -1711462110.389680 [0] gc: reduced nlocs=4 -1711462110.389685 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389691 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.389696 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389703 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389709 [0] gc: rdidx 1 lidx udp/10.101.12.71:7416@2 2 -> 0 -1711462110.389714 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389718 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.389724 [0] gc: rdidx 2 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.389729 [0] gc: rdidx 2 lidx udp/239.255.0.1:7400@2 3 -> 8 -1711462110.389734 [0] gc: a b c -1711462110.389742 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. .. } -1711462110.389749 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. .. +u } -1711462110.389755 [0] gc: loc 2 = udp/10.101.12.71:7416@2 1 { .. +u .. } -1711462110.389762 [0] gc: loc 3 = udp/239.255.0.1:7400@2 0 { +1 +1 +1 } -1711462110.389768 [0] gc: best = 3 -1711462110.389774 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.389780 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.389787 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=7 -1711462110.389792 [0] gc: gc 0x7408c8050f20: deleting -1711462110.389797 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8050f20, 110aba1:7b19badd:ce0adb73:3c2) -1711462110.389803 [0] gc: gc 0x7408c801ddd0: deleting -1711462110.389808 [0] gc: gc_delete_proxy_reader (0x7408c801ddd0, 110aba1:7b19badd:ce0adb73:100c7) -1711462110.389815 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=6 -1711462110.389824 [0] gc: gc 0x7408c8065c00: deleting -1711462110.389831 [0] gc: gc_delete_proxy_participant(0x7408c8065c00, 110aba1:7b19badd:ce0adb73:1c1) -1711462110.389838 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=5 -1711462110.389846 [0] gc: gc 0x7408c80929f0: deleting -1711462110.389854 [0] gc: gc_delete_proxy_writer (0x7408c80929f0, 110aba1:7b19badd:ce0adb73:301c3) -1711462110.389810 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8050f20, 110aba1:7b19badd:ce0adb73:3c2) -1711462110.389863 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:301c4, 7): 5 -> 7 -1711462110.389876 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:301c4, 3): 1 -> 3 -1711462110.389885 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:301c4, 5): 3 -> 5 -1711462110.389892 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=4 -1711462110.389899 [0] gc: gc 0x7408c807d440: deleting -1711462110.389904 [0] gc: gc_delete_proxy_writer (0x7408c807d440, 110aba1:7b19badd:ce0adb73:300c3) -1711462110.389911 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:300c4, 7): 5 -> 7 -1711462110.389917 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:300c4, 3): 1 -> 3 -1711462110.389927 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:300c4, 5): 3 -> 5 -1711462110.389933 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=3 -1711462110.389941 [0] gc: gc 0x7408c8035250: deleting -1711462110.389946 [0] gc: gc_delete_proxy_writer (0x7408c8035250, 110aba1:7b19badd:ce0adb73:200c2) -1711462110.389953 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:200c7, 11): 7 -> 11 -1711462110.389963 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:200c7, 5): 1 -> 5 -1711462110.389970 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:200c7, 9): 5 -> 9 -1711462110.389975 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=2 -1711462110.389981 [0] gc: gc 0x7408c800b910: deleting -1711462110.389987 [0] gc: gc_delete_proxy_writer (0x7408c800b910, 110aba1:7b19badd:ce0adb73:4c2) -1711462110.389995 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:4c7, 13): 10 -> 13 -1711462110.390004 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:4c7, 5): 1 -> 5 -1711462110.390009 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:4c7, 8): 5 -> 8 -1711462110.390014 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=1 -1711462110.390020 [0] gc: gc 0x7408c8050f20: deleting -1711462110.390025 [0] gc: gc_delete_proxy_writer (0x7408c8050f20, 110aba1:7b19badd:ce0adb73:3c2) -1711462110.390031 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:3c7, 16): 12 -> 16 -1711462110.390037 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:3c7, 6): 1 -> 6 -1711462110.390044 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:3c7, 10): 6 -> 10 -1711462110.390049 [0] gc: ddsi_unref_proxy_participant(110aba1:7b19badd:ce0adb73:1c1): refc=0, freeing -1711462110.390055 [0] gc: lease_unregister(l 0x7408c80256c0 guid 110aba1:7b19badd:ce0adb73:1c1) -1711462110.390061 [0] gc: lease_free(l 0x7408c80256c0 guid 110aba1:7b19badd:ce0adb73:1c1) -1711462110.390066 [0] gc: lease_free(l 0x7408c800c690 guid 110aba1:7b19badd:ce0adb73:1c1) -1711462110.390077 [0] gc: ddsi_remove_deleted_participant_guid(110aba1:7b19badd:ce0adb73:1c1 for_what=3) -1711462110.390084 [0] gc: gc 0x7408c8006f50: deleting -1711462110.390089 [0] gc: gc 0x7408c8015c30: deleting -1711462110.391743 [0] recvMC: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.391784 [0] recvMC: HEARTBEAT(#30:22..22 L(:1c1 16958.051270)110e21c:d0762c48:a9f0fb28:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@22(sync) 110cd0d:e3b81b8d:1ccb65b1:504@22(sync) 110cd0d:79d6eeac:ea4a8907:504@22(sync)) -1711462110.391803 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110e21c:d0762c48:a9f0fb28:403: F#6:23/0: -1711462110.391818 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462110.391829 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.391840 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110e21c:d0762c48:a9f0fb28:403: F#20:23/0: -1711462110.391848 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462110.391857 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.391867 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110e21c:d0762c48:a9f0fb28:403: F#11:23/0: -1711462110.391874 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110e21c:d0762c48:a9f0fb28:403) -1711462110.391882 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.391919 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0097e8:44 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408c4011ac0:24 0x7408c4011ae8:28 [ udp/10.101.12.71:7413@2 ] -1711462110.391929 [0] tev: traffic-xmit (1) 168 -1711462110.392150 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.392166 [0] recv: INFOTS(1711462110.392032273) -1711462110.392181 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0 #20 L(:1c1 16958.051667)) -1711462110.392211 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:3c2 #20: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:403}} -1711462110.392222 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:403 ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:403) - deleting -1711462110.392228 [0] dq.builtin: => EVERYONE -1711462110.392250 [0] dq.builtin: delete -1711462110.392257 [0] gc: gc 0x7408c8039950: deleting -1711462110.392264 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8039950, 110e21c:d0762c48:a9f0fb28:403) -1711462110.392280 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c8039950, 110e21c:d0762c48:a9f0fb28:403) -1711462110.392296 [0] gc: gc 0x7408c8039950: deleting -1711462110.392305 [0] gc: gc_delete_proxy_writer (0x7408c8039950, 110e21c:d0762c48:a9f0fb28:403) -1711462110.392314 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:504, 21): 22 -> 21 -1711462110.392326 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:504, 7): 6 -> 7 -1711462110.392335 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:504, 12): 13 -> 12 -1711462110.392342 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=13 -1711462110.392349 [0] gc: gc 0x7408c8015c30: deleting -1711462110.392422 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.392434 [0] recv: INFOTS(1711462110.384844509) -1711462110.392460 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #2) -1711462110.392466 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110443d:bb7f10a5:18901533:1c1}} -1711462110.392487 [0] recv: HDR(110443d:bb7f10a5:18901533 vendor 1.16) len 96 from udp/10.101.12.71:44991 -1711462110.392506 [0] recv: INFOTS(1711462110.384844509) -1711462110.392488 [0] dq.builtin: SPDP ST3 110443d:bb7f10a5:18901533:1c1ddsi_delete_proxy_participant_by_guid(110443d:bb7f10a5:18901533:1c1) - deleting -1711462110.392531 [0] dq.builtin: => EVERYONE -1711462110.392538 [0] recv: DATA(110443d:bb7f10a5:18901533:100c2 -> 0:0:0:0 #2) -1711462110.392547 [0] dq.builtin: delete_ppt(110443d:bb7f10a5:18901533:1c1) - deleting dependent proxy participants -1711462110.392555 [0] dq.builtin: delete_ppt(110443d:bb7f10a5:18901533:1c1) - deleting endpoints -1711462110.392562 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:301c4) - deleting -1711462110.392574 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:301c3) - deleting -1711462110.392579 [0] gc: gc 0x7408c80929f0: not yet, shortsleep -1711462110.392581 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:300c4) - deleting -1711462110.392599 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:300c3) - deleting -1711462110.392611 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:200c7) - deleting -1711462110.392619 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:200c2) - deleting -1711462110.392626 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:4c7) - deleting -1711462110.392635 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:4c2) - deleting -1711462110.392643 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:3c7) - deleting -1711462110.392651 [0] dq.builtin: ddsi_delete_proxy_writer (110443d:bb7f10a5:18901533:3c2) - deleting -1711462110.392657 [0] dq.builtin: ddsi_delete_proxy_reader (110443d:bb7f10a5:18901533:100c7) - deleting -1711462110.392664 [0] dq.builtin: delete -1711462110.392673 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110443d:bb7f10a5:18901533:1c1}} -1711462110.392680 [0] dq.builtin: SPDP ST3 110443d:bb7f10a5:18901533:1c1 unknown not allowed -1711462110.393364 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.393383 [0] recv: INFOTS(1711462110.393273698) -1711462110.393393 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0 #16 L(:1c1 16958.052885)) -1711462110.393427 [0] dq.builtin: data(builtin, vendor 1.16): 110e21c:d0762c48:a9f0fb28:4c2 #16: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110e21c:d0762c48:a9f0fb28:504}} -1711462110.393444 [0] dq.builtin: SEDP ST3 110e21c:d0762c48:a9f0fb28:504 ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:504) => EVERYONE -1711462110.393456 [0] dq.builtin: - deleting -1711462110.393466 [0] dq.builtin: delete -1711462110.393643 [0] gc: gc 0x7408c80929f0: deleting -1711462110.393679 [0] gc: gc_delete_proxy_reader (0x7408c80929f0, 110443d:bb7f10a5:18901533:301c4) -1711462110.393660 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.393692 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.393700 [0] gc: reduced nlocs=3 -1711462110.393706 [0] recv: HEARTBEAT(#11:16..16 L(:1c1 16958.053198)110e21c:d0762c48:a9f0fb28:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@16(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@16(sync) 110cd0d:79d6eeac:ea4a8907:4c7@16(sync)) -1711462110.393707 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393724 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.393727 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#5:17/0: -1711462110.393730 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393745 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.393757 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.393758 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393779 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.393786 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393794 [0] gc: a b -1711462110.393801 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.393771 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#13:17/0: -1711462110.393808 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.393837 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.393843 [0] gc: best = 2 -1711462110.393851 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.393820 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.393858 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.393875 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.393881 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.393897 [0] gc: reduced nlocs=3 -1711462110.393892 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110e21c:d0762c48:a9f0fb28:4c2: F#8:17/0: -1711462110.393901 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393921 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.393928 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393936 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.393912 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.393942 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.393961 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.393975 [0] gc: a b -1711462110.393983 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.393990 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.393998 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394003 [0] gc: best = 2 -1711462110.393956 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.394009 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394032 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394044 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.394049 [0] gc: reduced nlocs=3 -1711462110.394056 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394065 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.394071 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394079 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394087 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394096 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394102 [0] gc: a b -1711462110.394110 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.394120 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.394127 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394136 [0] gc: best = 2 -1711462110.394142 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394151 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:301c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394159 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=11 -1711462110.394167 [0] gc: gc 0x7408c80057c0: deleting -1711462110.394173 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c80057c0, 110443d:bb7f10a5:18901533:301c3) -1711462110.394057 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0097e8:44 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408c4011ac0:24 0x7408c4011ae8:28 [ udp/10.101.12.71:7412@2 ] -1711462110.394184 [0] gc: gc 0x7408c8016150: deleting -1711462110.394190 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110443d:bb7f10a5:18901533:300c4) -1711462110.394202 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.394210 [0] gc: reduced nlocs=3 -1711462110.394223 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394228 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.394202 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c80057c0, 110443d:bb7f10a5:18901533:301c3) -1711462110.394235 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394250 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394257 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394266 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394273 [0] gc: a b -1711462110.394283 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.394290 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.394297 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394304 [0] gc: best = 2 -1711462110.394315 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394202 [0] tev: traffic-xmit (1) 168 -1711462110.394323 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394333 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.394338 [0] gc: reduced nlocs=3 -1711462110.394344 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394350 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.394355 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394363 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394368 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394373 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394378 [0] gc: a b -1711462110.394384 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.394391 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.394398 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394406 [0] gc: best = 2 -1711462110.394413 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394422 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394431 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.394435 [0] gc: reduced nlocs=3 -1711462110.394440 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394447 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.394452 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394457 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394462 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394467 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394472 [0] gc: a b -1711462110.394480 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.394486 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.394492 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394496 [0] gc: best = 2 -1711462110.394501 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394507 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:300c3): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394513 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=10 -1711462110.394519 [0] gc: gc 0x7408c8039950: deleting -1711462110.394528 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8039950, 110443d:bb7f10a5:18901533:300c3) -1711462110.394536 [0] gc: gc 0x7408c8007880: deleting -1711462110.394541 [0] gc: gc_delete_proxy_reader (0x7408c8007880, 110443d:bb7f10a5:18901533:200c7) -1711462110.394541 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8039950, 110443d:bb7f10a5:18901533:300c3) -1711462110.394553 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.394567 [0] gc: reduced nlocs=3 -1711462110.394576 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394581 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.394587 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394592 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394604 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394609 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394616 [0] gc: a b -1711462110.394625 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.394631 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.394640 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394645 [0] gc: best = 2 -1711462110.394654 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394661 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394670 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.394675 [0] gc: reduced nlocs=3 -1711462110.394687 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394692 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.394699 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394703 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394709 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394714 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394720 [0] gc: a b -1711462110.394726 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.394732 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.394738 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394743 [0] gc: best = 2 -1711462110.394748 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394754 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394766 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.394772 [0] gc: reduced nlocs=3 -1711462110.394777 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394783 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.394788 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394793 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394800 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394805 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394811 [0] gc: a b -1711462110.394817 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.394823 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.394829 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394836 [0] gc: best = 2 -1711462110.394843 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394849 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:200c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.394854 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=9 -1711462110.394861 [0] gc: gc 0x7408c8041ae0: deleting -1711462110.394867 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8041ae0, 110443d:bb7f10a5:18901533:200c2) -1711462110.394875 [0] gc: gc 0x7408c800b910: deleting -1711462110.394880 [0] gc: gc_delete_proxy_reader (0x7408c800b910, 110443d:bb7f10a5:18901533:4c7) -1711462110.394891 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.394902 [0] gc: reduced nlocs=3 -1711462110.394907 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394917 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.394924 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394930 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.394936 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.394943 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.394952 [0] gc: a b -1711462110.394892 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8041ae0, 110443d:bb7f10a5:18901533:200c2) -1711462110.394959 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.394974 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.394981 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.394985 [0] gc: best = 2 -1711462110.394991 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.394998 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.395009 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.395013 [0] gc: reduced nlocs=3 -1711462110.395017 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395022 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.395027 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395031 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395036 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.395041 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395046 [0] gc: a b -1711462110.395053 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.395066 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.395072 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.395077 [0] gc: best = 2 -1711462110.395081 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.395087 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.395096 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.395100 [0] gc: reduced nlocs=3 -1711462110.395105 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395110 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.395116 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395122 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395127 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.395132 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395138 [0] gc: a b -1711462110.395145 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.395153 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.395160 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.395164 [0] gc: best = 2 -1711462110.395169 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.395175 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:4c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.395187 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=8 -1711462110.395192 [0] gc: gc 0x7408c802f7c0: deleting -1711462110.395197 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c802f7c0, 110443d:bb7f10a5:18901533:4c2) -1711462110.395203 [0] gc: gc 0x7408c801ddd0: deleting -1711462110.395207 [0] gc: gc_delete_proxy_reader (0x7408c801ddd0, 110443d:bb7f10a5:18901533:3c7) -1711462110.395208 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c802f7c0, 110443d:bb7f10a5:18901533:4c2) -1711462110.395216 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.395231 [0] gc: reduced nlocs=3 -1711462110.395236 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395241 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.395245 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395249 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395254 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.395259 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395264 [0] gc: a b -1711462110.395269 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.395275 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.395282 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.395286 [0] gc: best = 2 -1711462110.395291 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.395297 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.395306 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.395311 [0] gc: reduced nlocs=3 -1711462110.395315 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395320 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.395325 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395329 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395334 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.395338 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395343 [0] gc: a b -1711462110.395348 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.395354 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.395360 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.395364 [0] gc: best = 2 -1711462110.395370 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.395376 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.395384 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7412@2 udp/10.101.12.71:7414@2 -1711462110.395388 [0] gc: reduced nlocs=3 -1711462110.395394 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395399 [0] gc: rdidx 0 lidx udp/10.101.12.71:7412@2 0 -> 0 -1711462110.395404 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395409 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395413 [0] gc: rdidx 1 lidx udp/10.101.12.71:7414@2 1 -> 0 -1711462110.395418 [0] gc: rdidx 1 lidx udp/239.255.0.1:7400@2 2 -> 8 -1711462110.395423 [0] gc: a b -1711462110.395428 [0] gc: loc 0 = udp/10.101.12.71:7412@2 1 { +u .. } -1711462110.395558 [0] gc: loc 1 = udp/10.101.12.71:7414@2 1 { .. +u } -1711462110.395565 [0] gc: loc 2 = udp/239.255.0.1:7400@2 1 { +1 +1 } -1711462110.395571 [0] gc: best = 2 -1711462110.395575 [0] gc: simple udp/239.255.0.1:7400@2 -1711462110.395581 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:3c2): udp/239.255.0.1:7400@2 (burst size 4294901760 rexmit 1048576) -1711462110.395585 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=7 -1711462110.395590 [0] gc: gc 0x7408c8065c00: deleting -1711462110.395594 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8065c00, 110443d:bb7f10a5:18901533:3c2) -1711462110.395600 [0] gc: gc 0x7408c8050f20: deleting -1711462110.395608 [0] gc: gc_delete_proxy_reader (0x7408c8050f20, 110443d:bb7f10a5:18901533:100c7) -1711462110.395612 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=6 -1711462110.395617 [0] gc: gc 0x7408c8040ad0: deleting -1711462110.395623 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 52 from udp/10.101.12.71:58212 -1711462110.395629 [0] gc: gc_delete_proxy_participant(0x7408c8040ad0, 110443d:bb7f10a5:18901533:1c1) -1711462110.395655 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=5 -1711462110.395661 [0] gc: gc 0x7408c803bcd0: deleting -1711462110.395663 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#5:21/0: -1711462110.395616 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8065c00, 110443d:bb7f10a5:18901533:3c2) -1711462110.395683 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.395666 [0] gc: gc_delete_proxy_reader (0x7408c803bcd0, 110e21c:d0762c48:a9f0fb28:504) -1711462110.395703 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.395649 [0] recv: HEARTBEAT(#11:20..20 L(:1c1 16958.055145)110e21c:d0762c48:a9f0fb28:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@20(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@20(sync) 110cd0d:79d6eeac:ea4a8907:3c7@20(sync)) -1711462110.395719 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#15:21/0: -1711462110.395721 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 -1711462110.395740 [0] gc: reduced nlocs=2 -1711462110.395749 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395755 [0] gc: rdidx 0 lidx udp/10.101.12.71:7415@2 0 -> 0 -1711462110.395762 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 1 -> 8 -1711462110.395768 [0] gc: a -1711462110.395774 [0] gc: loc 0 = udp/10.101.12.71:7415@2 1 { +u } -1711462110.395781 [0] gc: loc 1 = udp/239.255.0.1:7401@2 2 { +1 } -1711462110.395785 [0] gc: best = 0 -1711462110.395742 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.395790 [0] gc: simple udp/10.101.12.71:7415@2 -1711462110.395810 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:403): udp/10.101.12.71:7415@2 (burst size 4294901760 rexmit 1048576) -1711462110.395802 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.395819 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 -1711462110.395833 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110e21c:d0762c48:a9f0fb28:3c2: F#9:21/0: -1711462110.395845 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.395836 [0] gc: reduced nlocs=2 -1711462110.395854 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.395867 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395873 [0] gc: rdidx 0 lidx udp/10.101.12.71:7415@2 0 -> 0 -1711462110.395878 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 1 -> 8 -1711462110.395882 [0] gc: a -1711462110.395887 [0] gc: loc 0 = udp/10.101.12.71:7415@2 1 { +u } -1711462110.395892 [0] gc: loc 1 = udp/239.255.0.1:7401@2 2 { +1 } -1711462110.395895 [0] gc: best = 0 -1711462110.395899 [0] gc: simple udp/10.101.12.71:7415@2 -1711462110.395904 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0097e8:44 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408c4011ac0:24 0x7408c4011ae8:28 [ udp/10.101.12.71:7412@2 ] -1711462110.395917 [0] tev: traffic-xmit (1) 168 -1711462110.395905 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:403): udp/10.101.12.71:7415@2 (burst size 4294901760 rexmit 1048576) -1711462110.395938 [0] gc: setcover: all_addrs udp/239.255.0.1:7401@2 udp/10.101.12.71:7415@2 -1711462110.395942 [0] gc: reduced nlocs=2 -1711462110.395948 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.395953 [0] gc: rdidx 0 lidx udp/10.101.12.71:7415@2 0 -> 0 -1711462110.395959 [0] gc: rdidx 0 lidx udp/239.255.0.1:7401@2 1 -> 8 -1711462110.395965 [0] gc: a -1711462110.395970 [0] gc: loc 0 = udp/10.101.12.71:7415@2 1 { +u } -1711462110.395975 [0] gc: loc 1 = udp/239.255.0.1:7401@2 2 { +1 } -1711462110.395979 [0] gc: best = 0 -1711462110.395983 [0] gc: simple udp/10.101.12.71:7415@2 -1711462110.395989 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:403): udp/10.101.12.71:7415@2 (burst size 4294901760 rexmit 1048576) -1711462110.395993 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=12 -1711462110.395999 [0] gc: gc 0x7408c80057c0: deleting -1711462110.396003 [0] gc: gc_delete_proxy_writer (0x7408c80057c0, 110443d:bb7f10a5:18901533:301c3) -1711462110.396008 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:301c4, 7): 7 -> 7 -1711462110.396015 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:301c4, 3): 3 -> 3 -1711462110.396020 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:301c4, 5): 5 -> 5 -1711462110.396026 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=4 -1711462110.396031 [0] gc: gc 0x7408c8039950: deleting -1711462110.396035 [0] gc: gc_delete_proxy_writer (0x7408c8039950, 110443d:bb7f10a5:18901533:300c3) -1711462110.396040 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:300c4, 7): 7 -> 7 -1711462110.396047 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:300c4, 3): 3 -> 3 -1711462110.396055 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:300c4, 5): 5 -> 5 -1711462110.396062 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=3 -1711462110.396067 [0] gc: gc 0x7408c8041ae0: deleting -1711462110.396074 [0] gc: gc_delete_proxy_writer (0x7408c8041ae0, 110443d:bb7f10a5:18901533:200c2) -1711462110.396079 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:200c7, 11): 11 -> 11 -1711462110.396086 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:200c7, 6): 5 -> 6 -1711462110.396094 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:200c7, 9): 9 -> 9 -1711462110.396101 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=2 -1711462110.396111 [0] gc: gc 0x7408c802f7c0: deleting -1711462110.396116 [0] gc: gc_delete_proxy_writer (0x7408c802f7c0, 110443d:bb7f10a5:18901533:4c2) -1711462110.396122 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:4c7, 13): 13 -> 13 -1711462110.396129 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:4c7, 5): 5 -> 5 -1711462110.396138 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:4c7, 8): 8 -> 8 -1711462110.396144 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=1 -1711462110.396150 [0] gc: gc 0x7408c8065c00: deleting -1711462110.396155 [0] gc: gc_delete_proxy_writer (0x7408c8065c00, 110443d:bb7f10a5:18901533:3c2) -1711462110.396160 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:3c7, 16): 16 -> 16 -1711462110.396167 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:3c7, 6): 6 -> 6 -1711462110.396174 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:3c7, 10): 10 -> 10 -1711462110.396181 [0] gc: ddsi_unref_proxy_participant(110443d:bb7f10a5:18901533:1c1): refc=0, freeing -1711462110.396187 [0] gc: lease_unregister(l 0x7408c803d510 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.396192 [0] gc: lease_free(l 0x7408c803d510 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.396197 [0] gc: lease_free(l 0x7408c801cc80 guid 110443d:bb7f10a5:18901533:1c1) -1711462110.396205 [0] gc: ddsi_remove_deleted_participant_guid(110443d:bb7f10a5:18901533:1c1 for_what=3) -1711462110.396211 [0] gc: gc 0x7408c8015c30: deleting -1711462110.396216 [0] gc: gc 0x7408c80057c0: deleting -1711462110.396220 [0] gc: gc 0x7408c8006f50: deleting -1711462110.404300 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.404335 [0] recv: INFOTS(1711462110.397918357) -1711462110.404355 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #2) -1711462110.404373 [0] recv: HDR(110e21c:d0762c48:a9f0fb28 vendor 1.16) len 96 from udp/10.101.12.71:58212 -1711462110.404382 [0] recv: INFOTS(1711462110.397918357) -1711462110.404391 [0] recv: DATA(110e21c:d0762c48:a9f0fb28:100c2 -> 0:0:0:0 #2) -1711462110.404417 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110e21c:d0762c48:a9f0fb28:1c1}} -1711462110.404453 [0] dq.builtin: SPDP ST3 110e21c:d0762c48:a9f0fb28:1c1ddsi_delete_proxy_participant_by_guid(110e21c:d0762c48:a9f0fb28:1c1) - deleting -1711462110.404464 [0] dq.builtin: => EVERYONE -1711462110.404493 [0] dq.builtin: delete_ppt(110e21c:d0762c48:a9f0fb28:1c1) - deleting dependent proxy participants -1711462110.404517 [0] dq.builtin: delete_ppt(110e21c:d0762c48:a9f0fb28:1c1) - deleting endpoints -1711462110.404528 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:301c4) - deleting -1711462110.404555 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:301c3) - deleting -1711462110.404564 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:300c4) - deleting -1711462110.404569 [0] gc: gc 0x7408c80191a0: not yet, shortsleep -1711462110.404574 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:300c3) - deleting -1711462110.404600 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:200c7) - deleting -1711462110.404610 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:200c2) - deleting -1711462110.404617 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:4c7) - deleting -1711462110.404628 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:4c2) - deleting -1711462110.404635 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:3c7) - deleting -1711462110.404648 [0] dq.builtin: ddsi_delete_proxy_writer (110e21c:d0762c48:a9f0fb28:3c2) - deleting -1711462110.404656 [0] dq.builtin: ddsi_delete_proxy_reader (110e21c:d0762c48:a9f0fb28:100c7) - deleting -1711462110.404677 [0] dq.builtin: delete -1711462110.404688 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110e21c:d0762c48:a9f0fb28:1c1}} -1711462110.404694 [0] dq.builtin: SPDP ST3 110e21c:d0762c48:a9f0fb28:1c1 unknown not allowed -1711462110.405663 [0] gc: gc 0x7408c80191a0: deleting -1711462110.405686 [0] gc: gc_delete_proxy_reader (0x7408c80191a0, 110e21c:d0762c48:a9f0fb28:301c4) -1711462110.405706 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.405715 [0] gc: reduced nlocs=2 -1711462110.405723 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.405732 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.405738 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.405749 [0] gc: a -1711462110.405757 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.405780 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.405787 [0] gc: best = 0 -1711462110.405793 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.405803 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:301c3): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.405815 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.405820 [0] gc: reduced nlocs=2 -1711462110.405829 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.405834 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.405841 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.405848 [0] gc: a -1711462110.405854 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.405861 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.405866 [0] gc: best = 0 -1711462110.405870 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.405878 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:301c3): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.405887 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.405891 [0] gc: reduced nlocs=2 -1711462110.405897 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.405902 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.405908 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.405913 [0] gc: a -1711462110.405919 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.405926 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.405929 [0] gc: best = 0 -1711462110.405938 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.405944 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:301c3): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.405950 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=11 -1711462110.405960 [0] gc: gc 0x7408c8004d10: deleting -1711462110.405966 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8004d10, 110e21c:d0762c48:a9f0fb28:301c3) -1711462110.405976 [0] gc: gc 0x7408c8004260: deleting -1711462110.405982 [0] gc: gc_delete_proxy_reader (0x7408c8004260, 110e21c:d0762c48:a9f0fb28:300c4) -1711462110.405992 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.405996 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8004d10, 110e21c:d0762c48:a9f0fb28:301c3) -1711462110.405999 [0] gc: reduced nlocs=2 -1711462110.406033 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406039 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406045 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406050 [0] gc: a -1711462110.406058 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406065 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406069 [0] gc: best = 0 -1711462110.406075 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406082 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:300c3): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406093 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406098 [0] gc: reduced nlocs=2 -1711462110.406103 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406108 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406116 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406121 [0] gc: a -1711462110.406127 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406136 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406142 [0] gc: best = 0 -1711462110.406147 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406154 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:300c3): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406163 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406167 [0] gc: reduced nlocs=2 -1711462110.406177 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406182 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406190 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406196 [0] gc: a -1711462110.406203 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406210 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406217 [0] gc: best = 0 -1711462110.406223 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406230 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:300c3): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406243 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=10 -1711462110.406249 [0] gc: gc 0x7408c8072920: deleting -1711462110.406253 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8072920, 110e21c:d0762c48:a9f0fb28:300c3) -1711462110.406262 [0] gc: gc 0x7408c8039950: deleting -1711462110.406267 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8072920, 110e21c:d0762c48:a9f0fb28:300c3) -1711462110.406269 [0] gc: gc_delete_proxy_reader (0x7408c8039950, 110e21c:d0762c48:a9f0fb28:200c7) -1711462110.406290 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406295 [0] gc: reduced nlocs=2 -1711462110.406301 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406306 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406312 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406317 [0] gc: a -1711462110.406326 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406331 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406347 [0] gc: best = 0 -1711462110.406353 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406363 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:200c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406373 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406378 [0] gc: reduced nlocs=2 -1711462110.406385 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406390 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406394 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406399 [0] gc: a -1711462110.406404 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406412 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406417 [0] gc: best = 0 -1711462110.406422 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406428 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:200c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406436 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406441 [0] gc: reduced nlocs=2 -1711462110.406446 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406452 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406457 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406463 [0] gc: a -1711462110.406469 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406475 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406479 [0] gc: best = 0 -1711462110.406484 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406491 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:200c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406496 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=9 -1711462110.406502 [0] gc: gc 0x7408c8016150: deleting -1711462110.406506 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:200c2) -1711462110.406512 [0] gc: gc 0x7408c8057380: deleting -1711462110.406517 [0] gc: gc_delete_proxy_reader (0x7408c8057380, 110e21c:d0762c48:a9f0fb28:4c7) -1711462110.406518 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8016150, 110e21c:d0762c48:a9f0fb28:200c2) -1711462110.406527 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406544 [0] gc: reduced nlocs=2 -1711462110.406549 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406554 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406559 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406566 [0] gc: a -1711462110.406574 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406580 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406584 [0] gc: best = 0 -1711462110.406589 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406596 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:4c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406606 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406610 [0] gc: reduced nlocs=2 -1711462110.406614 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406620 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406627 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406636 [0] gc: a -1711462110.406642 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406649 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406655 [0] gc: best = 0 -1711462110.406661 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406667 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:4c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406682 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406687 [0] gc: reduced nlocs=2 -1711462110.406692 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406698 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406706 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406710 [0] gc: a -1711462110.406716 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406724 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406728 [0] gc: best = 0 -1711462110.406733 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406739 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:4c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406745 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=8 -1711462110.406751 [0] gc: gc 0x7408c80929f0: deleting -1711462110.406755 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c80929f0, 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.406761 [0] gc: gc 0x7408c8005280: deleting -1711462110.406767 [0] gc: gc_delete_proxy_reader (0x7408c8005280, 110e21c:d0762c48:a9f0fb28:3c7) -1711462110.406767 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c80929f0, 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.406781 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406786 [0] gc: reduced nlocs=2 -1711462110.406791 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406796 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406801 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406806 [0] gc: a -1711462110.406811 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406817 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406821 [0] gc: best = 0 -1711462110.406826 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406832 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:3c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406842 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406847 [0] gc: reduced nlocs=2 -1711462110.406852 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406857 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406865 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406869 [0] gc: a -1711462110.406876 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406882 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406886 [0] gc: best = 0 -1711462110.406891 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406898 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:3c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406912 [0] gc: setcover: all_addrs udp/239.255.0.1:7400@2 udp/10.101.12.71:7414@2 -1711462110.406916 [0] gc: reduced nlocs=2 -1711462110.406923 [0] gc: nloopback = 0, nlocs = 2, redundant_networking = 0 -1711462110.406928 [0] gc: rdidx 0 lidx udp/10.101.12.71:7414@2 0 -> 0 -1711462110.406934 [0] gc: rdidx 0 lidx udp/239.255.0.1:7400@2 1 -> 8 -1711462110.406938 [0] gc: a -1711462110.406944 [0] gc: loc 0 = udp/10.101.12.71:7414@2 1 { +u } -1711462110.406950 [0] gc: loc 1 = udp/239.255.0.1:7400@2 2 { +1 } -1711462110.406956 [0] gc: best = 0 -1711462110.406961 [0] gc: simple udp/10.101.12.71:7414@2 -1711462110.406967 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:3c2): udp/10.101.12.71:7414@2 (burst size 4294901760 rexmit 1048576) -1711462110.406973 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=7 -1711462110.406979 [0] gc: gc 0x7408c8057780: deleting -1711462110.406983 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8057780, 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.406989 [0] gc: gc 0x7408c8002660: deleting -1711462110.406994 [0] gc: gc_delete_proxy_reader (0x7408c8002660, 110e21c:d0762c48:a9f0fb28:100c7) -1711462110.407002 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=6 -1711462110.407009 [0] gc: gc 0x7408c801daa0: deleting -1711462110.407018 [0] gc: gc_delete_proxy_participant(0x7408c801daa0, 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.407024 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=5 -1711462110.407030 [0] gc: gc 0x7408c8004d10: deleting -1711462110.407036 [0] gc: gc_delete_proxy_writer (0x7408c8004d10, 110e21c:d0762c48:a9f0fb28:301c3) -1711462110.406995 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8057780, 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.407047 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:301c4, 7): 7 -> 7 -1711462110.407073 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:301c4, 3): 3 -> 3 -1711462110.407079 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:301c4, 5): 5 -> 5 -1711462110.407085 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=4 -1711462110.407090 [0] gc: gc 0x7408c8072920: deleting -1711462110.407094 [0] gc: gc_delete_proxy_writer (0x7408c8072920, 110e21c:d0762c48:a9f0fb28:300c3) -1711462110.407100 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:300c4, 7): 7 -> 7 -1711462110.407108 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:300c4, 3): 3 -> 3 -1711462110.407115 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:300c4, 5): 5 -> 5 -1711462110.407122 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=3 -1711462110.407127 [0] gc: gc 0x7408c8016150: deleting -1711462110.407132 [0] gc: gc_delete_proxy_writer (0x7408c8016150, 110e21c:d0762c48:a9f0fb28:200c2) -1711462110.407138 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:200c7, 10): 11 -> 10 -1711462110.407146 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:200c7, 5): 6 -> 5 -1711462110.407154 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:200c7, 9): 9 -> 9 -1711462110.407160 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=2 -1711462110.407165 [0] gc: gc 0x7408c80929f0: deleting -1711462110.407170 [0] gc: gc_delete_proxy_writer (0x7408c80929f0, 110e21c:d0762c48:a9f0fb28:4c2) -1711462110.407175 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:4c7, 14): 13 -> 14 -1711462110.407183 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:4c7, 6): 5 -> 6 -1711462110.407198 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:4c7, 9): 8 -> 9 -1711462110.407206 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=1 -1711462110.407211 [0] gc: gc 0x7408c8057780: deleting -1711462110.407215 [0] gc: gc_delete_proxy_writer (0x7408c8057780, 110e21c:d0762c48:a9f0fb28:3c2) -1711462110.407220 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:3c7, 16): 16 -> 16 -1711462110.407228 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:3c7, 6): 6 -> 6 -1711462110.407238 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:3c7, 10): 10 -> 10 -1711462110.407243 [0] gc: ddsi_unref_proxy_participant(110e21c:d0762c48:a9f0fb28:1c1): refc=0, freeing -1711462110.407248 [0] gc: lease_unregister(l 0x7408c8051660 guid 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.407253 [0] gc: lease_free(l 0x7408c8051660 guid 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.407257 [0] gc: lease_free(l 0x7408c801d3a0 guid 110e21c:d0762c48:a9f0fb28:1c1) -1711462110.407268 [0] gc: ddsi_remove_deleted_participant_guid(110e21c:d0762c48:a9f0fb28:1c1 for_what=3) -1711462110.407274 [0] gc: gc 0x7408c80057c0: deleting -1711462110.407278 [0] gc: gc 0x7408c8015c30: deleting -1711462110.413533 [0] recvUC: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.413594 [0] recvUC: HEARTBEAT(#100:90..90 L(:1c1 16958.073073)110d7b4:17c5b8c5:94bd0ff4:403 -> 0:0:0:0: 110cd0d:56a27910:57318171:504@90(sync) 110cd0d:e3b81b8d:1ccb65b1:504@90(sync) 110cd0d:79d6eeac:ea4a8907:504@90(sync)) -1711462110.413635 [0] tev: acknack 110cd0d:56a27910:57318171:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#7:91/0: -1711462110.413666 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.413678 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.413690 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#23:91/0: -1711462110.413698 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.413706 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.413717 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:504 -> 110d7b4:17c5b8c5:94bd0ff4:403: F#14:91/0: -1711462110.413724 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:504 -> pwr 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.413731 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.413794 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0097e8:44 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408c4011ac0:24 0x7408c4011ae8:28 [ udp/10.101.12.71:7415@2 ] -1711462110.413805 [0] tev: traffic-xmit (1) 168 -1711462110.414185 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.414208 [0] recv: INFOTS(1711462110.414026522) -1711462110.414228 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0 #90 L(:1c1 16958.073709)) -1711462110.414272 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:3c2 #90: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:403}} -1711462110.414304 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:403 ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:403) - deleting -1711462110.414317 [0] dq.builtin: => EVERYONE -1711462110.414368 [0] dq.builtin: delete -1711462110.414383 [0] gc: gc 0x7408c80929f0: deleting -1711462110.414400 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c80929f0, 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.414430 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.414442 [0] dq.user: gc_delete_proxy_writer_dqueue_bubble(0x7408c80929f0, 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.414468 [0] recv: INFOTS(1711462110.414306501) -1711462110.414485 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0 #80 L(:1c1 16958.073969)) -1711462110.414489 [0] gc: gc 0x7408c80929f0: deleting -1711462110.414506 [0] gc: gc_delete_proxy_writer (0x7408c80929f0, 110d7b4:17c5b8c5:94bd0ff4:403) -1711462110.414512 [0] dq.builtin: data(builtin, vendor 1.16): 110d7b4:17c5b8c5:94bd0ff4:4c2 #80: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110d7b4:17c5b8c5:94bd0ff4:504}} -1711462110.414522 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:504, 24): 22 -> 24 -1711462110.414528 [0] dq.builtin: SEDP ST3 110d7b4:17c5b8c5:94bd0ff4:504 ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:504) => EVERYONE -1711462110.414536 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:504, 8): 7 -> 8 -1711462110.414546 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:504, 15): 13 -> 15 -1711462110.414553 [0] dq.builtin: - deleting -1711462110.414554 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=13 -1711462110.414574 [0] dq.builtin: delete -1711462110.414588 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.414590 [0] gc: gc 0x7408c8016150: not yet, shortsleep -1711462110.414636 [0] recv: HEARTBEAT(#10:80..80 L(:1c1 16958.074122)110d7b4:17c5b8c5:94bd0ff4:4c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:4c7@80(sync) 110cd0d:e3b81b8d:1ccb65b1:4c7@80(sync) 110cd0d:79d6eeac:ea4a8907:4c7@80(sync)) -1711462110.414652 [0] tev: acknack 110cd0d:56a27910:57318171:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#5:81/0: -1711462110.414678 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.414691 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.414703 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#13:81/0: -1711462110.414711 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.414719 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.414728 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:4c7 -> 110d7b4:17c5b8c5:94bd0ff4:4c2: F#8:81/0: -1711462110.414736 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:4c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.414743 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.414793 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0097e8:44 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408c4011ac0:24 0x7408c4011ae8:28 [ udp/10.101.12.71:7414@2 ] -1711462110.414802 [0] tev: traffic-xmit (1) 168 -1711462110.415712 [0] gc: gc 0x7408c8016150: deleting -1711462110.415738 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:504) -1711462110.415750 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:403): (burst size 4294901760 rexmit 1048576) -1711462110.415759 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:403): (burst size 4294901760 rexmit 1048576) -1711462110.415767 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:403): (burst size 4294901760 rexmit 1048576) -1711462110.415774 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=12 -1711462110.415783 [0] gc: gc 0x7408c8015c30: deleting -1711462110.415789 [0] gc: gc 0x7408c8016150: deleting -1711462110.416047 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 52 from udp/10.101.12.71:40473 -1711462110.416081 [0] recv: HEARTBEAT(#11:90..90 L(:1c1 16958.075568)110d7b4:17c5b8c5:94bd0ff4:3c2 -> 0:0:0:0: 110cd0d:56a27910:57318171:3c7@90(sync) 110cd0d:e3b81b8d:1ccb65b1:3c7@90(sync) 110cd0d:79d6eeac:ea4a8907:3c7@90(sync)) -1711462110.416094 [0] tev: acknack 110cd0d:56a27910:57318171:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#6:91/0: -1711462110.416118 [0] tev: send acknack(rd 110cd0d:56a27910:57318171:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.416127 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009700 0(control): niov 0 sz 0 => now niov 2 sz 64 -1711462110.416137 [0] tev: acknack 110cd0d:e3b81b8d:1ccb65b1:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#16:91/0: -1711462110.416144 [0] tev: send acknack(rd 110cd0d:e3b81b8d:1ccb65b1:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.416152 [0] tev: xpack_addmsg 0x7408c0001390 0x7408bc009400 0(control): niov 2 sz 64 => now niov 4 sz 116 -1711462110.416161 [0] tev: acknack 110cd0d:79d6eeac:ea4a8907:3c7 -> 110d7b4:17c5b8c5:94bd0ff4:3c2: F#10:91/0: -1711462110.416169 [0] tev: send acknack(rd 110cd0d:79d6eeac:ea4a8907:3c7 -> pwr 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.416176 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c40119f0 0(control): niov 4 sz 116 => now niov 6 sz 168 -1711462110.416205 [0] tev: nn_xpack_send 168: 0x7408c000139c:20 0x7408bc0097e8:44 0x7408bc0094d0:24 0x7408bc0094f8:28 0x7408c4011ac0:24 0x7408c4011ae8:28 [ udp/10.101.12.71:7414@2 ] -1711462110.416214 [0] tev: traffic-xmit (1) 168 -1711462110.424208 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.424249 [0] recv: INFOTS(1711462110.418367566) -1711462110.424271 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #2) -1711462110.424293 [0] recv: HDR(110d7b4:17c5b8c5:94bd0ff4 vendor 1.16) len 96 from udp/10.101.12.71:40473 -1711462110.424302 [0] recv: INFOTS(1711462110.418367566) -1711462110.424312 [0] recv: DATA(110d7b4:17c5b8c5:94bd0ff4:100c2 -> 0:0:0:0 #2) -1711462110.424330 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1}} -1711462110.424359 [0] dq.builtin: SPDP ST3 110d7b4:17c5b8c5:94bd0ff4:1c1ddsi_delete_proxy_participant_by_guid(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting -1711462110.424368 [0] dq.builtin: => EVERYONE -1711462110.424397 [0] dq.builtin: delete_ppt(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting dependent proxy participants -1711462110.424405 [0] dq.builtin: delete_ppt(110d7b4:17c5b8c5:94bd0ff4:1c1) - deleting endpoints -1711462110.424413 [0] dq.builtin: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:301c4) - deleting -1711462110.424431 [0] dq.builtin: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:301c3) - deleting -1711462110.424439 [0] dq.builtin: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:300c4) - deleting -1711462110.424449 [0] dq.builtin: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:300c3) - deleting -1711462110.424456 [0] dq.builtin: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:200c7) - deleting -1711462110.424457 [0] gc: gc 0x7408c8072920: not yet, shortsleep -1711462110.424467 [0] dq.builtin: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:200c2) - deleting -1711462110.424569 [0] dq.builtin: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:4c7) - deleting -1711462110.424586 [0] dq.builtin: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:4c2) - deleting -1711462110.424593 [0] dq.builtin: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:3c7) - deleting -1711462110.424604 [0] dq.builtin: ddsi_delete_proxy_writer (110d7b4:17c5b8c5:94bd0ff4:3c2) - deleting -1711462110.424612 [0] dq.builtin: ddsi_delete_proxy_reader (110d7b4:17c5b8c5:94bd0ff4:100c7) - deleting -1711462110.424618 [0] dq.builtin: delete -1711462110.424628 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110d7b4:17c5b8c5:94bd0ff4:1c1}} -1711462110.424635 [0] dq.builtin: SPDP ST3 110d7b4:17c5b8c5:94bd0ff4:1c1 unknown not allowed -1711462110.425621 [0] gc: gc 0x7408c8072920: deleting -1711462110.425669 [0] gc: gc_delete_proxy_reader (0x7408c8072920, 110d7b4:17c5b8c5:94bd0ff4:301c4) -1711462110.425693 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:301c3): (burst size 4294901760 rexmit 1048576) -1711462110.425711 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:301c3): (burst size 4294901760 rexmit 1048576) -1711462110.425726 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:301c3): (burst size 4294901760 rexmit 1048576) -1711462110.425737 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=11 -1711462110.425751 [0] gc: gc 0x7408c8004d10: deleting -1711462110.425759 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8004d10, 110d7b4:17c5b8c5:94bd0ff4:301c3) -1711462110.425771 [0] gc: gc 0x7408c8057380: deleting -1711462110.425778 [0] gc: gc_delete_proxy_reader (0x7408c8057380, 110d7b4:17c5b8c5:94bd0ff4:300c4) -1711462110.425792 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:300c3): (burst size 4294901760 rexmit 1048576) -1711462110.425809 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:300c3): (burst size 4294901760 rexmit 1048576) -1711462110.425792 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8004d10, 110d7b4:17c5b8c5:94bd0ff4:301c3) -1711462110.425826 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:300c3): (burst size 4294901760 rexmit 1048576) -1711462110.425851 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=10 -1711462110.425867 [0] gc: gc 0x7408c8039950: deleting -1711462110.425875 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8039950, 110d7b4:17c5b8c5:94bd0ff4:300c3) -1711462110.425886 [0] gc: gc 0x7408c8004260: deleting -1711462110.425890 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8039950, 110d7b4:17c5b8c5:94bd0ff4:300c3) -1711462110.425894 [0] gc: gc_delete_proxy_reader (0x7408c8004260, 110d7b4:17c5b8c5:94bd0ff4:200c7) -1711462110.425935 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:200c2): (burst size 4294901760 rexmit 1048576) -1711462110.425949 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:200c2): (burst size 4294901760 rexmit 1048576) -1711462110.425962 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:200c2): (burst size 4294901760 rexmit 1048576) -1711462110.425972 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=9 -1711462110.425984 [0] gc: gc 0x7408c80191a0: deleting -1711462110.425991 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c80191a0, 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462110.426001 [0] gc: gc 0x7408c8016150: deleting -1711462110.426006 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c80191a0, 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462110.426008 [0] gc: gc_delete_proxy_reader (0x7408c8016150, 110d7b4:17c5b8c5:94bd0ff4:4c7) -1711462110.426035 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:4c2): (burst size 4294901760 rexmit 1048576) -1711462110.426049 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:4c2): (burst size 4294901760 rexmit 1048576) -1711462110.426062 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:4c2): (burst size 4294901760 rexmit 1048576) -1711462110.426073 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=8 -1711462110.426082 [0] gc: gc 0x7408c80929f0: deleting -1711462110.426090 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c80929f0, 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.426100 [0] gc: gc 0x7408c8005280: deleting -1711462110.426111 [0] gc: gc_delete_proxy_reader (0x7408c8005280, 110d7b4:17c5b8c5:94bd0ff4:3c7) -1711462110.426104 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c80929f0, 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.426124 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:e3b81b8d:1ccb65b1:3c2): (burst size 4294901760 rexmit 1048576) -1711462110.426165 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:56a27910:57318171:3c2): (burst size 4294901760 rexmit 1048576) -1711462110.426180 [0] gc: ddsi_rebuild_writer_addrset(110cd0d:79d6eeac:ea4a8907:3c2): (burst size 4294901760 rexmit 1048576) -1711462110.426190 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=7 -1711462110.426201 [0] gc: gc 0x7408c8002660: deleting -1711462110.426208 [0] gc: gc_delete_proxy_writer_dqueue(0x7408c8002660, 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.426217 [0] gc: gc 0x7408c801daa0: deleting -1711462110.426229 [0] gc: gc_delete_proxy_reader (0x7408c801daa0, 110d7b4:17c5b8c5:94bd0ff4:100c7) -1711462110.426240 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=6 -1711462110.426222 [0] dq.builtin: gc_delete_proxy_writer_dqueue_bubble(0x7408c8002660, 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.426254 [0] gc: gc 0x7408c8057780: deleting -1711462110.426262 [0] gc: gc_delete_proxy_participant(0x7408c8057780, 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.426270 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=5 -1711462110.426277 [0] gc: gc 0x7408c8004d10: deleting -1711462110.426284 [0] gc: gc_delete_proxy_writer (0x7408c8004d10, 110d7b4:17c5b8c5:94bd0ff4:301c3) -1711462110.426296 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:301c4, 6): 7 -> 6 -1711462110.426312 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:301c4, 2): 3 -> 2 -1711462110.426323 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:301c4, 4): 5 -> 4 -1711462110.426332 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=4 -1711462110.426343 [0] gc: gc 0x7408c8039950: deleting -1711462110.426349 [0] gc: gc_delete_proxy_writer (0x7408c8039950, 110d7b4:17c5b8c5:94bd0ff4:300c3) -1711462110.426361 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:300c4, 6): 7 -> 6 -1711462110.426375 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:300c4, 2): 3 -> 2 -1711462110.426389 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:300c4, 4): 5 -> 4 -1711462110.426401 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=3 -1711462110.426411 [0] gc: gc 0x7408c80191a0: deleting -1711462110.426418 [0] gc: gc_delete_proxy_writer (0x7408c80191a0, 110d7b4:17c5b8c5:94bd0ff4:200c2) -1711462110.426428 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:200c7, 10): 11 -> 10 -1711462110.426441 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:200c7, 5): 6 -> 5 -1711462110.426455 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:200c7, 8): 9 -> 8 -1711462110.426467 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=2 -1711462110.426478 [0] gc: gc 0x7408c80929f0: deleting -1711462110.426486 [0] gc: gc_delete_proxy_writer (0x7408c80929f0, 110d7b4:17c5b8c5:94bd0ff4:4c2) -1711462110.426497 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:4c7, 14): 14 -> 14 -1711462110.426510 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:4c7, 6): 6 -> 6 -1711462110.426524 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:4c7, 9): 9 -> 9 -1711462110.426536 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=1 -1711462110.426546 [0] gc: gc 0x7408c8002660: deleting -1711462110.426553 [0] gc: gc_delete_proxy_writer (0x7408c8002660, 110d7b4:17c5b8c5:94bd0ff4:3c2) -1711462110.426563 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:e3b81b8d:1ccb65b1:3c7, 17): 16 -> 17 -1711462110.426575 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:56a27910:57318171:3c7, 7): 6 -> 7 -1711462110.426597 [0] gc: ddsi_update_reader_init_acknack_count (110cd0d:79d6eeac:ea4a8907:3c7, 11): 10 -> 11 -1711462110.426609 [0] gc: ddsi_unref_proxy_participant(110d7b4:17c5b8c5:94bd0ff4:1c1): refc=0, freeing -1711462110.426619 [0] gc: lease_unregister(l 0x7408c80114b0 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.426629 [0] gc: lease_free(l 0x7408c80114b0 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.426636 [0] gc: lease_free(l 0x7408c800b500 guid 110d7b4:17c5b8c5:94bd0ff4:1c1) -1711462110.426654 [0] gc: ddsi_remove_deleted_participant_guid(110d7b4:17c5b8c5:94bd0ff4:1c1 for_what=3) -1711462110.426668 [0] gc: gc 0x7408c8015c30: deleting -1711462110.426675 [0] gc: gc 0x7408c80057c0: deleting -1711462110.555245 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,14,3,0,0,0,0,0,0 -1711462110.555300 [0] python3: => EVERYONE -1711462110.555337 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:1304) ... -1711462110.555346 [0] python3: => EVERYONE -1711462110.555467 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.555506 [0] gc: gc_delete_reader(0x6443028ed6f0, 110cd0d:e3b81b8d:1ccb65b1:1304) -1711462110.555543 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1304}} -1711462110.555570 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:1304 @ 0x7408d40864b4) user 21 builtin 12 -1711462110.555583 [0] gc: gc 0x7408c80057c0: deleting -1711462110.555583 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:e3b81b8d:1ccb65b1:1203) ... -1711462110.555593 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:1203) state transition 0 -> 1 -1711462110.555599 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:1203) ... -1711462110.555605 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:1203) - no unack'ed samples -1711462110.555609 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:1203) ... -1711462110.555614 [0] python3: => EVERYONE -1711462110.555628 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:1203) state transition 1 -> 3 -1711462110.555643 [0] gc: gc 0x6443028ece30: deleting -1711462110.555653 [0] gc: gc_delete_writer(0x6443028ece30, 110cd0d:e3b81b8d:1ccb65b1:1203) -1711462110.555673 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:3c2 #11: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1203}} -1711462110.555695 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:1203 @ 0x7408d40851d4) user 20 builtin 12 -1711462110.555704 [0] gc: gc 0x7408c80057c0: deleting -1711462110.555877 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,14,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.555923 [0] python3: => EVERYONE -1711462110.555950 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:1104) ... -1711462110.555958 [0] python3: => EVERYONE -1711462110.555994 [0] gc: gc 0x6443026b71b0: deleting -1711462110.556015 [0] gc: gc_delete_reader(0x6443026b71b0, 110cd0d:e3b81b8d:1ccb65b1:1104) -1711462110.556039 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1104}} -1711462110.556060 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:1104 @ 0x7408d4102744) user 19 builtin 12 -1711462110.556066 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:e3b81b8d:1ccb65b1:1003) ... -1711462110.556073 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:1003) state transition 0 -> 1 -1711462110.556077 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:1003) ... -1711462110.556082 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:1003) - no unack'ed samples -1711462110.556085 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:1003) ... -1711462110.556090 [0] python3: => EVERYONE -1711462110.556102 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:1003) state transition 1 -> 3 -1711462110.556175 [0] gc: gc 0x7408b8009e10: deleting -1711462110.556198 [0] gc: gc 0x6443028e6c00: deleting -1711462110.556206 [0] gc: gc_delete_writer(0x6443028e6c00, 110cd0d:e3b81b8d:1ccb65b1:1003) -1711462110.556234 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:3c2 #12: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1003}} -1711462110.556256 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:1003 @ 0x7408d41010c4) user 18 builtin 12 -1711462110.556267 [0] gc: gc 0x7408c80057c0: deleting -1711462110.556317 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,12,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.556323 [0] python3: => EVERYONE -1711462110.556335 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:f04) ... -1711462110.556348 [0] python3: => EVERYONE -1711462110.556420 [0] gc: gc 0x6443026b6d50: deleting -1711462110.556440 [0] gc: gc_delete_reader(0x6443026b6d50, 110cd0d:e3b81b8d:1ccb65b1:f04) -1711462110.556463 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:f04}} -1711462110.556484 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:f04 @ 0x7408d4069174) user 17 builtin 12 -1711462110.556487 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:e3b81b8d:1ccb65b1:e03) ... -1711462110.556515 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:e03) state transition 0 -> 1 -1711462110.556519 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:e03) ... -1711462110.556523 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:e03) - no unack'ed samples -1711462110.556527 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:e03) ... -1711462110.556531 [0] python3: => EVERYONE -1711462110.556535 [0] gc: gc 0x7408c80057c0: not yet, shortsleep -1711462110.556543 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:e03) state transition 1 -> 3 -1711462110.557648 [0] gc: gc 0x7408c80057c0: deleting -1711462110.557683 [0] gc: gc 0x6443028ee1e0: deleting -1711462110.557692 [0] gc: gc_delete_writer(0x6443028ee1e0, 110cd0d:e3b81b8d:1ccb65b1:e03) -1711462110.557721 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:e03}} -1711462110.557740 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:e03 @ 0x7408d4067f54) user 16 builtin 12 -1711462110.557746 [0] gc: gc 0x7408c80057c0: deleting -1711462110.557926 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,10,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.557959 [0] python3: => EVERYONE -1711462110.557979 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:d04) ... -1711462110.557990 [0] python3: => EVERYONE -1711462110.558015 [0] gc: gc 0x6443028b92b0: deleting -1711462110.558022 [0] gc: gc_delete_reader(0x6443028b92b0, 110cd0d:e3b81b8d:1ccb65b1:d04) -1711462110.558031 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:d04}} -1711462110.558041 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:d04 @ 0x7408d4063ba4) user 15 builtin 12 -1711462110.558047 [0] gc: gc 0x7408c80057c0: not yet, shortsleep -1711462110.558053 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:e3b81b8d:1ccb65b1:c03) ... -1711462110.558059 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:c03) state transition 0 -> 1 -1711462110.558064 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:c03) ... -1711462110.558069 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:c03) - no unack'ed samples -1711462110.558074 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:c03) ... -1711462110.558078 [0] python3: => EVERYONE -1711462110.558103 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:c03) state transition 1 -> 3 -1711462110.559121 [0] gc: gc 0x7408c80057c0: deleting -1711462110.559144 [0] gc: gc 0x6443028ae970: deleting -1711462110.559160 [0] gc: gc_delete_writer(0x6443028ae970, 110cd0d:e3b81b8d:1ccb65b1:c03) -1711462110.559172 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:c03}} -1711462110.559183 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:c03 @ 0x7408d40627f4) user 14 builtin 12 -1711462110.559188 [0] gc: gc 0x7408c80057c0: deleting -1711462110.559267 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,8,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.559274 [0] python3: => EVERYONE -1711462110.559288 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:b04) ... -1711462110.559295 [0] python3: => EVERYONE -1711462110.559317 [0] gc: gc 0x6443028ae970: deleting -1711462110.559322 [0] gc: gc_delete_reader(0x6443028ae970, 110cd0d:e3b81b8d:1ccb65b1:b04) -1711462110.559330 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #15: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:b04}} -1711462110.559338 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:b04 @ 0x7408d405ec44) user 13 builtin 12 -1711462110.559344 [0] gc: gc 0x7408c80057c0: deleting -1711462110.559347 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:e3b81b8d:1ccb65b1:a03) ... -1711462110.559353 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:a03) state transition 0 -> 1 -1711462110.559357 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:a03) ... -1711462110.559361 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:a03) - no unack'ed samples -1711462110.559365 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:a03) ... -1711462110.559369 [0] python3: => EVERYONE -1711462110.559382 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:a03) state transition 1 -> 3 -1711462110.559394 [0] gc: gc 0x6443028b92b0: deleting -1711462110.559400 [0] gc: gc_delete_writer(0x6443028b92b0, 110cd0d:e3b81b8d:1ccb65b1:a03) -1711462110.559410 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:a03}} -1711462110.559419 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:a03 @ 0x7408d405d934) user 12 builtin 12 -1711462110.559424 [0] gc: gc 0x7408c80057c0: deleting -1711462110.559458 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,7,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.559464 [0] python3: => EVERYONE -1711462110.559479 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:904) ... -1711462110.559493 [0] python3: => EVERYONE -1711462110.559513 [0] gc: gc 0x6443028ee1e0: deleting -1711462110.559519 [0] gc: gc_delete_reader(0x6443028ee1e0, 110cd0d:e3b81b8d:1ccb65b1:904) -1711462110.559528 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #16: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:904}} -1711462110.559538 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:904 @ 0x7408d409c394) user 11 builtin 12 -1711462110.559544 [0] gc: gc 0x7408c80057c0: deleting -1711462110.559548 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:e3b81b8d:1ccb65b1:803) ... -1711462110.559554 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:803) state transition 0 -> 1 -1711462110.559559 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:803) ... -1711462110.559563 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:803) - no unack'ed samples -1711462110.559568 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:803) ... -1711462110.559572 [0] python3: => EVERYONE -1711462110.559586 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:803) state transition 1 -> 3 -1711462110.559600 [0] gc: gc 0x6443026b6d50: deleting -1711462110.559607 [0] gc: gc_delete_writer(0x6443026b6d50, 110cd0d:e3b81b8d:1ccb65b1:803) -1711462110.559617 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:803}} -1711462110.559627 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:803 @ 0x7408d4070934) user 10 builtin 12 -1711462110.559656 [0] gc: gc 0x7408c80057c0: deleting -1711462110.559710 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.559718 [0] python3: => EVERYONE -1711462110.559729 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:e3b81b8d:1ccb65b1:703) ... -1711462110.559737 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:703) state transition 0 -> 1 -1711462110.559745 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:703) ... -1711462110.559750 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:703) - no unack'ed samples -1711462110.559754 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:703) ... -1711462110.559758 [0] python3: => EVERYONE -1711462110.559773 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:703) state transition 1 -> 3 -1711462110.559787 [0] gc: gc 0x6443028e6c00: deleting -1711462110.559793 [0] gc: gc_delete_writer(0x6443028e6c00, 110cd0d:e3b81b8d:1ccb65b1:703) -1711462110.559803 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:703}} -1711462110.559821 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:703 @ 0x7408d40310b4) user 9 builtin 12 -1711462110.559827 [0] gc: gc 0x7408c80057c0: deleting -1711462110.559869 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #21: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.559876 [0] python3: => EVERYONE -1711462110.559893 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:1404) ... -1711462110.559901 [0] python3: => EVERYONE -1711462110.559923 [0] gc: gc 0x6443028ece30: deleting -1711462110.559929 [0] gc: gc_delete_reader(0x6443028ece30, 110cd0d:e3b81b8d:1ccb65b1:1404) -1711462110.559937 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #17: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1404}} -1711462110.559947 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:1404 @ 0x7408d408eee4) user 8 builtin 12 -1711462110.559953 [0] gc: gc 0x7408c80057c0: not yet, shortsleep -1711462110.559987 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #22: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.559994 [0] python3: => EVERYONE -1711462110.560007 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:1504) ... -1711462110.560014 [0] python3: => EVERYONE -1711462110.561028 [0] gc: gc 0x7408c80057c0: deleting -1711462110.561051 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.561056 [0] gc: gc_delete_reader(0x6443028ed6f0, 110cd0d:e3b81b8d:1ccb65b1:1504) -1711462110.561065 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #18: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1504}} -1711462110.561078 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:1504 @ 0x7408d4091214) user 7 builtin 12 -1711462110.561084 [0] gc: gc 0x7408c80057c0: deleting -1711462110.561135 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #23: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{},{}}}} -1711462110.561142 [0] python3: => EVERYONE -1711462110.561157 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:1604) ... -1711462110.561164 [0] python3: => EVERYONE -1711462110.561193 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.561199 [0] gc: gc_delete_reader(0x6443028ed6f0, 110cd0d:e3b81b8d:1ccb65b1:1604) -1711462110.561208 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #19: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:1604}} -1711462110.561218 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:1604 @ 0x7408d4093214) user 6 builtin 12 -1711462110.561225 [0] gc: gc 0x7408c80057c0: deleting -1711462110.561244 [0] python3: write_sample 110cd0d:e3b81b8d:1ccb65b1:403 #24: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,227,184,27,141,28,203,101,177,0,0,1,193,0,0,0,0,0,0,0,0}}},{}} -1711462110.561252 [0] python3: => EVERYONE -1711462110.561445 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:e3b81b8d:1ccb65b1:403) ... -1711462110.561452 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:403) state transition 0 -> 1 -1711462110.561458 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:403) ... -1711462110.561463 [0] python3: delete_writer(guid 110cd0d:e3b81b8d:1ccb65b1:403) - no unack'ed samples -1711462110.561468 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:403) ... -1711462110.561473 [0] python3: => EVERYONE -1711462110.561488 [0] python3: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:403) state transition 1 -> 3 -1711462110.561505 [0] gc: gc 0x6443028ece30: deleting -1711462110.561513 [0] gc: gc_delete_writer(0x6443028ece30, 110cd0d:e3b81b8d:1ccb65b1:403) -1711462110.561536 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:403}} -1711462110.561569 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:403 @ 0x7408d4029104) user 5 builtin 12 -1711462110.561582 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:504) ... -1711462110.561590 [0] python3: => EVERYONE -1711462110.561612 [0] gc: gc 0x6443028e6c00: deleting -1711462110.561620 [0] gc: gc_delete_reader(0x6443028e6c00, 110cd0d:e3b81b8d:1ccb65b1:504) -1711462110.561634 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:4c2 #20: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:e3b81b8d:1ccb65b1:504}} -1711462110.561646 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:504 @ 0x7408d402c844) user 4 builtin 12 -1711462110.561665 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:207) ... -1711462110.561672 [0] python3: => EVERYONE -1711462110.561694 [0] gc: gc 0x6443026b6d50: deleting -1711462110.561701 [0] gc: gc_delete_reader(0x6443026b6d50, 110cd0d:e3b81b8d:1ccb65b1:207) -1711462110.561713 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:207 @ 0x7408d4021604) user 3 builtin 12 -1711462110.561728 [0] gc: gc 0x6443028ee1e0: not yet, shortsleep -1711462110.561743 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:307) ... -1711462110.561751 [0] python3: => EVERYONE -1711462110.562795 [0] gc: gc 0x6443028ee1e0: deleting -1711462110.562811 [0] gc: gc 0x6443028b92b0: deleting -1711462110.562819 [0] gc: gc 0x6443028ae970: deleting -1711462110.562825 [0] gc: gc_delete_reader(0x6443028ae970, 110cd0d:e3b81b8d:1ccb65b1:307) -1711462110.562845 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:307 @ 0x7408d4023224) user 2 builtin 12 -1711462110.562852 [0] gc: gc 0x7408c80057c0: deleting -1711462110.562891 [0] gc: gc 0x6443028ae970: deleting -1711462110.562909 [0] python3: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:107) ... -1711462110.562958 [0] python3: => EVERYONE -1711462110.562993 [0] gc: gc 0x6443028b92b0: deleting -1711462110.563002 [0] gc: gc_delete_reader(0x6443028b92b0, 110cd0d:e3b81b8d:1ccb65b1:107) -1711462110.563015 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:107 @ 0x7408d40202a4) user 1 builtin 12 -1711462110.563043 [0] python3: ddsi_delete_participant (110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.563058 [0] python3: => EVERYONE -1711462110.563045 [0] gc: gc 0x7408b8013060: deleting -1711462110.563085 [0] gc: gc 0x7408d40241d0: deleting -1711462110.563091 [0] gc: gc_delete_participant (0x7408d40241d0, 110cd0d:e3b81b8d:1ccb65b1:1c1) -1711462110.563100 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 0:0:0:0 @ (nil)) user 0 builtin 12 -1711462110.563122 [0] gc: write_sample 110cd0d:e3b81b8d:1ccb65b1:100c2 #2: ST3 DCPSParticipant/ParticipantBuiltinTopicData:{participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1}} -1711462110.563138 [0] gc: non-timed queue now has 1 items -1711462110.563148 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:100c2) ... -1711462110.563154 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:100c2) ... -1711462110.563165 [0] gc: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:100c2) state transition 0 -> 3 -1711462110.563167 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c4011b70 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.563173 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:3c2) ... -1711462110.563235 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:3c2) ... -1711462110.563243 [0] gc: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:3c2) state transition 0 -> 3 -1711462110.563257 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:4c2) ... -1711462110.563262 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:4c2) ... -1711462110.563270 [0] gc: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:4c2) state transition 0 -> 3 -1711462110.563276 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:200c2) ... -1711462110.563282 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:200c2) ... -1711462110.563289 [0] gc: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:200c2) state transition 0 -> 3 -1711462110.563296 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:100c7) ... -1711462110.563305 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:2c2) - unknown guid -1711462110.563311 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:2c7) - unknown guid -1711462110.563317 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:3c7) ... -1711462110.563324 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:4c7) ... -1711462110.563331 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:200c7) ... -1711462110.563340 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:300c3) ... -1711462110.563345 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:300c3) ... -1711462110.563353 [0] gc: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:300c3) state transition 0 -> 3 -1711462110.563359 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:301c3) ... -1711462110.563365 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:301c3) ... -1711462110.563373 [0] gc: writer_set_state(110cd0d:e3b81b8d:1ccb65b1:301c3) state transition 0 -> 3 -1711462110.563379 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:300c4) ... -1711462110.563388 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:301c4) ... -1711462110.563390 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 96 from udp/10.101.12.71:50807 -1711462110.563409 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:ff0003c2) - unknown guid -1711462110.563457 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:ff0003c7) - unknown guid -1711462110.563464 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:ff0004c2) - unknown guid -1711462110.563475 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:ff0004c7) - unknown guid -1711462110.563481 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:ff0200c2) - unknown guid -1711462110.563443 [0] recv: INFOTS(1711462110.563113561) -1711462110.563487 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:ff0200c7) - unknown guid -1711462110.563497 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:201c3) - unknown guid -1711462110.563503 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:201c4) - unknown guid -1711462110.563509 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:ff0101c2) - unknown guid -1711462110.563516 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:ff0101c7) - unknown guid -1711462110.563522 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:e3b81b8d:1ccb65b1:ff0202c3) - unknown guid -1711462110.563531 [0] gc: delete_reader_guid(guid 110cd0d:e3b81b8d:1ccb65b1:ff0202c4) - unknown guid -1711462110.563523 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #2) -1711462110.563539 [0] gc: gc 0x7408c80057c0: not yet, shortsleep -1711462110.563567 [0] recv: HDR(110cd0d:e3b81b8d:1ccb65b1 vendor 1.16) len 96 from udp/10.101.12.71:50807 -1711462110.563593 [0] recv: INFOTS(1711462110.563113561) -1711462110.563602 [0] recv: DATA(110cd0d:e3b81b8d:1ccb65b1:100c2 -> 0:0:0:0 #2) -1711462110.563627 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16 -1711462110.563681 [0] python3: => EVERYONE -1711462110.563685 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1}} -1711462110.563763 [0] dq.builtin: SPDP ST3 110cd0d:e3b81b8d:1ccb65b1:1c1 unknown not allowed -1711462110.563711 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:1304) ... -1711462110.563800 [0] python3: => EVERYONE -1711462110.563783 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110cd0d:e3b81b8d:1ccb65b1:1c1}} -1711462110.563830 [0] dq.builtin: SPDP ST3 110cd0d:e3b81b8d:1ccb65b1:1c1 unknown not allowed -1711462110.565040 [0] tev: nn_xpack_send 96: 0x7408c000139c:20 0x7408c4011c68:48 0x7408c80938f4:28 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.0.(trunc) -1711462110.565066 [0] tev: traffic-xmit (101) 96 -1711462110.565700 [0] gc: gc 0x7408c80057c0: deleting -1711462110.565726 [0] gc: gc_delete_writer(0x7408c80057c0, 110cd0d:e3b81b8d:1ccb65b1:100c2) -1711462110.565746 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:100c2 @ 0x7408d401a274) user 0 builtin 11 -1711462110.565754 [0] gc: gc 0x7408c8015c30: deleting -1711462110.565760 [0] gc: gc_delete_writer(0x7408c8015c30, 110cd0d:e3b81b8d:1ccb65b1:3c2) -1711462110.565772 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:3c2 @ 0x7408d401b2e4) user 0 builtin 10 -1711462110.565777 [0] gc: gc 0x7408c800ad10: deleting -1711462110.565783 [0] gc: gc_delete_writer(0x7408c800ad10, 110cd0d:e3b81b8d:1ccb65b1:4c2) -1711462110.565794 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:4c2 @ 0x7408d401aaa4) user 0 builtin 9 -1711462110.565800 [0] gc: gc 0x7408c80064a0: deleting -1711462110.565805 [0] gc: gc_delete_writer(0x7408c80064a0, 110cd0d:e3b81b8d:1ccb65b1:200c2) -1711462110.565817 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:200c2 @ 0x7408d401bb84) user 0 builtin 8 -1711462110.565822 [0] gc: gc 0x7408d4101f70: deleting -1711462110.565828 [0] gc: gc_delete_reader(0x7408d4101f70, 110cd0d:e3b81b8d:1ccb65b1:100c7) -1711462110.565835 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:100c7 @ 0x7408d401d284) user 0 builtin 7 -1711462110.565840 [0] gc: gc 0x7408d4101690: deleting -1711462110.565846 [0] gc: gc_delete_reader(0x7408d4101690, 110cd0d:e3b81b8d:1ccb65b1:3c7) -1711462110.565852 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:3c7 @ 0x7408d401d964) user 0 builtin 6 -1711462110.565857 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.565862 [0] gc: gc_delete_reader(0x7408d4017ad0, 110cd0d:e3b81b8d:1ccb65b1:4c7) -1711462110.565868 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:4c7 @ 0x7408d401d5e4) user 0 builtin 5 -1711462110.565874 [0] gc: gc 0x7408d0001690: deleting -1711462110.565879 [0] gc: gc_delete_reader(0x7408d0001690, 110cd0d:e3b81b8d:1ccb65b1:200c7) -1711462110.565885 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:200c7 @ 0x7408d401dce4) user 0 builtin 4 -1711462110.565892 [0] gc: gc 0x7408d00018c0: deleting -1711462110.565897 [0] gc: gc_delete_writer(0x7408d00018c0, 110cd0d:e3b81b8d:1ccb65b1:300c3) -1711462110.565907 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:300c3 @ 0x7408d401c434) user 0 builtin 3 -1711462110.565913 [0] gc: gc 0x7408d0001af0: deleting -1711462110.565918 [0] gc: gc_delete_writer(0x7408d0001af0, 110cd0d:e3b81b8d:1ccb65b1:301c3) -1711462110.565928 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:301c3 @ 0x7408d401ccd4) user 0 builtin 2 -1711462110.565933 [0] gc: gc 0x7408d0001d20: deleting -1711462110.565938 [0] gc: gc_delete_reader(0x7408d0001d20, 110cd0d:e3b81b8d:1ccb65b1:300c4) -1711462110.565945 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:300c4 @ 0x7408d401e054) user 0 builtin 1 -1711462110.565950 [0] gc: gc 0x7408d0001f50: deleting -1711462110.565955 [0] gc: gc_delete_reader(0x7408d0001f50, 110cd0d:e3b81b8d:1ccb65b1:301c4) -1711462110.565962 [0] gc: ddsi_unref_participant(110cd0d:e3b81b8d:1ccb65b1:1c1 @ 0x7408d4017230 <- 110cd0d:e3b81b8d:1ccb65b1:301c4 @ 0x7408d401e3d4) user 0 builtin 0 -1711462110.565972 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:e3b81b8d:1ccb65b1:1c1 for_what=1) -1711462110.565979 [0] gc: gc 0x7408d401ec10: deleting -1711462110.565984 [0] gc: gc_delete_reader(0x7408d401ec10, 110cd0d:79d6eeac:ea4a8907:1304) -1711462110.566003 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:1304}} -1711462110.566020 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:1304 @ 0x7408e00383b4) user 21 builtin 12 -1711462110.566026 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.566031 [0] gc: gc 0x7408d401ec10: deleting -1711462110.566078 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:79d6eeac:ea4a8907:1203) ... -1711462110.566112 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:1203) state transition 0 -> 1 -1711462110.566119 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:1203) ... -1711462110.566125 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:1203) - no unack'ed samples -1711462110.566131 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:1203) ... -1711462110.566140 [0] python3: => EVERYONE -1711462110.566161 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:1203) state transition 1 -> 3 -1711462110.566178 [0] gc: gc 0x7408d401f760: deleting -1711462110.566186 [0] gc: gc_delete_writer(0x7408d401f760, 110cd0d:79d6eeac:ea4a8907:1203) -1711462110.566199 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:3c2 #11: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:1203}} -1711462110.566215 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:1203 @ 0x7408e0037304) user 20 builtin 12 -1711462110.566255 [0] gc: gc 0x7408b8066520: deleting -1711462110.566351 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,14,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.566365 [0] python3: => EVERYONE -1711462110.566381 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:1104) ... -1711462110.566390 [0] python3: => EVERYONE -1711462110.566416 [0] gc: gc 0x6443028ee1e0: deleting -1711462110.566423 [0] gc: gc_delete_reader(0x6443028ee1e0, 110cd0d:79d6eeac:ea4a8907:1104) -1711462110.566433 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:1104}} -1711462110.566446 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:1104 @ 0x7408e0033f84) user 19 builtin 12 -1711462110.566453 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.566461 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:79d6eeac:ea4a8907:1003) ... -1711462110.566469 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:1003) state transition 0 -> 1 -1711462110.566474 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:1003) ... -1711462110.566479 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:1003) - no unack'ed samples -1711462110.566484 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:1003) ... -1711462110.566507 [0] python3: => EVERYONE -1711462110.566520 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:1003) state transition 1 -> 3 -1711462110.566537 [0] gc: gc 0x6443026b6d50: deleting -1711462110.566544 [0] gc: gc_delete_writer(0x6443026b6d50, 110cd0d:79d6eeac:ea4a8907:1003) -1711462110.566556 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:3c2 #12: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:1003}} -1711462110.566569 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:1003 @ 0x7408e00330e4) user 18 builtin 12 -1711462110.566577 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.566647 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,12,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.566656 [0] python3: => EVERYONE -1711462110.566671 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:f04) ... -1711462110.566679 [0] python3: => EVERYONE -1711462110.566700 [0] gc: gc 0x6443028e6c00: deleting -1711462110.566706 [0] gc: gc_delete_reader(0x6443028e6c00, 110cd0d:79d6eeac:ea4a8907:f04) -1711462110.566715 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:f04}} -1711462110.566725 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:f04 @ 0x7408e0023eb4) user 17 builtin 12 -1711462110.566735 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:79d6eeac:ea4a8907:e03) ... -1711462110.566742 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:e03) state transition 0 -> 1 -1711462110.566747 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:e03) ... -1711462110.566751 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:e03) - no unack'ed samples -1711462110.566756 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:e03) ... -1711462110.566761 [0] python3: => EVERYONE -1711462110.566771 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:e03) state transition 1 -> 3 -1711462110.566773 [0] gc: gc 0x7408b800bcd0: not yet, shortsleep -1711462110.567858 [0] gc: gc 0x7408b800bcd0: deleting -1711462110.567877 [0] gc: gc 0x6443028ece30: deleting -1711462110.567882 [0] gc: gc_delete_writer(0x6443028ece30, 110cd0d:79d6eeac:ea4a8907:e03) -1711462110.567892 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:e03}} -1711462110.567904 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:e03 @ 0x7408e002d4d4) user 16 builtin 12 -1711462110.567910 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.567992 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,10,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.568003 [0] python3: => EVERYONE -1711462110.568013 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:d04) ... -1711462110.568021 [0] python3: => EVERYONE -1711462110.568039 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.568045 [0] gc: gc_delete_reader(0x6443028ed6f0, 110cd0d:79d6eeac:ea4a8907:d04) -1711462110.568053 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:d04}} -1711462110.568063 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:d04 @ 0x7408e00234f4) user 15 builtin 12 -1711462110.568072 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:79d6eeac:ea4a8907:c03) ... -1711462110.568078 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:c03) state transition 0 -> 1 -1711462110.568083 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:c03) ... -1711462110.568087 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:c03) - no unack'ed samples -1711462110.568091 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:c03) ... -1711462110.568096 [0] python3: => EVERYONE -1711462110.568106 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:c03) state transition 1 -> 3 -1711462110.568125 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.568132 [0] gc: gc_delete_writer(0x6443028ed6f0, 110cd0d:79d6eeac:ea4a8907:c03) -1711462110.568142 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:c03}} -1711462110.568152 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:c03 @ 0x7408e0028fd4) user 14 builtin 12 -1711462110.568169 [0] gc: gc 0x7408a8009f20: deleting -1711462110.568194 [0] gc: gc 0x7408a8005ae0: not yet, shortsleep -1711462110.568216 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,8,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.568223 [0] python3: => EVERYONE -1711462110.568233 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:b04) ... -1711462110.568240 [0] python3: => EVERYONE -1711462110.569255 [0] gc: gc 0x7408a8005ae0: deleting -1711462110.569266 [0] gc: gc 0x6443028ece30: deleting -1711462110.569271 [0] gc: gc_delete_reader(0x6443028ece30, 110cd0d:79d6eeac:ea4a8907:b04) -1711462110.569279 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #15: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:b04}} -1711462110.569289 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:b04 @ 0x7408e00228c4) user 13 builtin 12 -1711462110.569294 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.569321 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:79d6eeac:ea4a8907:a03) ... -1711462110.569333 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:a03) state transition 0 -> 1 -1711462110.569337 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:a03) ... -1711462110.569341 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:a03) - no unack'ed samples -1711462110.569345 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:a03) ... -1711462110.569349 [0] python3: => EVERYONE -1711462110.569361 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:a03) state transition 1 -> 3 -1711462110.569374 [0] gc: gc 0x6443028e6c00: deleting -1711462110.569380 [0] gc: gc_delete_writer(0x6443028e6c00, 110cd0d:79d6eeac:ea4a8907:a03) -1711462110.569390 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:a03}} -1711462110.569400 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:a03 @ 0x7408e0026a64) user 12 builtin 12 -1711462110.569405 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.569439 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,7,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.569445 [0] python3: => EVERYONE -1711462110.569454 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:904) ... -1711462110.569460 [0] python3: => EVERYONE -1711462110.569539 [0] gc: gc 0x6443026b6d50: deleting -1711462110.569570 [0] gc: gc_delete_reader(0x6443026b6d50, 110cd0d:79d6eeac:ea4a8907:904) -1711462110.569603 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #16: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:904}} -1711462110.569632 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:904 @ 0x7408e001fb84) user 11 builtin 12 -1711462110.569634 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:79d6eeac:ea4a8907:803) ... -1711462110.569654 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:803) state transition 0 -> 1 -1711462110.569645 [0] gc: gc 0x7408d4017ad0: not yet, shortsleep -1711462110.569658 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:803) ... -1711462110.569673 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:803) - no unack'ed samples -1711462110.569679 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:803) ... -1711462110.569683 [0] python3: => EVERYONE -1711462110.569692 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:803) state transition 1 -> 3 -1711462110.570760 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.570777 [0] gc: gc 0x6443028ee1e0: deleting -1711462110.570784 [0] gc: gc_delete_writer(0x6443028ee1e0, 110cd0d:79d6eeac:ea4a8907:803) -1711462110.570806 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:803}} -1711462110.570829 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:803 @ 0x7408e0023674) user 10 builtin 12 -1711462110.570839 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.570896 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.570906 [0] python3: => EVERYONE -1711462110.570913 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:79d6eeac:ea4a8907:703) ... -1711462110.570920 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:703) state transition 0 -> 1 -1711462110.570926 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:703) ... -1711462110.570931 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:703) - no unack'ed samples -1711462110.570935 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:703) ... -1711462110.570939 [0] python3: => EVERYONE -1711462110.570951 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:703) state transition 1 -> 3 -1711462110.571030 [0] gc: gc 0x6443028b92b0: deleting -1711462110.571051 [0] gc: gc_delete_writer(0x6443028b92b0, 110cd0d:79d6eeac:ea4a8907:703) -1711462110.571077 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:703}} -1711462110.571102 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:703 @ 0x7408e00213b4) user 9 builtin 12 -1711462110.571114 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.571141 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #21: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.571147 [0] python3: => EVERYONE -1711462110.571156 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:1404) ... -1711462110.571163 [0] python3: => EVERYONE -1711462110.571223 [0] gc: gc 0x6443028ae970: deleting -1711462110.571243 [0] gc: gc_delete_reader(0x6443028ae970, 110cd0d:79d6eeac:ea4a8907:1404) -1711462110.571265 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #17: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:1404}} -1711462110.571286 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:1404 @ 0x7408e002bef4) user 8 builtin 12 -1711462110.571298 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.571316 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #22: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.571321 [0] python3: => EVERYONE -1711462110.571330 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:1504) ... -1711462110.571336 [0] python3: => EVERYONE -1711462110.571403 [0] gc: gc 0x6443028ae970: deleting -1711462110.571424 [0] gc: gc_delete_reader(0x6443028ae970, 110cd0d:79d6eeac:ea4a8907:1504) -1711462110.571445 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #18: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:1504}} -1711462110.571466 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:1504 @ 0x7408e003c5e4) user 7 builtin 12 -1711462110.571478 [0] gc: gc 0x7408d4017ad0: not yet, shortsleep -1711462110.571485 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #23: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{},{}}}} -1711462110.571491 [0] python3: => EVERYONE -1711462110.571499 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:1604) ... -1711462110.571511 [0] python3: => EVERYONE -1711462110.572635 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.572660 [0] gc: gc 0x6443028b92b0: deleting -1711462110.572669 [0] gc: gc_delete_reader(0x6443028b92b0, 110cd0d:79d6eeac:ea4a8907:1604) -1711462110.572689 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #19: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:1604}} -1711462110.572710 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:1604 @ 0x7408e003e774) user 6 builtin 12 -1711462110.572721 [0] gc: gc 0x7408d4017ad0: not yet, shortsleep -1711462110.572723 [0] python3: write_sample 110cd0d:79d6eeac:ea4a8907:403 #24: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,121,214,238,172,234,74,137,7,0,0,1,193,0,0,0,0,0,0,0,0}}},{}} -1711462110.572738 [0] python3: => EVERYONE -1711462110.572892 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:79d6eeac:ea4a8907:403) ... -1711462110.572898 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:403) state transition 0 -> 1 -1711462110.572901 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:403) ... -1711462110.572905 [0] python3: delete_writer(guid 110cd0d:79d6eeac:ea4a8907:403) - no unack'ed samples -1711462110.572909 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:403) ... -1711462110.572913 [0] python3: => EVERYONE -1711462110.572922 [0] python3: writer_set_state(110cd0d:79d6eeac:ea4a8907:403) state transition 1 -> 3 -1711462110.573841 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.573867 [0] gc: gc 0x6443028ee1e0: deleting -1711462110.573876 [0] gc: gc_delete_writer(0x6443028ee1e0, 110cd0d:79d6eeac:ea4a8907:403) -1711462110.573910 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:403}} -1711462110.573939 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:403 @ 0x7408e00190b4) user 5 builtin 12 -1711462110.573977 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:504) ... -1711462110.573983 [0] python3: => EVERYONE -1711462110.574047 [0] gc: gc 0x6443026b6d50: deleting -1711462110.574068 [0] gc: gc_delete_reader(0x6443026b6d50, 110cd0d:79d6eeac:ea4a8907:504) -1711462110.574092 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:4c2 #20: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:79d6eeac:ea4a8907:504}} -1711462110.574113 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:504 @ 0x7408e001abd4) user 4 builtin 12 -1711462110.574122 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:307) ... -1711462110.574127 [0] python3: => EVERYONE -1711462110.574154 [0] gc: gc 0x7408e0015030: deleting -1711462110.574164 [0] gc: gc_delete_reader(0x7408e0015030, 110cd0d:79d6eeac:ea4a8907:307) -1711462110.574180 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:307 @ 0x7408e0012054) user 3 builtin 12 -1711462110.574189 [0] gc: gc 0x6443028e6c00: deleting -1711462110.574190 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:107) ... -1711462110.574204 [0] python3: => EVERYONE -1711462110.574296 [0] gc: gc 0x6443028ece30: deleting -1711462110.574317 [0] gc: gc_delete_reader(0x6443028ece30, 110cd0d:79d6eeac:ea4a8907:107) -1711462110.574339 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:107 @ 0x7408e000b794) user 2 builtin 12 -1711462110.574342 [0] python3: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:207) ... -1711462110.574356 [0] python3: => EVERYONE -1711462110.574373 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.574396 [0] gc: gc_delete_reader(0x6443028ed6f0, 110cd0d:79d6eeac:ea4a8907:207) -1711462110.574412 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:207 @ 0x7408e000d524) user 1 builtin 12 -1711462110.574426 [0] gc: gc 0x6443028ed6f0: not yet, shortsleep -1711462110.574429 [0] python3: ddsi_delete_participant (110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.574443 [0] python3: => EVERYONE -1711462110.575526 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.575542 [0] gc: gc 0x6443028ece30: deleting -1711462110.575549 [0] gc: gc 0x6443028e6c00: deleting -1711462110.575555 [0] gc: gc 0x7408d4017ad0: deleting -1711462110.575563 [0] gc: gc 0x7408e000ac50: deleting -1711462110.575569 [0] gc: gc_delete_participant (0x7408e000ac50, 110cd0d:79d6eeac:ea4a8907:1c1) -1711462110.575576 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 0:0:0:0 @ (nil)) user 0 builtin 12 -1711462110.575597 [0] gc: write_sample 110cd0d:79d6eeac:ea4a8907:100c2 #2: ST3 DCPSParticipant/ParticipantBuiltinTopicData:{participant_guid={110cd0d:79d6eeac:ea4a8907:1c1}} -1711462110.575614 [0] gc: non-timed queue now has 1 items -1711462110.575623 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:100c2) ... -1711462110.575630 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:100c2) ... -1711462110.575641 [0] gc: writer_set_state(110cd0d:79d6eeac:ea4a8907:100c2) state transition 0 -> 3 -1711462110.575650 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:3c2) ... -1711462110.575657 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:3c2) ... -1711462110.575631 [0] tev: xpack_addmsg 0x7408c0001390 0x7408c400f8f0 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.575666 [0] gc: writer_set_state(110cd0d:79d6eeac:ea4a8907:3c2) state transition 0 -> 3 -1711462110.575696 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:4c2) ... -1711462110.575705 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:4c2) ... -1711462110.575715 [0] gc: writer_set_state(110cd0d:79d6eeac:ea4a8907:4c2) state transition 0 -> 3 -1711462110.575722 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:200c2) ... -1711462110.575728 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:200c2) ... -1711462110.575736 [0] gc: writer_set_state(110cd0d:79d6eeac:ea4a8907:200c2) state transition 0 -> 3 -1711462110.575743 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:100c7) ... -1711462110.575753 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:2c2) - unknown guid -1711462110.575760 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:2c7) - unknown guid -1711462110.575765 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:3c7) ... -1711462110.575764 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 96 from udp/10.101.12.71:50807 -1711462110.575776 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:4c7) ... -1711462110.575802 [0] recv: INFOTS(1711462110.575588337) -1711462110.575815 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #2) -1711462110.575819 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:200c7) ... -1711462110.575835 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:300c3) ... -1711462110.575842 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:300c3) ... -1711462110.575826 [0] recv: HDR(110cd0d:79d6eeac:ea4a8907 vendor 1.16) len 96 from udp/10.101.12.71:50807 -1711462110.575859 [0] recv: INFOTS(1711462110.575588337) -1711462110.575866 [0] recv: DATA(110cd0d:79d6eeac:ea4a8907:100c2 -> 0:0:0:0 #2) -1711462110.575851 [0] gc: writer_set_state(110cd0d:79d6eeac:ea4a8907:300c3) state transition 0 -> 3 -1711462110.575875 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110cd0d:79d6eeac:ea4a8907:1c1}} -1711462110.575895 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:301c3) ... -1711462110.575928 [0] dq.builtin: SPDP ST3 110cd0d:79d6eeac:ea4a8907:1c1 unknown not allowed -1711462110.575930 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:301c3) ... -1711462110.575940 [0] dq.builtin: data(builtin, vendor 1.16): 0:0:0:0 #2: ST3 /ParticipantBuiltinTopicData:{participant_guid={110cd0d:79d6eeac:ea4a8907:1c1}} -1711462110.575960 [0] dq.builtin: SPDP ST3 110cd0d:79d6eeac:ea4a8907:1c1 unknown not allowed -1711462110.575952 [0] gc: writer_set_state(110cd0d:79d6eeac:ea4a8907:301c3) state transition 0 -> 3 -1711462110.575975 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:300c4) ... -1711462110.575984 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:301c4) ... -1711462110.575993 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:ff0003c2) - unknown guid -1711462110.575999 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:ff0003c7) - unknown guid -1711462110.576005 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:ff0004c2) - unknown guid -1711462110.576010 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:ff0004c7) - unknown guid -1711462110.576016 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:ff0200c2) - unknown guid -1711462110.576022 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:ff0200c7) - unknown guid -1711462110.576027 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:201c3) - unknown guid -1711462110.576033 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:201c4) - unknown guid -1711462110.576039 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:ff0101c2) - unknown guid -1711462110.576045 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:ff0101c7) - unknown guid -1711462110.576050 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:79d6eeac:ea4a8907:ff0202c3) - unknown guid -1711462110.576056 [0] gc: delete_reader_guid(guid 110cd0d:79d6eeac:ea4a8907:ff0202c4) - unknown guid -1711462110.576062 [0] gc: gc 0x6443028ed6f0: not yet, shortsleep -1711462110.576772 [0] tev: nn_xpack_send 96: 0x7408c000139c:20 0x7408c400f9e8:48 0x7408c8058b84:28 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.0.(trunc) -1711462110.576810 [0] tev: traffic-xmit (101) 96 -1711462110.577149 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.577186 [0] gc: gc_delete_writer(0x6443028ed6f0, 110cd0d:79d6eeac:ea4a8907:100c2) -1711462110.577201 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:100c2 @ 0x7408e0005b94) user 0 builtin 11 -1711462110.577208 [0] gc: gc 0x7408d4101690: deleting -1711462110.577214 [0] gc: gc_delete_writer(0x7408d4101690, 110cd0d:79d6eeac:ea4a8907:3c2) -1711462110.577229 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:3c2 @ 0x7408e00068b4) user 0 builtin 10 -1711462110.577236 [0] gc: gc 0x7408d4101f70: deleting -1711462110.577241 [0] gc: gc_delete_writer(0x7408d4101f70, 110cd0d:79d6eeac:ea4a8907:4c2) -1711462110.577257 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:4c2 @ 0x7408e0006154) user 0 builtin 9 -1711462110.577263 [0] gc: gc 0x7408c80064a0: deleting -1711462110.577269 [0] gc: gc_delete_writer(0x7408c80064a0, 110cd0d:79d6eeac:ea4a8907:200c2) -1711462110.577284 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:200c2 @ 0x7408e0007104) user 0 builtin 8 -1711462110.577292 [0] gc: gc 0x7408c800ad10: deleting -1711462110.577297 [0] gc: gc_delete_reader(0x7408c800ad10, 110cd0d:79d6eeac:ea4a8907:100c7) -1711462110.577305 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:100c7 @ 0x7408e0008804) user 0 builtin 7 -1711462110.577311 [0] gc: gc 0x7408c8015c30: deleting -1711462110.577317 [0] gc: gc_delete_reader(0x7408c8015c30, 110cd0d:79d6eeac:ea4a8907:3c7) -1711462110.577324 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:3c7 @ 0x7408e0008ee4) user 0 builtin 6 -1711462110.577330 [0] gc: gc 0x7408c80057c0: deleting -1711462110.577336 [0] gc: gc_delete_reader(0x7408c80057c0, 110cd0d:79d6eeac:ea4a8907:4c7) -1711462110.577343 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:4c7 @ 0x7408e0008b64) user 0 builtin 5 -1711462110.577349 [0] gc: gc 0x7408d0001640: deleting -1711462110.577355 [0] gc: gc_delete_reader(0x7408d0001640, 110cd0d:79d6eeac:ea4a8907:200c7) -1711462110.577362 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:200c7 @ 0x7408e0009264) user 0 builtin 4 -1711462110.577368 [0] gc: gc 0x7408d0001870: deleting -1711462110.577373 [0] gc: gc_delete_writer(0x7408d0001870, 110cd0d:79d6eeac:ea4a8907:300c3) -1711462110.577384 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:300c3 @ 0x7408e00079b4) user 0 builtin 3 -1711462110.577391 [0] gc: gc 0x7408d0001aa0: deleting -1711462110.577397 [0] gc: gc_delete_writer(0x7408d0001aa0, 110cd0d:79d6eeac:ea4a8907:301c3) -1711462110.577409 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:301c3 @ 0x7408e0008254) user 0 builtin 2 -1711462110.577416 [0] gc: gc 0x7408d0001cd0: deleting -1711462110.577422 [0] gc: gc_delete_reader(0x7408d0001cd0, 110cd0d:79d6eeac:ea4a8907:300c4) -1711462110.577429 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:300c4 @ 0x7408e00095d4) user 0 builtin 1 -1711462110.577442 [0] gc: gc 0x7408d0001f00: deleting -1711462110.577448 [0] gc: gc_delete_reader(0x7408d0001f00, 110cd0d:79d6eeac:ea4a8907:301c4) -1711462110.577455 [0] gc: ddsi_unref_participant(110cd0d:79d6eeac:ea4a8907:1c1 @ 0x7408e0001f40 <- 110cd0d:79d6eeac:ea4a8907:301c4 @ 0x7408e0009954) user 0 builtin 0 -1711462110.577466 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:79d6eeac:ea4a8907:1c1 for_what=1) -1711462110.577473 [0] gc: gc 0x7408c80057c0: deleting -1711462110.596752 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #14: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,17,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,14,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162 -1711462110.596805 [0] python3: => EVERYONE -1711462110.596823 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:1304) ... -1711462110.596830 [0] python3: => EVERYONE -1711462110.597004 [0] gc: gc 0x6443028ae2e0: deleting -1711462110.597034 [0] gc: gc_delete_reader(0x6443028ae2e0, 110cd0d:56a27910:57318171:1304) -1711462110.597066 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #11: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:1304}} -1711462110.597090 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:1304 @ 0x7408ac0384d4) user 21 builtin 12 -1711462110.597101 [0] gc: gc 0x7408c80057c0: deleting -1711462110.597164 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:1203) ... -1711462110.597189 [0] python3: writer_set_state(110cd0d:56a27910:57318171:1203) state transition 0 -> 1 -1711462110.597195 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:1203) ... -1711462110.597199 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:1203) - no unack'ed samples -1711462110.597203 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:1203) ... -1711462110.597208 [0] python3: => EVERYONE -1711462110.597219 [0] python3: writer_set_state(110cd0d:56a27910:57318171:1203) state transition 1 -> 3 -1711462110.597304 [0] gc: gc 0x6443028e67a0: deleting -1711462110.597331 [0] gc: gc_delete_writer(0x6443028e67a0, 110cd0d:56a27910:57318171:1203) -1711462110.597368 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #11: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:1203}} -1711462110.597394 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:1203 @ 0x7408ac0375f4) user 20 builtin 12 -1711462110.597405 [0] gc: gc 0x7408c80057c0: deleting -1711462110.597524 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #15: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,15,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,12,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,14,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.597563 [0] python3: => EVERYONE -1711462110.597571 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:1104) ... -1711462110.597576 [0] python3: => EVERYONE -1711462110.597653 [0] gc: gc 0x7408e000a270: deleting -1711462110.597672 [0] gc: gc_delete_reader(0x7408e000a270, 110cd0d:56a27910:57318171:1104) -1711462110.597693 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #12: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:1104}} -1711462110.597714 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:1104 @ 0x7408ac034754) user 19 builtin 12 -1711462110.597724 [0] gc: gc 0x7408c80057c0: deleting -1711462110.597785 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:1003) ... -1711462110.597816 [0] python3: writer_set_state(110cd0d:56a27910:57318171:1003) state transition 0 -> 1 -1711462110.597825 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:1003) ... -1711462110.597831 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:1003) - no unack'ed samples -1711462110.597836 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:1003) ... -1711462110.597844 [0] python3: => EVERYONE -1711462110.597859 [0] python3: writer_set_state(110cd0d:56a27910:57318171:1003) state transition 1 -> 3 -1711462110.597932 [0] gc: gc 0x6443026b6d50: deleting -1711462110.597950 [0] gc: gc_delete_writer(0x6443026b6d50, 110cd0d:56a27910:57318171:1003) -1711462110.597975 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #12: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:1003}} -1711462110.597998 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:1003 @ 0x7408ac0337c4) user 18 builtin 12 -1711462110.598009 [0] gc: gc 0x7408c80057c0: deleting -1711462110.598062 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #16: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,13,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,10,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,12,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.598070 [0] python3: => EVERYONE -1711462110.598076 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:f04) ... -1711462110.598080 [0] python3: => EVERYONE -1711462110.598155 [0] gc: gc 0x6443028ee1e0: deleting -1711462110.598173 [0] gc: gc_delete_reader(0x6443028ee1e0, 110cd0d:56a27910:57318171:f04) -1711462110.598193 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #13: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:f04}} -1711462110.598225 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:f04 @ 0x7408ac02f8a4) user 17 builtin 12 -1711462110.598236 [0] gc: gc 0x7408c80057c0: deleting -1711462110.598240 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:e03) ... -1711462110.598247 [0] python3: writer_set_state(110cd0d:56a27910:57318171:e03) state transition 0 -> 1 -1711462110.598253 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:e03) ... -1711462110.598258 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:e03) - no unack'ed samples -1711462110.598263 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:e03) ... -1711462110.598269 [0] python3: => EVERYONE -1711462110.598278 [0] python3: writer_set_state(110cd0d:56a27910:57318171:e03) state transition 1 -> 3 -1711462110.598350 [0] gc: gc 0x6443028b92b0: deleting -1711462110.598369 [0] gc: gc_delete_writer(0x6443028b92b0, 110cd0d:56a27910:57318171:e03) -1711462110.598392 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #13: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:e03}} -1711462110.598414 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:e03 @ 0x7408ac02e9a4) user 16 builtin 12 -1711462110.598424 [0] gc: gc 0x7408c80057c0: deleting -1711462110.598469 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #17: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,11,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,8,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,10,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.598477 [0] python3: => EVERYONE -1711462110.598483 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:d04) ... -1711462110.598487 [0] python3: => EVERYONE -1711462110.598563 [0] gc: gc 0x6443028ae970: deleting -1711462110.598581 [0] gc: gc_delete_reader(0x6443028ae970, 110cd0d:56a27910:57318171:d04) -1711462110.598602 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #14: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:d04}} -1711462110.598621 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:d04 @ 0x7408ac023524) user 15 builtin 12 -1711462110.598628 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:c03) ... -1711462110.598631 [0] gc: gc 0x7408c80057c0: not yet, shortsleep -1711462110.598634 [0] python3: writer_set_state(110cd0d:56a27910:57318171:c03) state transition 0 -> 1 -1711462110.598648 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:c03) ... -1711462110.598652 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:c03) - no unack'ed samples -1711462110.598656 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:c03) ... -1711462110.598660 [0] python3: => EVERYONE -1711462110.598667 [0] python3: writer_set_state(110cd0d:56a27910:57318171:c03) state transition 1 -> 3 -1711462110.599754 [0] gc: gc 0x7408c80057c0: deleting -1711462110.599779 [0] gc: gc 0x6443028ae970: deleting -1711462110.599788 [0] gc: gc_delete_writer(0x6443028ae970, 110cd0d:56a27910:57318171:c03) -1711462110.599813 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #14: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:c03}} -1711462110.599849 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:c03 @ 0x7408ac02afb4) user 14 builtin 12 -1711462110.599860 [0] gc: gc 0x7408c80057c0: deleting -1711462110.599916 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #18: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,9,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,8,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.599927 [0] python3: => EVERYONE -1711462110.599933 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:b04) ... -1711462110.599938 [0] python3: => EVERYONE -1711462110.600013 [0] gc: gc 0x6443026b6d50: deleting -1711462110.600033 [0] gc: gc_delete_reader(0x6443026b6d50, 110cd0d:56a27910:57318171:b04) -1711462110.600055 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #15: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:b04}} -1711462110.600077 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:b04 @ 0x7408ac016d14) user 13 builtin 12 -1711462110.600083 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:a03) ... -1711462110.600088 [0] python3: writer_set_state(110cd0d:56a27910:57318171:a03) state transition 0 -> 1 -1711462110.600095 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:a03) ... -1711462110.600088 [0] gc: gc 0x7408c80057c0: not yet, shortsleep -1711462110.600101 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:a03) - no unack'ed samples -1711462110.600120 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:a03) ... -1711462110.600125 [0] python3: => EVERYONE -1711462110.600134 [0] python3: writer_set_state(110cd0d:56a27910:57318171:a03) state transition 1 -> 3 -1711462110.601180 [0] gc: gc 0x7408c80057c0: deleting -1711462110.601206 [0] gc: gc 0x6443028ae2e0: deleting -1711462110.601213 [0] gc: gc_delete_writer(0x6443028ae2e0, 110cd0d:56a27910:57318171:a03) -1711462110.601240 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #15: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:a03}} -1711462110.601262 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:a03 @ 0x7408ac028124) user 12 builtin 12 -1711462110.601272 [0] gc: gc 0x7408c80057c0: deleting -1711462110.601313 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #19: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,7,3,0,0,0,0,0,0,0,0}}}}}}} -1711462110.601323 [0] python3: => EVERYONE -1711462110.601332 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:904) ... -1711462110.601336 [0] python3: => EVERYONE -1711462110.601353 [0] gc: gc 0x6443028ee870: deleting -1711462110.601363 [0] gc: gc_delete_reader(0x6443028ee870, 110cd0d:56a27910:57318171:904) -1711462110.601378 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #16: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:904}} -1711462110.601408 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:904 @ 0x7408ac01c794) user 11 builtin 12 -1711462110.601418 [0] gc: gc 0x7408c80057c0: deleting -1711462110.601420 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:803) ... -1711462110.601440 [0] python3: writer_set_state(110cd0d:56a27910:57318171:803) state transition 0 -> 1 -1711462110.601446 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:803) ... -1711462110.601451 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:803) - no unack'ed samples -1711462110.601457 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:803) ... -1711462110.601463 [0] python3: => EVERYONE -1711462110.601472 [0] python3: writer_set_state(110cd0d:56a27910:57318171:803) state transition 1 -> 3 -1711462110.601488 [0] gc: gc 0x6443028fcff0: deleting -1711462110.601497 [0] gc: gc_delete_writer(0x6443028fcff0, 110cd0d:56a27910:57318171:803) -1711462110.601512 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #16: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:803}} -1711462110.601532 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:803 @ 0x7408ac023254) user 10 builtin 12 -1711462110.601541 [0] gc: gc 0x7408c80057c0: deleting -1711462110.601598 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #20: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,20,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.601604 [0] python3: => EVERYONE -1711462110.601609 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:703) ... -1711462110.601613 [0] python3: writer_set_state(110cd0d:56a27910:57318171:703) state transition 0 -> 1 -1711462110.601617 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:703) ... -1711462110.601620 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:703) - no unack'ed samples -1711462110.601624 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:703) ... -1711462110.601628 [0] python3: => EVERYONE -1711462110.601635 [0] python3: writer_set_state(110cd0d:56a27910:57318171:703) state transition 1 -> 3 -1711462110.601646 [0] gc: gc 0x64430289d600: deleting -1711462110.601655 [0] gc: gc_delete_writer(0x64430289d600, 110cd0d:56a27910:57318171:703) -1711462110.601670 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #17: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:703}} -1711462110.601688 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:703 @ 0x7408ac023a64) user 9 builtin 12 -1711462110.601697 [0] gc: gc 0x7408c80057c0: deleting -1711462110.601807 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #21: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,21,4,0,0,0,0,0,0,0,0}}}{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.601813 [0] python3: => EVERYONE -1711462110.601819 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:1404) ... -1711462110.601823 [0] python3: => EVERYONE -1711462110.601839 [0] gc: gc 0x6443028e6c00: deleting -1711462110.601847 [0] gc: gc_delete_reader(0x6443028e6c00, 110cd0d:56a27910:57318171:1404) -1711462110.601860 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #17: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:1404}} -1711462110.601882 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:1404 @ 0x7408ac03aef4) user 8 builtin 12 -1711462110.601892 [0] gc: gc 0x7408c80057c0: deleting -1711462110.601907 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #22: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,22,4,0,0,0,0,0,0,0,0}}}},{}}}} -1711462110.601913 [0] python3: => EVERYONE -1711462110.601919 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:1504) ... -1711462110.601922 [0] python3: => EVERYONE -1711462110.601938 [0] gc: gc 0x6443028e6c00: deleting -1711462110.601946 [0] gc: gc_delete_reader(0x6443028e6c00, 110cd0d:56a27910:57318171:1504) -1711462110.601958 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #18: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:1504}} -1711462110.601972 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:1504 @ 0x7408ac03cb04) user 7 builtin 12 -1711462110.601981 [0] gc: gc 0x7408c80057c0: deleting -1711462110.601993 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #23: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{{"/","test_node",{},{}}}} -1711462110.601999 [0] python3: => EVERYONE -1711462110.602004 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:1604) ... -1711462110.602008 [0] python3: => EVERYONE -1711462110.602021 [0] gc: gc 0x64430289d600: deleting -1711462110.602030 [0] gc: gc_delete_reader(0x64430289d600, 110cd0d:56a27910:57318171:1604) -1711462110.602043 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #19: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:1604}} -1711462110.602057 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:1604 @ 0x7408ac03eb04) user 6 builtin 12 -1711462110.602066 [0] gc: gc 0x7408c80057c0: deleting -1711462110.602074 [0] python3: write_sample 110cd0d:56a27910:57318171:403 #24: ST0 ros_discovery_info/rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_:{{{{1,16,205,13,86,162,121,16,87,49,129,113,0,0,1,193,0,0,0,0,0,0,0,0}}},{}} -1711462110.602079 [0] python3: => EVERYONE -1711462110.602188 [0] python3: ddsi_unblock_throttled_writer(guid 110cd0d:56a27910:57318171:403) ... -1711462110.602193 [0] python3: writer_set_state(110cd0d:56a27910:57318171:403) state transition 0 -> 1 -1711462110.602198 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:403) ... -1711462110.602204 [0] python3: delete_writer(guid 110cd0d:56a27910:57318171:403) - no unack'ed samples -1711462110.602209 [0] python3: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:403) ... -1711462110.602215 [0] python3: => EVERYONE -1711462110.602223 [0] python3: writer_set_state(110cd0d:56a27910:57318171:403) state transition 1 -> 3 -1711462110.602240 [0] gc: gc 0x6443028fcff0: deleting -1711462110.602252 [0] gc: gc_delete_writer(0x6443028fcff0, 110cd0d:56a27910:57318171:403) -1711462110.602268 [0] gc: write_sample 110cd0d:56a27910:57318171:3c2 #18: ST3 DCPSPublication/PublicationBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:403}} -1711462110.602289 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:403 @ 0x7408ac01d5a4) user 5 builtin 12 -1711462110.602332 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:504) ... -1711462110.602337 [0] python3: => EVERYONE -1711462110.602361 [0] gc: gc 0x6443028ee870: deleting -1711462110.602371 [0] gc: gc_delete_reader(0x6443028ee870, 110cd0d:56a27910:57318171:504) -1711462110.602384 [0] gc: write_sample 110cd0d:56a27910:57318171:4c2 #20: ST3 DCPSSubscription/SubscriptionBuiltinTopicData:{endpoint_guid={110cd0d:56a27910:57318171:504}} -1711462110.602400 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:504 @ 0x7408ac01f034) user 4 builtin 12 -1711462110.602417 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:307) ... -1711462110.602422 [0] python3: => EVERYONE -1711462110.602436 [0] gc: gc 0x7408ac019790: deleting -1711462110.602445 [0] gc: gc_delete_reader(0x7408ac019790, 110cd0d:56a27910:57318171:307) -1711462110.602460 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:307 @ 0x7408ac014d24) user 3 builtin 12 -1711462110.602467 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:207) ... -1711462110.602474 [0] python3: => EVERYONE -1711462110.602468 [0] gc: gc 0x7408ac005130: deleting -1711462110.602489 [0] gc: gc 0x6443028ae2e0: deleting -1711462110.602495 [0] gc: gc_delete_reader(0x6443028ae2e0, 110cd0d:56a27910:57318171:207) -1711462110.602508 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:207 @ 0x7408ac00d0b4) user 2 builtin 12 -1711462110.602515 [0] python3: delete_reader_guid(guid 110cd0d:56a27910:57318171:107) ... -1711462110.602523 [0] python3: => EVERYONE -1711462110.602517 [0] gc: gc 0x6443026b6d50: deleting -1711462110.602537 [0] gc: gc 0x6443028ae970: deleting -1711462110.602544 [0] gc: gc 0x7408c80057c0: deleting -1711462110.602552 [0] gc: gc 0x6443028ae2e0: deleting -1711462110.602558 [0] gc: gc_delete_reader(0x6443028ae2e0, 110cd0d:56a27910:57318171:107) -1711462110.602571 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:107 @ 0x7408ac00aeb4) user 1 builtin 12 -1711462110.602576 [0] python3: ddsi_delete_participant (110cd0d:56a27910:57318171:1c1) -1711462110.602581 [0] gc: gc 0x6443026b6d50: not yet, shortsleep -1711462110.602583 [0] python3: => EVERYONE -1711462110.602610 [0] python3: trigger_recv_threads: 0 many 0x7408d4016670 -1711462110.602625 [0] python3: trigger_recv_threads: 1 single udp/239.255.0.1:7401 -1711462110.602649 [0] recv: done -1711462110.602685 [0] recvMC: done -1711462110.602699 [0] python3: trigger_recv_threads: 2 single udp/10.101.12.71:7411 -1711462110.602769 [0] recvUC: done -1711462110.603654 [0] gc: gc 0x6443026b6d50: deleting -1711462110.603674 [0] gc: gc 0x7408ac00a3e0: deleting -1711462110.603681 [0] gc: gc_delete_participant (0x7408ac00a3e0, 110cd0d:56a27910:57318171:1c1) -1711462110.603689 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 0:0:0:0 @ (nil)) user 0 builtin 12 -1711462110.603709 [0] gc: write_sample 110cd0d:56a27910:57318171:100c2 #2: ST3 DCPSParticipant/ParticipantBuiltinTopicData:{participant_guid={110cd0d:56a27910:57318171:1c1}} -1711462110.603724 [0] gc: non-timed queue now has 1 items -1711462110.603732 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:100c2) ... -1711462110.603738 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:100c2) ... -1711462110.603747 [0] gc: writer_set_state(110cd0d:56a27910:57318171:100c2) state transition 0 -> 3 -1711462110.603754 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:3c2) ... -1711462110.603759 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:3c2) ... -1711462110.603766 [0] gc: writer_set_state(110cd0d:56a27910:57318171:3c2) state transition 0 -> 3 -1711462110.603773 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:4c2) ... -1711462110.603788 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:4c2) ... -1711462110.603795 [0] gc: writer_set_state(110cd0d:56a27910:57318171:4c2) state transition 0 -> 3 -1711462110.603803 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:200c2) ... -1711462110.603808 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:200c2) ... -1711462110.603815 [0] gc: writer_set_state(110cd0d:56a27910:57318171:200c2) state transition 0 -> 3 -1711462110.603821 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:100c7) ... -1711462110.603829 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:2c2) - unknown guid -1711462110.603836 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:2c7) - unknown guid -1711462110.603842 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:3c7) ... -1711462110.603850 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:4c7) ... -1711462110.603857 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:200c7) ... -1711462110.603866 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:300c3) ... -1711462110.603872 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:300c3) ... -1711462110.603878 [0] gc: writer_set_state(110cd0d:56a27910:57318171:300c3) state transition 0 -> 3 -1711462110.603885 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:301c3) ... -1711462110.603891 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:301c3) ... -1711462110.603897 [0] gc: writer_set_state(110cd0d:56a27910:57318171:301c3) state transition 0 -> 3 -1711462110.603904 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:300c4) ... -1711462110.603911 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:301c4) ... -1711462110.603918 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:ff0003c2) - unknown guid -1711462110.603924 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:ff0003c7) - unknown guid -1711462110.603930 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:ff0004c2) - unknown guid -1711462110.603936 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:ff0004c7) - unknown guid -1711462110.603941 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:ff0200c2) - unknown guid -1711462110.603948 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:ff0200c7) - unknown guid -1711462110.603953 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:201c3) - unknown guid -1711462110.603959 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:201c4) - unknown guid -1711462110.603964 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:ff0101c2) - unknown guid -1711462110.603970 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:ff0101c7) - unknown guid -1711462110.603975 [0] gc: ddsi_delete_writer_nolinger(guid 110cd0d:56a27910:57318171:ff0202c3) - unknown guid -1711462110.603980 [0] gc: delete_reader_guid(guid 110cd0d:56a27910:57318171:ff0202c4) - unknown guid -1711462110.603986 [0] gc: gc 0x6443026b6d50: deleting -1711462110.603991 [0] gc: gc_delete_writer(0x6443026b6d50, 110cd0d:56a27910:57318171:100c2) -1711462110.604003 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:100c2 @ 0x7408ac005564) user 0 builtin 11 -1711462110.604010 [0] gc: gc 0x7408c8015c30: deleting -1711462110.604016 [0] gc: gc_delete_writer(0x7408c8015c30, 110cd0d:56a27910:57318171:3c2) -1711462110.604026 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:3c2 @ 0x7408ac0063a4) user 0 builtin 10 -1711462110.604033 [0] gc: gc 0x7408c800ad10: deleting -1711462110.604039 [0] gc: gc_delete_writer(0x7408c800ad10, 110cd0d:56a27910:57318171:4c2) -1711462110.604054 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:4c2 @ 0x7408ac005bb4) user 0 builtin 9 -1711462110.604061 [0] gc: gc 0x7408c80064a0: deleting -1711462110.604068 [0] gc: gc_delete_writer(0x7408c80064a0, 110cd0d:56a27910:57318171:200c2) -1711462110.604081 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:200c2 @ 0x7408ac006bf4) user 0 builtin 8 -1711462110.604091 [0] gc: gc 0x7408d4101f70: deleting -1711462110.604096 [0] gc: gc_delete_reader(0x7408d4101f70, 110cd0d:56a27910:57318171:100c7) -1711462110.604104 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:100c7 @ 0x7408ac001534) user 0 builtin 7 -1711462110.604109 [0] gc: gc 0x7408d4101690: deleting -1711462110.604114 [0] gc: gc_delete_reader(0x7408d4101690, 110cd0d:56a27910:57318171:3c7) -1711462110.604121 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:3c7 @ 0x7408ac008884) user 0 builtin 6 -1711462110.604127 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.604132 [0] gc: gc_delete_reader(0x6443028ed6f0, 110cd0d:56a27910:57318171:4c7) -1711462110.604139 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:4c7 @ 0x7408ac008504) user 0 builtin 5 -1711462110.604144 [0] gc: gc 0x7408d0001640: deleting -1711462110.604149 [0] gc: gc_delete_reader(0x7408d0001640, 110cd0d:56a27910:57318171:200c7) -1711462110.604156 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:200c7 @ 0x7408ac008c04) user 0 builtin 4 -1711462110.604162 [0] gc: gc 0x7408d0001870: deleting -1711462110.604167 [0] gc: gc_delete_writer(0x7408d0001870, 110cd0d:56a27910:57318171:300c3) -1711462110.604176 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:300c3 @ 0x7408ac0074a4) user 0 builtin 3 -1711462110.604182 [0] gc: gc 0x7408d0001aa0: deleting -1711462110.604188 [0] gc: gc_delete_writer(0x7408d0001aa0, 110cd0d:56a27910:57318171:301c3) -1711462110.604213 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:301c3 @ 0x7408ac007d44) user 0 builtin 2 -1711462110.604220 [0] gc: gc 0x7408d0001cd0: deleting -1711462110.604225 [0] gc: gc_delete_reader(0x7408d0001cd0, 110cd0d:56a27910:57318171:300c4) -1711462110.604232 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:300c4 @ 0x7408ac008f74) user 0 builtin 1 -1711462110.604237 [0] gc: gc 0x7408d0001f00: deleting -1711462110.604243 [0] gc: gc_delete_reader(0x7408d0001f00, 110cd0d:56a27910:57318171:301c4) -1711462110.604250 [0] gc: ddsi_unref_participant(110cd0d:56a27910:57318171:1c1 @ 0x7408ac001d20 <- 110cd0d:56a27910:57318171:301c4 @ 0x7408ac0092f4) user 0 builtin 0 -1711462110.604262 [0] gc: ddsi_remove_deleted_participant_guid(110cd0d:56a27910:57318171:1c1 for_what=1) -1711462110.604269 [0] gc: gc 0x6443028ed6f0: deleting -1711462110.604289 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:100c2) ... -1711462110.604312 [0] python3: writer_set_state(0:0:0:100c2) state transition 0 -> 3 -1711462110.604323 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:2c2) ... -1711462110.604329 [0] python3: writer_set_state(0:0:0:2c2) state transition 0 -> 3 -1711462110.604335 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:3c2) ... -1711462110.604329 [0] gc: gc 0x7408c40185f0: not yet, shortsleep -1711462110.604339 [0] python3: writer_set_state(0:0:0:3c2) state transition 0 -> 3 -1711462110.604355 [0] python3: ddsi_delete_writer_nolinger(guid 0:0:0:4c2) ... -1711462110.604359 [0] python3: writer_set_state(0:0:0:4c2) state transition 0 -> 3 -1711462110.605406 [0] gc: gc 0x7408c40185f0: deleting -1711462110.605417 [0] gc: gc_delete_writer(0x7408c40185f0, 0:0:0:100c2) -1711462110.605432 [0] gc: gc 0x6443028ee870: deleting -1711462110.605438 [0] gc: gc_delete_writer(0x6443028ee870, 0:0:0:2c2) -1711462110.605447 [0] gc: gc 0x6443028fcff0: deleting -1711462110.605453 [0] gc: gc_delete_writer(0x6443028fcff0, 0:0:0:3c2) -1711462110.605462 [0] gc: gc 0x64430289d600: deleting -1711462110.605467 [0] gc: gc_delete_writer(0x64430289d600, 0:0:0:4c2) -1711462110.605507 [0] gc: gc 0x6443028e6c00: deleting -1711462110.605736 [0] python3: xpack_addmsg 0x6443027daf60 0x7408c400fa70 0(data(0:0:0:0:#0/1)): niov 0 sz 0 => now niov 3 sz 96 -1711462110.606455 [0] python3: nn_xpack_send 96: 0x6443027daf6c:20 0x7408c400fb68:48 0x7408c8059604:28 [ udp/239.255.0.1:7400@2 udp/127.0.0.1:7410@2 udp/127.0.0.1:7412@2 udp/127.0.0.1:7414@2 udp/127.0.0.1:7416@2 udp/127.0.0.1:7418@2 udp/127.0.0.1:7420@2 udp/127.0.0.1:7422@2 udp/127.0.0.1:7424@2 udp/127.0.0.1:7426@2 udp/127.0.0.1:7428@2 udp/127.0.0.1:7430@2 udp/127.0.0.1:7432@2 udp/127.0.0.1:7434@2 udp/127.0.0.1:7436@2 udp/127.0.0.1:7438@2 udp/127.0.0.1:7440@2 udp/127.0.0.1:7442@2 udp/127.0.0.1:7444@2 udp/127.0.0.1:7446@2 udp/127.0.0.1:7448@2 udp/127.0.0.1:7450@2 udp/127.0.0.1:7452@2 udp/127.0.0.1:7454@2 udp/127.0.0.1:7456@2 udp/127.0.0.1:7458@2 udp/127.0.0.1:7460@2 udp/127.0.0.1:7462@2 udp/127.0.0.1:7464@2 udp/127.0.0.1:7466@2 udp/127.0.0.1:7468@2 udp/127.0.0.1:7470@2 udp/127.0.0.1:7472@2 udp/127.0.0.1:7474@2 udp/127.0.0.1:7476@2 udp/127.0.0.1:7478@2 udp/127.0.0.1:7480@2 udp/127.0.0.1:7482@2 udp/127.0.0.1:7484@2 udp/127.0.0.1:7486@2 udp/127.0.0.1:7488@2 udp/127.0.0.1:7490@2 udp/127.0.0.1:7492@2 udp/127.0.0.1:7494@2 udp/127.0.0.1:7496@2 udp/127.0.0.1:7498@2 udp/127.0.0.1:7500@2 udp/127.0.0.1:7502@2 udp/127.0.0.1:7504@2 udp/127.0.0.1:7506@2 udp/127.0.0.1:7508@2 udp/127.0.0.1:7510@2 udp/127.0.0.1:7512@2 udp/127.0.0.1:7514@2 udp/127.0.0.1:7516@2 udp/127.0.0.1:7518@2 udp/127.0.0.1:7520@2 udp/127.0.0.1:7522@2 udp/127.0.0.1:7524@2 udp/127.0.0.1:7526@2 udp/127.0.0.1:7528@2 udp/127.0.0.1:7530@2 udp/127.0.0.1:7532@2 udp/127.0.0.1:7534@2 udp/127.0.0.1:7536@2 udp/127.0.0.1:7538@2 udp/127.0.0.1:7540@2 udp/127.0.0.1:7542@2 udp/127.0.0.1:7544@2 udp/127.0.0.1:7546@2 udp/127.0.0.1:7548@2 udp/127.0.0.1:7550@2 udp/127.0.0.1:7552@2 udp/127.0.0.1:7554@2 udp/127.0.0.1:7556@2 udp/127.0.0.1:7558@2 udp/127.0.0.1:7560@2 udp/127.0.0.1:7562@2 udp/127.0.0.1:7564@2 udp/127.0.0.1:7566@2 udp/127.0.0.1:7568@2 udp/127.0.0.1:7570@2 udp/127.0.0.1:7572@2 udp/127.0.0.1:7574@2 udp/127.0.0.1:7576@2 udp/127.0.0.1:7578@2 udp/127.0.0.1:7580@2 udp/127.0.0.1:7582@2 udp/127.0.0.1:7584@2 udp/127.0.0.1:7586@2 udp/127.0.0.1:7588@2 udp/127.0.(trunc) -1711462110.606464 [0] python3: traffic-xmit (101) 96 -1711462110.606478 [0] python3: leave conn 0x7408d400ab50 for (udp/239.255.0.1, *) interface (default): not leaving yet -1711462110.606483 [0] python3: leave conn 0x7408d40088c0 for (udp/239.255.0.1, *) interface (default): not leaving yet -1711462110.606490 [0] python3: leave conn 0x7408d400ab50 for (udp/239.255.0.1, *) interface udp/10.101.12.71 -1711462110.606500 [0] python3: leave conn 0x7408d40088c0 for (udp/239.255.0.1, *) interface udp/10.101.12.71 -1711462110.606515 [0] python3: ddsi_udp_release_conn multicast socket 36 port 7400 -1711462110.606535 [0] python3: ddsi_udp_release_conn multicast socket 37 port 7401 -1711462110.606542 [0] python3: ddsi_udp_release_conn unicast socket 34 port 7410 -1711462110.606547 [0] python3: ddsi_udp_release_conn unicast socket 35 port 7411 -1711462110.606553 [0] python3: ddsi_udp_release_conn unicast socket 38 port 50807 -1711462110.606560 [0] python3: udp finalized -1711462110.606771 [0] python3: Finis. diff --git a/topological_navigation/doc/NAVIGATION2.md b/topological_navigation/doc/NAVIGATION2.md new file mode 100644 index 00000000..b3b79bbf --- /dev/null +++ b/topological_navigation/doc/NAVIGATION2.md @@ -0,0 +1,374 @@ +# navigation2.py: Map-Driven Topological Navigation Server + +This document describes the ROS 2 node implemented in `topological_navigation/scripts/navigation2.py`, including how it interacts with the rest of the topological navigation stack. + +## Purpose + +`navigation2.py` is the execution engine for topological navigation: + +- Accepts high-level goals (`GotoNode`, `ExecutePolicyMode`) +- Plans graph routes using NetworkX +- Converts route edges into executable action segments +- Dynamically builds Nav2 action goals from map-defined templates +- Executes each segment through ROS 2 `ActionClient`s +- Publishes route, status, edge, boundary, and timing telemetry + +The script is **map-driven**: action behavior, action server bindings, and behavior tree templates come from the topological map YAML (`definitions` + `actions` sections). + +--- + +## Runtime Context in the Stack + +`navigation2.py` depends on two upstream nodes at startup: + +1. **Map manager (`map_manager2.py`)** + - Publishes `/topological_map_2` (latched / transient local) + - Provides the full map content used to build route graph and action config + +2. **Localisation (`localisation2.py`)** + - Publishes `closest_node`, `current_node`, `closest_edges` (latched) + - Supplies robot topological position and closest-edge heuristic input + +Typical launch order (see `launch/topological_navigation.launch.py`): map manager -> localisation -> navigation server. + +--- + +## ROS Interfaces Exposed by navigation2.py + +## Subscriptions + +- `/topological_map_2` (`std_msgs/String`, transient local) +- `closest_node` (`std_msgs/String`, transient local) +- `closest_edges` (`topological_navigation_msgs/ClosestEdges`, transient local) +- `current_node` (`std_msgs/String`, transient local) + +## Publishers + +- `topological_navigation/Statistics` (`topological_navigation_msgs/NavStatistics`, latched) +- `topological_navigation/Route` (`topological_navigation_msgs/TopologicalRoute`, best effort) +- `current_edge` (`std_msgs/String`, latched) +- `/boundary_checker` (`geometry_msgs/PolygonStamped`, latched) +- `/robot_operation_current_status` (`std_msgs/String`, latched) +- `topological_navigation/move_action_status` (`std_msgs/String`, latched) +- Action feedback topics for both action servers + +## Action servers + +- `/` (`topological_navigation_msgs/action/GotoNode`) +- `/topological_navigation/execute_policy_mode` (`topological_navigation_msgs/action/ExecutePolicyMode`) + +## Service client + +- `//set_parameters` (`rcl_interfaces/srv/SetParameters`) + - Used to push per-target `xy_goal_tolerance` and `yaw_goal_tolerance` + +--- + +## Main Lifecycle + +### 1) Construction and state initialization + +`TopologicalNavServer` initializes: + +- State machine (`NavStateMachine`) from `navigation_graph.py` +- Internal map/graph buffers (`_tmap`, `_graph`, pending map update buffer) +- Runtime flags (cancel/preempt/goal reached) +- QoS profiles (`_latch`, `_best`) +- Route/status/statistics publishers + +### 2) Blocking wait for first map + +The node subscribes to `/topological_map_2` and blocks until first map arrives. + +On first map application: + +- YAML parsed with `_FloatSafeLoader` +- Graph built via `build_graph_from_tmap()` (from `networkx_utils.py`) +- Map ID fields cached (`_topol_map`) +- Map-driven action configuration loaded (`_load_map_config`) + +### 3) Build map-driven action clients + +From map `actions` section: + +- Dynamically imports each `action_type` +- Creates/reuses `ActionClient`s by `action_server` +- Resolves behavior tree template references (`${definitions.}` -> temp XML file) + +Result: `self._action_clients[action_name] = {client, action_class, config}`. + +### 4) Blocking wait for localisation + +Subscribes to `closest_node` and blocks until first localisation message. + +Then subscribes to `closest_edges` and `current_node` for continuous updates. + +### 5) Action servers become READY + +Creates both action servers and transitions state machine to `READY`. + +--- + +## Route Planning and Execution Flow + +## Goal entry points + +- `GotoNode`: plan from current estimated origin to target +- `ExecutePolicyMode`: execute caller-provided route (after sanity checks) + +## Origin determination + +`_determine_origin(target)` priority: + +1. `current_node` if known +2. Closest-edge heuristic if edge distance <= `max_dist_to_closest_edge` +3. `closest_node` + +For equal-distance dual edges, compares route distance from candidate sources. + +## Route planning + +Uses `plan_route()` from `navigation_graph.py` with parameters: + +- `route_algorithm`: `astar` (default) or `dijkstra` +- `route_weight_attr`: edge attribute used as cost (default `weight`) + +## Segment generation + +From planned nodes: + +1. `get_route_edges()` to expand node list into edge dicts +2. `merge_action_segments()` to group consecutive compatible edges + - Controlled by map action config `composable` + +## Segment execution + +For each segment: + +- Transition to action-specific execution state (`ACTION_TO_STATE`) +- Publish boundary polygon when boundary properties exist +- Set Nav2 goal tolerances from target node properties +- Build goal dynamically from map template (`_build_segment_goal`) +- Dispatch goal with `_send_nav2_goal` +- Publish move status + nav statistics +- Fail/cancel handling updates state machine and feedback + +--- + +## Mid-Navigation Map Updates + +`navigation2.py` supports runtime map updates with deferred application: + +- Incoming map messages are buffered (`_pending_map_msg`) +- If robot is navigating, update is postponed to safe points (between segments) +- At safe point, `_apply_pending_map()` rebuilds graph/config +- Remaining route validity checked by `_validate_remaining_route()` + +If remaining nodes/edges no longer exist: + +- Current execution returns `False` +- `_map_updated_during_nav` is set +- `_navigate()` triggers a full replan from current situation + +This design avoids mutating route structures while a segment is in flight. + +--- + +## Boundary Polygon Behavior + +Boundary publishing is delegated to `compute_boundary_polygon()` in `navigation_graph.py`. + +Trigger condition: + +- Any edge in segment contains `properties.boundary_left` or `properties.boundary_right` + +Behavior: + +- Computes corridor polygon around segment waypoints +- Publishes to `/boundary_checker` in node nav frame +- Publishes empty polygon when no boundary applies (clears stale boundary) + +Defaults controlled by parameters: + +- `default_boundary_left` +- `default_boundary_right` + +--- + +## Map Schema Expectations Relevant to navigation2.py + +Although validation is performed upstream in map manager, `navigation2.py` expects: + +- `nodes[*].node.name` +- `nodes[*].node.pose.{position,orientation}` +- `nodes[*].node.edges[*].{edge_id,node,action}` +- `actions` dictionary with per-action config +- Optional `definitions` dictionary for inline BT XML +- `transformation.topo_frame_id` (or fallback) for pose frame defaults + +Action config shape (typical): + +```yaml +actions: + navigate_to_pose: + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + composable: false + action_goal_template: + pose: + header: + frame_id: ${node.nav_frame} + pose: ${node.pose} + behavior_tree: ${definitions.default_bt} +``` + +--- + +## Module Interaction Details + +## With `networkx_utils.py` + +- `build_graph_from_tmap()` provides normalized graph attributes used by planner and goal builders: + - Node coordinates, orientation, properties, `nav_frame` + - Edge metadata (`edge_id`, `action`, `action_type`, `properties`, `weight`) + +`navigation2.py` relies on these attributes for: + +- shortest-path planning +- route edge extraction +- dynamic pose generation +- frame resolution +- property-based tolerances + +## With `navigation_graph.py` + +Directly uses: + +- `NavState`, `NavStateMachine`, `ACTION_TO_STATE` +- `plan_route()` +- `get_route_edges()` +- `merge_action_segments()` +- `compute_boundary_polygon()` +- `get_route_distance()` + +This module holds most pure planning/state logic; `navigation2.py` orchestrates ROS I/O and action execution. + +## With `tmap_utils.py` + +Uses: + +- `get_node_from_tmap2()` as fallback when graph-based node lookup fails +- `get_edge_from_id_tmap2()` for policy-mode route edge resolution and segment preflight + +## With `localisation2.py` + +Consumes localisation outputs: + +- `closest_node` for route start fallback +- `current_node` for strongest origin evidence +- `closest_edges` for edge-based origin heuristic + +If localisation is unavailable, server intentionally blocks during startup waiting for first message. + +## With `map_manager2.py` + +Depends on `/topological_map_2` as source of truth. + +QoS compatibility is important: + +- map manager publishes with transient local durability +- navigation server subscribes with transient local durability +- late joiner still receives latest map + +--- + +## Concurrency and Thread Safety + +Key mechanisms: + +- `MultiThreadedExecutor` +- Reentrant callback groups for map and action servers +- `_map_lock` (`RLock`) protecting pending/current map mutation + +Design choice: + +- Nav2 futures are polled with `time.sleep()` in `_send_nav2_goal` +- avoids `spin_once()` conflict with external executor ownership + +--- + +## Parameters + +Declared and loaded in `_declare_parameters()` / `_load_parameters()`: + +- `max_dist_to_closest_edge` (double, default `1.0`) +- `default_boundary_left` (double, default `0.5`) +- `default_boundary_right` (double, default `0.5`) +- `route_algorithm` (string, `astar`|`dijkstra`, default `astar`) +- `route_weight_attr` (string, default `weight`) +- `goal_checker_node` (string, default `controller_server`) +- `xy_tolerance_param` (string, default `goal_checker.xy_goal_tolerance`) +- `yaw_tolerance_param` (string, default `goal_checker.yaw_goal_tolerance`) + +--- + +## Failure/Recovery Semantics + +Common failure points and behavior: + +- Missing target node -> immediate `FAILED` +- No route found -> immediate `FAILED` +- Action type import failure in map config -> action skipped/unavailable +- Nav2 server unavailable -> segment `FAILED` +- Mid-route map invalidates remaining path -> force replan +- Cancel/preempt -> propagate cancel to Nav2 and transition `CANCELLED` + +Statistics are still published on failed segments when sufficient context exists. + +--- + +## Extension Guide + +Safe extension points: + +1. **New edge action types** + - Add action definition to map `actions` + - Ensure `action_type` import path is valid + - Provide compatible `action_goal_template` + +2. **Custom route costs** + - Add edge property used as weight + - Set `route_weight_attr` accordingly + +3. **Boundary behavior tuning** + - Use edge properties `boundary_left/right` + - or change default boundary parameters + +4. **Goal tolerance policies** + - Provide per-node tolerances in node `properties` + - map to correct Nav2 parameter names + +--- + +## Operational Tips + +- If node hangs at startup, confirm latched map/localisation topics are present. +- If planning fails unexpectedly, verify edge directionality in map graph. +- If frame mismatches occur, check map `transformation.topo_frame_id`, node `nav_frame`, and Nav2 frame expectations. +- For policy execution, ensure route starts at current/closest node. + +--- + +## Related Files + +- `topological_navigation/scripts/navigation2.py` +- `topological_navigation/navigation_graph.py` +- `topological_navigation/networkx_utils.py` +- `topological_navigation/scripts/localisation2.py` +- `topological_navigation/scripts/map_manager2.py` +- `topological_navigation/tmap_utils.py` +- `topological_navigation/launch/topological_navigation.launch.py` + +--- + +Last Updated: 2026-03-09 diff --git a/topological_navigation/doc/PROPERTIES.md b/topological_navigation/doc/PROPERTIES.md deleted file mode 100644 index fc7de101..00000000 --- a/topological_navigation/doc/PROPERTIES.md +++ /dev/null @@ -1,375 +0,0 @@ -# Flexible Node and Edge Properties System - -This document describes the flexible properties system for topological maps, which allows application-specific metadata to be attached to both nodes and edges without requiring schema modifications. - -## Overview - -The topological map schema supports optional `properties` dictionaries for both nodes and edges. These properties enable domain-specific customisation while maintaining backwards compatibility with existing maps. - -## Node Properties - -Node properties are defined in `nodes[].node.properties` as a YAML dictionary. The schema allows any key-value pairs. - -### Default Properties - -The following properties are commonly used for navigation control: - -| Property | Type | Description | -|----------|------|-------------| -| `xy_goal_tolerance` | float | XY position tolerance for goal reaching (metres) | -| `yaw_goal_tolerance` | float | Yaw orientation tolerance for goal reaching (radians) | - -### Example Custom Properties - -| Property | Type | Description | -|----------|------|-------------| -| `row` | integer | Row identifier (e.g., for agricultural polytunnel scenarios) | -| `semantics` | string | Semantic meaning of the node (e.g., "charging_station", "inspection_point") | -| `zone` | string | Operational zone designation | -| `access_level` | string | Permission level required for access | -| `capacity` | integer | Maximum number of robots that can occupy the node | - -### Node Properties Example - -```yaml -nodes: -- meta: - map: riseholme - node: ChargingStation1 - pointset: riseholme - node: - name: ChargingStation1 - parent_frame: map - pose: - position: {x: 10.0, y: 5.0, z: 0.0} - orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0} - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - semantics: "charging_station" - row: 3 - zone: "A" - capacity: 2 - edges: [] -``` - -## Edge Properties - -Edge properties are defined in `nodes[].node.edges[].properties` as a YAML dictionary. The schema allows any key-value pairs. - -### Example Edge Properties - -| Property | Type | Description | -|----------|------|-------------| -| `max_speed` | float | Maximum traversal speed (m/s) | -| `priority` | integer | Preference weight for path planning (higher = more preferred) | -| `width` | float | Physical width of the traversable path (metres) | -| `surface_type` | string | Terrain classification (e.g., "concrete", "grass", "gravel") | -| `bidirectional` | boolean | Whether the edge can be traversed in both directions | -| `weather_restrictions` | list | Conditions under which edge should not be used | - -### Edge Properties Example - -```yaml -edges: -- edge_id: ChargingStation1_WayPoint2 - node: WayPoint2 - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - properties: - max_speed: 0.5 - priority: 10 - surface_type: "concrete" - bidirectional: true - weather_restrictions: ["heavy_rain", "snow"] - # ... other edge fields -``` - -## Backwards Compatibility - -The `properties` field is optional for both nodes and edges. Existing topological maps without properties remain valid and will continue to work without modification. - -When properties are not specified: -- Node properties default to standard navigation tolerances if needed by the navigation system -- Edge properties are simply absent (empty dictionary) - -## Usage Guidelines - -### Naming Conventions - -- Use `snake_case` for property names -- Use descriptive, domain-appropriate names -- Avoid abbreviations unless they are widely understood - -### Using Namespaces for Properties - -To organise properties by functional area or application, use **namespaces**. A namespace is simply a nested dictionary that groups related properties together. This approach: - -- Prevents naming conflicts between different systems -- Makes it clear which application or module owns each property -- Improves readability and maintainability -- Supports both domain-based and package-based organisation - -#### Namespace Structure - -```yaml -properties: - namespace_name: - property1: value1 - property2: value2 -``` - -#### Namespace Organisation Approaches - -You can organise namespaces in different ways depending on your needs: - -1. **Domain-based namespaces** (by concept or functional area): - ```yaml - properties: - restrictions: # Physical and access constraints - capacity: 2 - max_external_width: 0.8 - semantics: # Semantic labels and classifications - zone: "warehouse" - type: "charging_station" - ``` - -2. **Package-based namespaces** (by ROS package): - ```yaml - properties: - my_fleet_manager: - priority_zone: true - my_safety_module: - emergency_stop_point: false - ``` - -3. **Flat structure** (no namespaces): - ```yaml - properties: - capacity: 2 - zone: "warehouse" - ``` - -All three approaches are valid. Choose the organisation that best fits your application's needs and team conventions. - -#### Common Namespace Examples - -**Domain-based namespaces:** - -| Namespace | Purpose | Example Properties | -|-----------|---------|-------------------| -| `restrictions` | Physical and access constraints | `capacity`, `max_external_width`, `max_external_height`, `access_level` | -| `semantics` | Semantic labels and classifications | `zone`, `type`, `features`, `environmental_conditions` | -| `navigation` | Navigation-specific parameters | `xy_goal_tolerance`, `yaw_goal_tolerance` | -| `safety` | Safety-related constraints | `restricted_hours`, `emergency_access`, `hazard_zones` | - -**Application/package-based namespaces:** - -| Namespace | Purpose | Example Properties | -|-----------|---------|-------------------| -| `fleet_management` | Multi-robot coordination properties | `priority_zone`, `queue_enabled` | -| `logistics` | Warehouse/delivery application properties | `pickup_point`, `dropoff_point`, `storage_type` | -| `agriculture` | Agricultural robotics properties | `row_id`, `crop_type`, `irrigation_zone` | - -#### Namespaced Node Properties Example - -**Package-based organisation:** -```yaml -node: - name: ChargingStation1 - properties: - navigation: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - fleet_management: - capacity: 2 - priority_zone: true - queue_enabled: true - semantics: - type: "charging_station" - zone: "A" - logistics: - pickup_point: false - dropoff_point: false -``` - -**Domain-based organisation (alternative):** -```yaml -node: - name: PolytunnelRow4_Col2 - properties: - restrictions: - capacity: 1 - max_external_width: 0.8 # metres - max_external_height: 1.5 # metres - min_internal_width: 1.2 # metres - min_internal_height: 2.0 # metres - access_level: "operator" - semantics: - zone: - labels: ['polytunnel', 'growing_area'] - details: {tunnel_id: 4, row_id: 5, column_idx: 2} - features: - labels: ['irrigation_point', 'sensor_node'] - details: {sensor_types: ['humidity', 'temperature']} - environmental: - labels: ['soil', 'shade'] - details: {soil_type: 'loam', 'shade_percentage': 30} -``` - -#### Namespaced Edge Properties Example - -**Package-based organisation:** -```yaml -edges: -- edge_id: ChargingStation1_WayPoint2 - node: WayPoint2 - properties: - navigation: - max_speed: 0.5 - safety: - width: 1.2 - restricted_hours: ["22:00-06:00"] - logistics: - priority: 10 - surface_type: "concrete" -``` - -**Domain-based organisation (alternative):** -```yaml -edges: -- edge_id: Row4_Col2_Row4_Col3 - node: Row4_Col3 - properties: - restrictions: - max_external_width: 0.75 # metres - narrow passage - max_external_height: 1.8 # metres - low overhead - capacity: 1 # single robot at a time - semantics: - zone: - labels: ['polytunnel_interior', 'growing_area'] - details: {tunnel_id: 4} - environmental: - labels: ['grass', 'uneven'] - details: {surface_quality: 'variable'} -``` - -#### Accessing Namespaced Properties in Code - -```python -# Safe access to namespaced node properties -node_props = node["node"].get("properties", {}) - -# Access domain-based namespace -restrictions = node_props.get("restrictions", {}) -capacity = restrictions.get("capacity", 1) -max_width = restrictions.get("max_external_width") - -# Access package-based namespace -fleet_props = node_props.get("fleet_management", {}) -priority_zone = fleet_props.get("priority_zone", False) - -# Access nested semantic properties -semantics = node_props.get("semantics", {}) -zone_info = semantics.get("zone", {}) -zone_labels = zone_info.get("labels", []) -zone_details = zone_info.get("details", {}) - -# Access namespaced edge properties -edge_props = edge.get("properties", {}) -restrictions = edge_props.get("restrictions", {}) -max_width = restrictions.get("max_external_width") -``` - -#### Guidelines for Namespace Usage - -1. **Choose the right organisation for your use case**: - - Use **domain-based namespaces** (like `restrictions`, `semantics`) when organising by conceptual categories - - Use **package-based namespaces** when multiple ROS packages share the same map and need to avoid conflicts - - Use **flat structure** for simple cases with few properties - -2. **Be consistent within your project**: Choose one organisational approach and stick with it across your maps. - -3. **Document your namespace conventions**: If you introduce new namespaces, document their purpose and expected properties in your project documentation. - -4. **Avoid deep nesting**: One level of namespace nesting is usually sufficient. Deeper nesting (like `properties.semantics.zone.labels`) should be reserved for truly hierarchical data. - -5. **Backwards compatibility**: For core navigation properties like `xy_goal_tolerance`, either keep them at root level or consistently place them in a `navigation` namespace across all maps. - -6. **Package name convention** (optional): ROS packages that define properties can use their package name as a namespace to avoid conflicts. E.g., a package called "topfleets_coordinator" could use this as the namespace. - -### Type Flexibility - -Properties support various data types: -- **Strings**: `"charging_station"`, `"concrete"` -- **Numbers**: `0.5`, `10`, `3.14` -- **Booleans**: `true`, `false` -- **Lists**: `["heavy_rain", "snow"]` -- **Nested Objects**: `{min: 0.1, max: 1.0}` - -### Application-Specific Properties - -Applications can define their own property schemas and document them appropriately. The topological navigation system will safely ignore properties it does not recognise, allowing different applications to coexist on the same map. - -### Accessing Properties in Code - -When accessing properties programmatically, always check for property existence before use: - -```python -# Safe access to node properties -node_props = node["node"].get("properties", {}) -xy_tolerance = node_props.get("xy_goal_tolerance", 0.3) # Default to 0.3 -semantics = node_props.get("semantics") # Returns None if not present - -# Safe access to edge properties -edge_props = edge.get("properties", {}) -max_speed = edge_props.get("max_speed") # Returns None if not present -if max_speed is not None: - # Use max_speed for navigation control - pass -``` - -## Schema Reference - -The properties fields are defined in `config/tmap-schema.yaml`: - -```yaml -# Node properties (at nodes[].node.properties) -properties: - type: object - additionalProperties: true - description: Flexible dictionary of application-specific node properties - -# Edge properties (at nodes[].node.edges[].properties) -properties: - type: object - additionalProperties: true - description: Flexible dictionary of application-specific edge properties -``` - -## Validating Topological Maps - -The `validate_map.py` script can be used to validate topological map YAML files against the schema: - -```bash -# Basic validation -ros2 run topological_navigation validate_map.py my_map.tmap2.yaml - -# With verbose output -ros2 run topological_navigation validate_map.py my_map.tmap2.yaml -v - -# With custom schema file -ros2 run topological_navigation validate_map.py my_map.tmap2.yaml --schema custom_schema.yaml -``` - -The validator will: -- Check the map structure against the JSON schema -- Report any missing required fields -- Warn about duplicate node names -- Warn about edges pointing to non-existent nodes - -## Related Resources - -- [Topological Map Schema](../config/tmap-schema.yaml) -- [Node Template](../config/template_node_2.yaml) -- [Edge Template](../config/template_edge.yaml) diff --git a/topological_navigation/launch/manual_topomapping.launch.py b/topological_navigation/launch/manual_topomapping.launch.py deleted file mode 100644 index cc84a844..00000000 --- a/topological_navigation/launch/manual_topomapping.launch.py +++ /dev/null @@ -1,42 +0,0 @@ -''' -Author: Ibrahim Hroob 2024 -''' -import os -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - # Declare the launch arguments - DeclareLaunchArgument('tmap', default_value='test.yaml'), - DeclareLaunchArgument('tmap_dir', default_value='/home/ros/tmap'), - DeclareLaunchArgument('node_thresh', default_value='0.5'), - DeclareLaunchArgument('lock_btn', default_value='6'), - DeclareLaunchArgument('add_btn', default_value='1'), - DeclareLaunchArgument('remove_btn', default_value='2'), - DeclareLaunchArgument('gen_map_btn', default_value='3'), - DeclareLaunchArgument('topic_joy', default_value='/joy'), - DeclareLaunchArgument('topic_pose', default_value='/gps_base/odometry'), - - # Launch the manual_topomapping node - Node( - package='topological_navigation', - executable='manual_topomapping.py', - name='manual_topomapping', - output='screen', - parameters=[{ - 'tmap' : LaunchConfiguration('tmap'), - 'tmap_dir' : LaunchConfiguration('tmap_dir'), - 'site_name' : LaunchConfiguration('tmap'), - 'node_thresh': LaunchConfiguration('node_thresh'), - 'lock_btn' : LaunchConfiguration('lock_btn'), - 'add_btn' : LaunchConfiguration('add_btn'), - 'remove_btn' : LaunchConfiguration('remove_btn'), - 'gen_map_btn': LaunchConfiguration('gen_map_btn'), - 'topic_joy' : LaunchConfiguration('topic_joy'), - 'topic_pose' : LaunchConfiguration('topic_pose'), - }] - ), - ]) diff --git a/topological_navigation/launch/minimal_topological_navigation.launch b/topological_navigation/launch/minimal_topological_navigation.launch deleted file mode 100644 index b1867534..00000000 --- a/topological_navigation/launch/minimal_topological_navigation.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_navigation/launch/reconf_at_edges_server.launch b/topological_navigation/launch/reconf_at_edges_server.launch deleted file mode 100644 index 17b4d80e..00000000 --- a/topological_navigation/launch/reconf_at_edges_server.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/topological_navigation/launch/topo_nav_global.launch b/topological_navigation/launch/topo_nav_global.launch deleted file mode 100644 index bfdb1b5d..00000000 --- a/topological_navigation/launch/topo_nav_global.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/topological_navigation/launch/topo_nav_local.launch b/topological_navigation/launch/topo_nav_local.launch deleted file mode 100644 index b842e275..00000000 --- a/topological_navigation/launch/topo_nav_local.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/topological_navigation/launch/topological_map_visualise.launch b/topological_navigation/launch/topological_map_visualise.launch deleted file mode 100644 index 7580bc48..00000000 --- a/topological_navigation/launch/topological_map_visualise.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/topological_navigation/launch/topological_navigation.launch b/topological_navigation/launch/topological_navigation.launch deleted file mode 100644 index 85307e30..00000000 --- a/topological_navigation/launch/topological_navigation.launch +++ /dev/null @@ -1,108 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - $(arg move_base_actions) - - - - - - - - - - - - - - - - diff --git a/topological_navigation/launch/topological_navigation.launch.py b/topological_navigation/launch/topological_navigation.launch.py new file mode 100644 index 00000000..7a147cbb --- /dev/null +++ b/topological_navigation/launch/topological_navigation.launch.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +# Copyright (c) 2026, topological_navigation contributors +# Licensed under the MIT License. +"""Launch the full topological navigation stack with simulation. + +Launches (in order): + 1. Map Manager -- loads and publishes the topological map + 2. Localisation -- determines current/closest node + 3. Fake Nav2 simulator -- virtual robot + fake action servers + 4. Topological map visualiser -- RViz markers + 5. RViz -- with a pre-configured display layout + +Usage +----- +Default (mixed_actions_map):: + + ros2 launch topological_navigation topological_navigation.launch.py + +Custom map file:: + + ros2 launch topological_navigation topological_navigation.launch.py \ + map_path:=/absolute/path/to/my_map.tmap2.yaml +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, TimerAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + topo_nav_share = get_package_share_directory('topological_navigation') + + # --- Default paths --- + default_map = os.path.join( + topo_nav_share, 'config', 'mixed_actions_map.yaml', + ) + default_rviz = os.path.join( + topo_nav_share, 'rviz', 'topological_navigation.rviz', + ) + + return LaunchDescription([ + # ============================================================= + # Launch arguments + # ============================================================= + DeclareLaunchArgument( + 'map_path', default_value=default_map, + description='Absolute path to a .tmap2.yaml topological map', + ), + DeclareLaunchArgument( + 'rviz_config', default_value=default_rviz, + description='Path to the RViz config file', + ), + DeclareLaunchArgument( + 'initial_x', default_value='0.0', + description='Initial robot X position', + ), + DeclareLaunchArgument( + 'initial_y', default_value='0.0', + description='Initial robot Y position', + ), + DeclareLaunchArgument( + 'initial_yaw', default_value='0.0', + description='Initial robot yaw (radians)', + ), + + # ============================================================= + # 1. Map Manager + # ============================================================= + Node( + package='topological_navigation', + executable='map_manager2.py', + name='topological_map_manager_2', + output='screen', + arguments=[LaunchConfiguration('map_path')], + ), + + # ============================================================= + # 2. Localisation (delayed 2 s so map is published first) + # ============================================================= + TimerAction( + period=2.0, + actions=[ + Node( + package='topological_navigation', + executable='localisation2.py', + name='topological_localisation', + output='screen', + ), + ], + ), + + # ============================================================= + # 3. Fake Nav2 simulator (delayed 2 s) + # ============================================================= + TimerAction( + period=2.0, + actions=[ + Node( + package='topological_nav_simulator', + executable='fake_nav2_server', + name='fake_nav2_server', + output='screen', + parameters=[{ + 'initial_x': LaunchConfiguration('initial_x'), + 'initial_y': LaunchConfiguration('initial_y'), + 'initial_yaw': LaunchConfiguration('initial_yaw'), + }], + ), + ], + ), + + # ============================================================= + # 4. Topological navigation server (delayed 3 s) + # ============================================================= + TimerAction( + period=3.0, + actions=[ + Node( + package='topological_navigation', + executable='navigation2.py', + name='topological_navigation', + output='screen', + ), + ], + ), + + # ============================================================= + # 5. Topological map visualiser (delayed 4 s) + # ============================================================= + TimerAction( + period=4.0, + actions=[ + Node( + package='topological_navigation_visual', + executable='topological_map_visualiser.py', + name='topological_map_visualiser', + output='screen', + parameters=[{ + 'edit_mode': True, + }], + ), + ], + ), + + # ============================================================= + # 6. RViz (delayed 5 s) + # ============================================================= + TimerAction( + period=5.0, + actions=[ + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=[ + '-d', LaunchConfiguration('rviz_config'), + ], + ), + ], + ), + ]) diff --git a/topological_navigation/launch/topological_navigation_empty_map.launch b/topological_navigation/launch/topological_navigation_empty_map.launch deleted file mode 100644 index 8db6ac7e..00000000 --- a/topological_navigation/launch/topological_navigation_empty_map.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_navigation/launch/topological_restrictions_multirobot.launch b/topological_navigation/launch/topological_restrictions_multirobot.launch deleted file mode 100644 index 1e7c2a39..00000000 --- a/topological_navigation/launch/topological_restrictions_multirobot.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/topological_navigation/package.xml b/topological_navigation/package.xml index b0d2b3e7..3c4c6817 100644 --- a/topological_navigation/package.xml +++ b/topological_navigation/package.xml @@ -2,12 +2,13 @@ topological_navigation - 3.0.5 - The ros2 topological_navigation package + 5.0.0 + The ros2 topological_navigation package (ROS1 support removed in v5.0.0) Marc Hanheide James R Heselden + Ibrahim Hroob Jaime Pulido Fentanes Adam Binch @@ -19,12 +20,20 @@ ament_flake8 ament_pep257 python3-pytest + python3-pytest-cov + python3-pytest-mock launch_pytest + python3-hypothesis ament_cmake + python3-networkx + python3-scipy + python3-numpy + python3-jsonschema geometry_msgs std_msgs + std_srvs nav_msgs sensor_msgs actionlib_msgs diff --git a/topological_navigation/rviz/topological_navigation.rviz b/topological_navigation/rviz/topological_navigation.rviz new file mode 100644 index 00000000..25e4a63b --- /dev/null +++ b/topological_navigation/rviz/topological_navigation.rviz @@ -0,0 +1,215 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /MarkerArray1 + - /MarkerArray1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 406 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + /route_edges: true + /route_endpoints: true + /route_label: true + /route_nodes: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /topological_route_visualisation + Value: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /topological_map_editor + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + virtual_robot: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /virtual_robot/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + /edges: true + /legend: true + /names: true + /nodes: true + /zones: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /topological_map_visualisation + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + /route_edges: true + /route_endpoints: true + /route_label: true + /route_nodes: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /topological_route_visualisation + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /boundary_checker + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 17.686002731323242 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 5.104556083679199 + Y: 0.9502983093261719 + Z: 0.5616544485092163 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.0697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.1703968048095703 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 697 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000021ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000021f000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000021ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000021f000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000051d0000003efc0100000002fb0000000800540069006d006501000000000000051d0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002ac0000021f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1309 + X: 0 + Y: 0 diff --git a/topological_navigation/setup.py b/topological_navigation/setup.py index 0cb18c5a..349112fd 100644 --- a/topological_navigation/setup.py +++ b/topological_navigation/setup.py @@ -6,58 +6,39 @@ setup( name=package_name, - version='3.0.5', + version='5.0.0', # Major version bump - ROS1 code removed packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ('share/' + package_name + '/config/', glob('config/*', recursive=True)), - ('share/' + package_name + '/launch/', glob('launch/*', recursive=True)) + ('share/' + package_name + '/config/', + glob('test/fixtures/mixed_actions_map.yaml')), + ('share/' + package_name + '/launch/', glob('launch/*', recursive=True)), + ('share/' + package_name + '/rviz/', glob('rviz/*', recursive=True)), ], install_requires=['setuptools'], zip_safe=True, - maintainer='Adam Binch', - maintainer_email='abinch@sagarobotics.com', - description='The ros2 topological_navigation package', + maintainer='Ibrahim Hroob', + maintainer_email='ihroob@lincoln.ac.uk', + description='ROS2 topological navigation package (ROS1 support removed in v4.0.0)', license='MIT', tests_require=['pytest', 'launch-pytest'], entry_points={ 'console_scripts': [ - 'get_simple_policy2.py = topological_navigation.scripts.get_simple_policy2:main', + # Core ROS2 Navigation Nodes + 'navigation2.py = topological_navigation.scripts.navigation2:main', 'localisation2.py = topological_navigation.scripts.localisation2:main', - 'manual_edge_predictions.py = topological_navigation.scripts.manual_edge_predictions:main', 'map_manager2.py = topological_navigation.scripts.map_manager2:main', - 'map_manager.py = topological_navigation.scripts.map_manager:main', - 'map_publisher.py = topological_navigation.scripts.map_publisher:main', - 'mean_based_prediction.py = topological_navigation.scripts.mean_based_prediction:main', - 'nav_client.py = topological_navigation.scripts.nav_client:main', - 'navigation.py = topological_navigation.scripts.navigation:main', - 'navigation2.py = topological_navigation.scripts.navigation2:main', - 'navstats_logger.py = topological_navigation.scripts.navstats_logger:main', - 'reconf_at_edges_server.py = topological_navigation.scripts.reconf_at_edges_server:main', - 'restrictions_manager.py = topological_navigation.scripts.restrictions_manager:main', - 'search_route.py = topological_navigation.scripts.search_route:main', - 'speed_based_prediction.py = topological_navigation.scripts.speed_based_prediction:main', - 'test_top_pred.py = topological_navigation.scripts.test_top_pred:main', - 'topological_prediction.py = topological_navigation.scripts.topological_prediction:main', - 'topological_transform_publisher.py = topological_navigation.scripts.topological_transform_publisher:main', - 'travel_time_estimator.py = topological_navigation.scripts.travel_time_estimator:main', - 'visualise_map2.py = topological_navigation.scripts.visualise_map2:main', - 'visualise_map.py = topological_navigation.scripts.visualise_map:main', - 'visualise_map_ros2.py = topological_navigation.scripts.visualise_map_ros2:main', - 'topomap_marker.py = topological_navigation.topomap_marker:main', - 'topomap_marker2.py = topological_navigation.topomap_marker2:main', - 'policy_marker.py = topological_navigation.policy_marker:main', + + # Supporting Utilities 'manual_topomapping.py = topological_navigation.scripts.manual_topomapping:main', - 'occupancy_checker.py = topological_navigation.scripts.occupancy_checker:main', - 'topological_visual.py = topological_navigation.scripts.topological_visual:main', - 'validate_map.py = topological_navigation.scripts.validate_map:main', + 'validate_map.py = topological_navigation.validate_map:main', + + # Map conversion + 'convert_tmap.py = topological_navigation.convert_tmap:main', ], }, ) - - - - diff --git a/topological_navigation/test/__init__.py b/topological_navigation/test/__init__.py new file mode 100644 index 00000000..7b58e2e9 --- /dev/null +++ b/topological_navigation/test/__init__.py @@ -0,0 +1 @@ +"""Test package for topological_navigation.""" diff --git a/topological_navigation/test/conf/network_autogen.tmap2.yaml b/topological_navigation/test/conf/network_autogen.tmap2.yaml deleted file mode 100644 index eca3a7c9..00000000 --- a/topological_navigation/test/conf/network_autogen.tmap2.yaml +++ /dev/null @@ -1,14393 +0,0 @@ -meta: - last_updated: 2022-06-23_13-10-11 -metric_map: riseholme -name: strawberry_polytunnel -nodes: -- meta: - map: riseholme - node: WayPoint140 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint140_WayPoint74 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint74 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint140_WayPoint141 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint141 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint140_WayPoint142 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint142 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: WayPoint140 - parent_frame: map - pose: - orientation: - w: 0.7897749049165983 - x: 0.0 - y: 0.0 - z: -0.6133967717260812 - position: - x: 20.7508434296 - y: -4.37950954437 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.177953019738 - y: -0.725811064243 - - x: 0.389276951551 - y: -0.63791257143 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint141 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint141_WayPoint73 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint73 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint141_WayPoint140 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint140 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint141_WayPoint144 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint144 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint141 - parent_frame: map - pose: - orientation: - w: 0.8029703158157347 - x: 0.0 - y: 0.0 - z: -0.5960190197626072 - position: - x: 23.3039474487 - y: -3.95409524918 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.177953019738 - y: -0.725811064243 - - x: 0.389276951551 - y: -0.63791257143 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint142 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint142_WayPoint140 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint140 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint142_WayPoint66 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint66 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint142_WayPoint56 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint56 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: WayPoint142 - parent_frame: map - pose: - orientation: - w: 0.7798152577689397 - x: 0.0 - y: 0.0 - z: -0.6260097153804901 - position: - x: 17.9042915344 - y: -4.71533472061 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.177953019738 - y: -0.725811064243 - - x: 0.389276951551 - y: -0.63791257143 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint143 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint143_WayPoint68 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint68 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 -# - action: NavigateToPose -# action_type: geometry_msgs/PoseStamped -# config: [] -# edge_id: WayPoint143_WayPoint144 -# fail_policy: fail -# fluid_navigation: true -# goal: -# target_pose: -# header: -# frame_id: $node.parent_frame -# pose: $node.pose -# node: WayPoint144 -# recovery_behaviours_config: '' -# restrictions_planning: 'True' -# restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint143 - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 28.45 - y: -1.7 - z: 1.65918641401e-05 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.528169989586 - y: 0.208579674363 - - x: -0.44027146697 - y: -0.358650296926 - - x: -0.177953019738 - y: -0.725811064243 - - x: 0.389276951551 - y: -0.63791257143 - - x: 0.528169989586 - y: -0.208579674363 - - x: 0.44027146697 - y: 0.358650296926 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint56 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint56_WayPoint142 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint142 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint56_WayPoint63 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint63 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint56_WayPoint66 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint66 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint56_r8.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint56_r7.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint56_r9-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint56_r8-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint56_r7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: WayPoint56 - parent_frame: map - pose: - orientation: - w: 0.7339387165828536 - x: 0.0 - y: 0.0 - z: -0.6792156949752515 - position: - x: 15.3362770081 - y: -7.2246799469 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.504145145416 - y: -1.52811074257 - - x: 1.00579595566 - y: -1.26752996445 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint63 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint63_WayPoint56 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint56 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint63_r9.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint63_r10-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint63_r9-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: WayPoint63 - parent_frame: map - pose: - orientation: - w: 0.7553558324223075 - x: 0.0 - y: 0.0 - z: -0.6553148605255362 - position: - x: 12.4204454422 - y: -7.72864294052 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.426953971386 - y: -1.58275640011 - - x: 0.954951286316 - y: -1.36671674252 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint66 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint66_WayPoint142 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint142 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint66_WayPoint56 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint56 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint66_WayPoint74 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint74 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint66_r6.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint66_r6-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint66_r7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: WayPoint66 - parent_frame: map - pose: - orientation: - w: 0.7141335628529389 - x: 0.0 - y: 0.0 - z: -0.7000094673695261 - position: - x: 18.2440853119 - y: -6.72151565552 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.65414917469 - y: -1.57640635967 - - x: 0.991357862949 - y: -1.31524348259 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint67 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint67_WayPoint73 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint73 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint67_r1.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint67_r1-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint67_r2-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint67_WayPoint144 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint144 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint67 - parent_frame: map - pose: - orientation: - w: 0.7031461383733332 - x: 0.0 - y: 0.0 - z: -0.7110453628923188 - position: - x: 26.2929611206 - y: -5.31065368652 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.639109969139 - y: -1.66431057453 - - x: 0.955555677414 - y: -1.47564923763 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint68 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint68_WayPoint69 - fail_policy: fail - fluid_navigation: false - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint69 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 -# - action: NavigateToPose -# action_type: geometry_msgs/PoseStamped -# config: [] -# edge_id: WayPoint68_WayPoint143 -# fail_policy: fail -# fluid_navigation: true -# goal: -# target_pose: -# header: -# frame_id: $node.parent_frame -# pose: $node.pose -# node: WayPoint143 -# recovery_behaviours_config: '' -# restrictions_planning: 'True' -# restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint68_WayPoint144 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint144 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint68 - parent_frame: map - pose: - orientation: - w: -0.0611201941617971 - x: -0.0 - y: 0.0 - z: 0.9981304132555145 - position: - x: 26.75 - y: 2.799999999999997 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.268447875977 - y: 0.700840473175 - - x: -1.01734352112 - y: 0.270284175873 - - x: -1.08588027954 - y: -0.547501802444 - - x: -0.496746063232 - y: -1.16582393646 - - x: 0.359563827515 - y: -1.04076695442 - - x: 0.839046478271 - y: -0.112289190292 - - x: 0.823705673218 - y: 0.478964328766 - - x: 0.357343673706 - y: 0.908497810364 -- meta: - map: riseholme - node: WayPoint69 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint69_WayPoint68 - fail_policy: fail - fluid_navigation: false - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint68 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint69_WayPoint70 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint70 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint69_WayPoint71 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint71 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint69_WayPoint72 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint72 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint69_s0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: s0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint69 - parent_frame: map - pose: - orientation: - w: 0.9967937200314319 - x: 0.0 - y: 0.0 - z: 0.08001424689328425 - position: - x: 21.0161151886 - y: 1.9613897800400002 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.177953019738 - y: -0.725811064243 - - x: 0.389276951551 - y: -0.63791257143 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint70 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint70_WayPoint69 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint69 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint70_dock-2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: dock-2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint70 - parent_frame: map - pose: - orientation: - w: 0.9971259382326672 - x: 0.0 - y: 0.0 - z: 0.07576188556011103 - position: - x: 19.257296070401097 - y: 0.3913308268742269 - z: 0.0 - properties: - xy_goal_tolerance: 0.8 - yaw_goal_tolerance: 7.0 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.177953019738 - y: -0.725811064243 - - x: 0.389276951551 - y: -0.63791257143 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint71 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint71_WayPoint69 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint69 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint71_dock-1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: dock-1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint71 - parent_frame: map - pose: - orientation: - w: 0.997458317983662 - x: 0.0 - y: 0.0 - z: 0.07125239564536533 - position: - x: 18.9572960704011 - y: 1.9913308268742353 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.177953019738 - y: -0.725811064243 - - x: 0.389276951551 - y: -0.63791257143 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint72 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint72_WayPoint69 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint69 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint72_dock-0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: dock-0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint72 - parent_frame: map - pose: - orientation: - w: 0.9981387030430853 - x: 0.0 - y: 0.0 - z: 0.0609846660027559 - position: - x: 18.7072960704011 - y: 3.4913308268742353 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.177953019738 - y: -0.725811064243 - - x: 0.389276951551 - y: -0.63791257143 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint73 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint73_WayPoint67 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint67 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint73_WayPoint74 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint74 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint73_WayPoint141 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint141 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint73_r3.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint73_r2.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint73_r4-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint73_r3-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint73_r2-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: WayPoint73 - parent_frame: map - pose: - orientation: - w: 0.689354695246495 - x: 0.0 - y: 0.0 - z: -0.7244239809266477 - position: - x: 23.6688137054 - y: -5.80918359756 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.567866265774 - y: -1.61818635464 - - x: 0.967507243156 - y: -1.30801534653 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: WayPoint74 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint74_WayPoint66 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint66 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint74_WayPoint73 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint73 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint74_WayPoint140 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint140 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint74_r4.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint74_r5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint74_r4-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: WayPoint74 - parent_frame: map - pose: - orientation: - w: 0.8173953878542746 - x: 0.0 - y: 0.0 - z: -0.5760770607432308 - position: - x: 21.1509933472 - y: -6.20381689072 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: -0.389276951551 - y: 0.63791257143 - - x: -0.725811064243 - y: 0.177953019738 - - x: -0.63791257143 - y: -0.389276951551 - - x: -0.567870855331 - y: -1.61818492413 - - x: 1.00797331333 - y: -1.34632718563 - - x: 0.725811064243 - y: -0.177953019738 - - x: 0.63791257143 - y: 0.389276951551 - - x: 0.177953019738 - y: 0.725811064243 -- meta: - map: riseholme - node: dock-0 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: dock-0_WayPoint72 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint72 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: dock-0 - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 16.557296070401094 - y: 3.141330826874227 - z: 1.65918641401e-05 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.689999997616 - y: 0.287000000477 - - x: 0.287000000477 - y: 0.689999997616 - - x: -0.287000000477 - y: 0.689999997616 - - x: -0.689999997616 - y: 0.287000000477 - - x: -0.689999997616 - y: -0.287000000477 - - x: -0.287000000477 - y: -0.689999997616 - - x: 0.287000000477 - y: -0.689999997616 - - x: 0.689999997616 - y: -0.287000000477 -- meta: - map: riseholme - node: dock-1 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: dock-1_WayPoint71 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint71 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: dock-1 - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 16.757296070401097 - y: 1.691330826874224 - z: 1.65918641401e-05 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.689999997616 - y: 0.287000000477 - - x: 0.287000000477 - y: 0.689999997616 - - x: -0.287000000477 - y: 0.689999997616 - - x: -0.689999997616 - y: 0.287000000477 - - x: -0.689999997616 - y: -0.287000000477 - - x: -0.287000000477 - y: -0.689999997616 - - x: 0.287000000477 - y: -0.689999997616 - - x: 0.689999997616 - y: -0.287000000477 -- meta: - map: riseholme - node: dock-2 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: dock-2_WayPoint70 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint70 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: dock-2 - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 17.007296070401097 - y: 0.04133082687423256 - z: 1.65918641401e-05 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: 'True' - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.689999997616 - y: 0.287000000477 - - x: 0.287000000477 - y: 0.689999997616 - - x: -0.287000000477 - y: 0.689999997616 - - x: -0.689999997616 - y: 0.287000000477 - - x: -0.689999997616 - y: -0.287000000477 - - x: -0.287000000477 - y: -0.689999997616 - - x: 0.287000000477 - y: -0.689999997616 - - x: 0.689999997616 - y: -0.287000000477 -- meta: - map: riseholme - node: r5.7-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-ca_r5.7-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-ca_r6.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r5.7-ca - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 19.528250846806248 - y: -9.350509289576738 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-cb_r5.7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-cb_r5.7-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-cb - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.049999999999997 - y: -12.9125 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c0_r5.7-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c0_r5.7-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-c0 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.497500000000002 - y: -15.8875 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c1_r5.7-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c1_r5.7-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-c1 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.9425 - y: -18.855 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c2_r5.7-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c2_r5.7-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-c2 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 21.3725 - y: -21.8325 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c3_r5.7-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c3_r5.7-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-c3 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 21.7975 - y: -24.822499999999998 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c4_r5.7-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c4_r5.7-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-c4 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 22.23 - y: -27.785 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c5_r5.7-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-c5_r5.7-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-c5 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 22.657500000000002 - y: -30.7025 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-cy_r5.7-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-cy_r5.7-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-cy - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 23.1 - y: -33.615 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r5.7-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.7-cz_r5.7-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.7-cz - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 23.62174915319375 - y: -37.17699071042326 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-ca_r6.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-ca_WayPoint66 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint66 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-ca_r7.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-ca_r5.7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.7-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r6.5-ca - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 18.252493548747736 - y: -9.694890412504861 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-cb_r6.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-cb_r6.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-cb - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 18.77 - y: -13.2575 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c0_r6.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c0_r6.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-c0 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 19.217499999999998 - y: -16.225 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c1_r6.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c1_r6.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-c1 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 19.645 - y: -19.1875 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c2_r6.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c2_r6.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-c2 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.075000000000003 - y: -22.1725 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c3_r6.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c3_r6.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-c3 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.5175 - y: -25.1475 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c4_r6.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c4_r6.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-c4 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.942500000000003 - y: -28.1125 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c5_r6.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-c5_r6.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-c5 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 21.35 - y: -30.9625 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-cy_r6.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-cy_r6.5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-cy - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 21.7375 - y: -33.6625 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r6.5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6.5-cz_r6.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r6.5-cz - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 22.255006451252264 - y: -37.22510958749514 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-ca_r7.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-ca_WayPoint56 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint56 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-ca_r8.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-ca_r6.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r7.5-ca - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.920934013541007 - y: -9.87584942170954 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-cb_r7.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-cb_r7.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-cb - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 17.445 - y: -13.4375 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c0_r7.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c0_r7.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-c0 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 17.8975 - y: -16.4 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c1_r7.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c1_r7.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-c1 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 18.330000000000002 - y: -19.377499999999998 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c2_r7.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c2_r7.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-c2 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 18.7675 - y: -22.375000000000004 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c3_r7.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c3_r7.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-c3 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 19.2125 - y: -25.34 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c4_r7.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c4_r7.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-c4 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 19.6375 - y: -28.305 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c5_r7.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-c5_r7.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-c5 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.049999999999997 - y: -31.15 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-cy_r7.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-cy_r7.5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-cy - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.462500000000002 - y: -33.849999999999994 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r7.5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7.5-cz_r7.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r7.5-cz - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 20.986565986458995 - y: -37.411650578290455 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-ca_r8.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-ca_WayPoint56 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint56 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-ca_r9.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-ca_r7.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r8.5-ca - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 15.580958737071159 - y: -10.061214516016259 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-cb_r8.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-cb_r8.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-cb - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.107499999999998 - y: -13.622499999999999 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c0_r8.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c0_r8.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-c0 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.5575 - y: -16.5825 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c1_r8.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c1_r8.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-c1 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.995 - y: -19.5625 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c2_r8.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c2_r8.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-c2 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 17.4425 - y: -22.552500000000002 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c3_r8.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c3_r8.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-c3 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 17.8875 - y: -25.53 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c4_r8.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c4_r8.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-c4 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 18.317500000000003 - y: -28.505000000000003 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c5_r8.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-c5_r8.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-c5 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 18.73 - y: -31.362499999999997 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-cy_r8.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-cy_r8.5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-cy - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 19.137500000000003 - y: -34.0625 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r8.5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8.5-cz_r8.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r8.5-cz - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 19.66404126292884 - y: -37.62378548398374 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-ca_r9.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-ca_WayPoint63 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint63 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-ca_r10.3-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-ca_r8.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r9.5-ca - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 14.258157714437713 - y: -10.248390087675054 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-cb_r9.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-cb_r9.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-cb - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 14.782499999999999 - y: -13.81 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c0_r9.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c0_r9.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-c0 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 15.23 - y: -16.770000000000003 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c1_r9.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c1_r9.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-c1 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 15.677499999999998 - y: -19.7725 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c2_r9.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c2_r9.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-c2 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.125 - y: -22.762499999999996 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c3_r9.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c3_r9.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-c3 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.5625 - y: -25.724999999999998 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c4_r9.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c4_r9.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-c4 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.990000000000002 - y: -28.7025 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c5_r9.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-c5_r9.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-c5 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 17.4025 - y: -31.575000000000003 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-cy_r9.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-cy_r9.5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-cy - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 17.799999999999997 - y: -34.272499999999994 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r9.5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9.5-cz_r9.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r9.5-cz - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 18.32434228556228 - y: -37.83410991232494 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-ca_r10.3-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-ca_r9.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r10.3-ca - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 12.921362395974217 - y: -10.255786415270013 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-cb_r10.3-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-cb_r10.3-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-cb - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 13.445000000000002 - y: -13.817499999999999 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c0_r10.3-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c0_r10.3-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-c0 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 13.895 - y: -16.7875 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c1_r10.3-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c1_r10.3-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-c1 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 14.35 - y: -19.822499999999998 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c2_r10.3-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c2_r10.3-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-c2 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 14.7975 - y: -22.83 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c3_r10.3-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c3_r10.3-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-c3 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 15.2225 - y: -25.78 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c4_r10.3-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c4_r10.3-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-c4 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 15.6475 - y: -28.7525 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c5_r10.3-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-c5_r10.3-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-c5 - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.08 - y: -31.6875 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-cy_r10.3-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-cy_r10.3-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-cy - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 16.502499999999998 - y: -34.5375 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r10.3-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10.3-cz_r10.3-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10.3-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r10.3-cz - parent_frame: map - pose: - orientation: - w: 0.7566142743603234 - x: 0.0 - y: 0.0 - z: -0.6538614836754045 - position: - x: 17.026137604025784 - y: -38.09921358472999 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3839717361251064 - y: -0.6411198818013286 - - x: 0.5264215248037732 - y: -0.212953957097135 - - x: 0.4432315207951729 - y: 0.3549856737865013 - - x: 0.1839678947585299 - y: 0.724309885809929 - - x: -0.3839717361251064 - y: 0.6411198818013286 - - x: -0.5264215248037732 - y: 0.212953957097135 - - x: -0.4432315207951729 - y: -0.3549856737865013 - - x: -0.1839678947585299 - y: -0.724309885809929 -- meta: - map: riseholme - node: r0.7-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-ca_r0.7-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-ca_r1.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r0.7-ca - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 27.478100868522482 - y: -8.216267445778325 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-cb_r0.7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-cb_r0.7-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-cb - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.005000000000003 - y: -11.7775 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c0_r0.7-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c0_r0.7-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-c0 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.4425 - y: -14.760000000000002 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c1_r0.7-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c1_r0.7-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-c1 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.865000000000002 - y: -17.72 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c2_r0.7-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c2_r0.7-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-c2 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 29.295 - y: -20.6775 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c3_r0.7-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c3_r0.7-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-c3 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 29.747500000000002 - y: -23.655 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c4_r0.7-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c4_r0.7-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-c4 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 30.1975 - y: -26.6625 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c5_r0.7-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-c5_r0.7-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-c5 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 30.635 - y: -29.575 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-cy_r0.7-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-cy_r0.7-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-cy - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 31.0475 - y: -32.385 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r0.7-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r0.7-cz_r0.7-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r0.7-cz - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 31.57439913147752 - y: -35.94623255422167 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-ca_r1.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-ca_WayPoint67 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint67 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-ca_r0.7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r0.7-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-ca_r2.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r1.5-ca - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 26.169302720038942 - y: -8.551459754026677 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-cb_r1.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-cb_r1.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-cb - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 26.697499999999998 - y: -12.1125 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c0_r1.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c0_r1.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-c0 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 27.150000000000002 - y: -15.075 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c1_r1.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c1_r1.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-c1 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 27.585 - y: -18.022499999999997 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c2_r1.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c2_r1.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-c2 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.020000000000003 - y: -20.985 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c3_r1.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c3_r1.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-c3 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.47 - y: -23.962500000000002 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c4_r1.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c4_r1.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-c4 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.9225 - y: -26.9625 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c5_r1.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-c5_r1.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-c5 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 29.335 - y: -29.7975 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-cy_r1.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-cy_r1.5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-cy - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 29.7075 - y: -32.46 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1.5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1.5-cz_r1.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r1.5-cz - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 30.235697279961055 - y: -36.021040245973325 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-ca_r2.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-ca_WayPoint73 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint73 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-ca_r3.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-ca_r1.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r2.5-ca - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.843474656987834 - y: -8.739082676268534 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-cb_r2.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-cb_r2.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-cb - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 25.3725 - y: -12.299999999999999 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c0_r2.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c0_r2.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-c0 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 25.825000000000003 - y: -15.2725 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c1_r2.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c1_r2.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-c1 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 26.26 - y: -18.22 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c2_r2.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c2_r2.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-c2 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 26.6975 - y: -21.197499999999998 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c3_r2.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c3_r2.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-c3 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 27.1475 - y: -24.1625 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c4_r2.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c4_r2.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-c4 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 27.597499999999997 - y: -27.1375 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c5_r2.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-c5_r2.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-c5 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.01 - y: -29.972500000000004 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-cy_r2.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-cy_r2.5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-cy - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.384999999999998 - y: -32.635000000000005 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r2.5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2.5-cz_r2.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r2.5-cz - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 28.914025343012163 - y: -36.19591732373147 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-ca_r3.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-ca_WayPoint73 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint73 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-ca_r4.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-ca_r2.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r3.5-ca - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 23.548916109785623 - y: -8.926146922803511 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-cb_r3.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-cb_r3.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-cb - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.075000000000003 - y: -12.4875 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c0_r3.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c0_r3.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-c0 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.5125 - y: -15.4725 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c1_r3.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c1_r3.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-c1 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.935 - y: -18.4225 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c2_r3.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c2_r3.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-c2 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 25.3725 - y: -21.3875 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c3_r3.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c3_r3.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-c3 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 25.8225 - y: -24.3375 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c4_r3.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c4_r3.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-c4 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 26.2725 - y: -27.3125 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c5_r3.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-c5_r3.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-c5 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 26.687500000000004 - y: -30.1625 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-cy_r3.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-cy_r3.5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-cy - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 27.0625 - y: -32.824999999999996 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r3.5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3.5-cz_r3.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r3.5-cz - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 27.58858389021438 - y: -36.38635307719649 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-ca_r4.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-ca_WayPoint74 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint74 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-ca_r5.3-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-ca_r3.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r4.5-ca - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 22.24675567404429 - y: -9.118228618319929 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-cb_r4.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-cb_r4.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-cb - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 22.769999999999996 - y: -12.68 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c0_r4.5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c0_r4.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-c0 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 23.205000000000002 - y: -15.669999999999998 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c1_r4.5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c1_r4.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-c1 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 23.625 - y: -18.6175 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c2_r4.5-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c2_r4.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-c2 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.07 - y: -21.57 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c3_r4.5-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c3_r4.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-c3 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.515 - y: -24.5325 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c4_r4.5-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c4_r4.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-c4 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.95 - y: -27.5075 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c5_r4.5-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-c5_r4.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-c5 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 25.36 - y: -30.349999999999998 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-cy_r4.5-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-cy_r4.5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-cy - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 25.749999999999996 - y: -33.025 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r4.5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4.5-cz_r4.5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r4.5-cz - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 26.2732443259557 - y: -36.586771381680066 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-ca - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-ca_r5.3-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-ca_r4.5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4.5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r5.3-ca - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 20.873122296269567 - y: -9.160895302138318 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-cb_r5.3-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-ca - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-cb_r5.3-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-cb - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 21.3975 - y: -12.7225 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c0_r5.3-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-cb - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c0_r5.3-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-c0 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 21.845000000000002 - y: -15.7075 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-c1 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c1_r5.3-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c0 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c1_r5.3-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-c1 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 22.2875 - y: -18.6725 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-c2 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c2_r5.3-c1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c1 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c2_r5.3-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-c2 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 22.72 - y: -21.64 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-c3 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c3_r5.3-c2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c2 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c3_r5.3-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-c3 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 23.1475 - y: -24.6175 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-c4 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c4_r5.3-c3 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c3 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c4_r5.3-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-c4 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 23.5775 - y: -27.580000000000002 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-c5 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c5_r5.3-c4 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c4 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-c5_r5.3-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-c5 - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.005000000000003 - y: -30.490000000000002 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-cy_r5.3-c5 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-c5 - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-cy_r5.3-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-cz - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-cy - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.462500000000002 - y: -33.39 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r5.3-cz - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5.3-cz_r5.3-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5.3-cy - recovery_behaviours_config: '' - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - localise_by_topic: '' - name: r5.3-cz - parent_frame: map - pose: - orientation: - w: 0.7570868080527051 - x: 0.0 - y: 0.0 - z: -0.6533142927202545 - position: - x: 24.986877703730435 - y: -36.95160469786168 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 6.28 - restrictions_planning: robot_short - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.3848983733954697 - y: -0.6405640008159993 - - x: 0.526728899032632 - y: -0.21219254591980724 - - x: 0.44271775959631 - y: 0.3556262011692642 - - x: 0.18292037369360176 - y: 0.7245751402523212 - - x: -0.3848983733954697 - y: 0.6405640008159993 - - x: -0.526728899032632 - y: 0.21219254591980724 - - x: -0.44271775959631 - y: -0.3556262011692642 - - x: -0.18292037369360176 - y: -0.7245751402523212 -- meta: - map: riseholme - node: r1-ca - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-ca_WayPoint67 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint67 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-ca_r1-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-ca_r2-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r1-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 26.871219635 - y: -8.57828330994 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r1-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-c0_r1-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-c0_r1-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r1-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 28.9023780823 - y: -22.3284606934 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r1-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-cz_r2-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-cz_r1-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r1-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 30.894743677585105 - y: -35.94505645705402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r2-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-ca_r3-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-ca_WayPoint73 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint73 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-ca_WayPoint67 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint67 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-ca_r2-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-ca_r1-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r2-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 25.519826889 - y: -8.80274772644 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r2-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-c0_r2-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-c0_r2-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r2-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 27.5617446899 - y: -22.5340423584 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r2-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-cz_r1-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-cz_r3-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-cz_r2-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r2-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 29.598967310385106 - y: -36.14810134845402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r3-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-ca_r4-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-ca_r2-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-ca_WayPoint73 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint73 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-ca_r3-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r3-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 24.2120990753 - y: -9.00120162964 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r3-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-c0_r3-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-c0_r3-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r3-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 26.240644455 - y: -22.7276000977 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: robot_tall - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r3-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-cz_r2-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-cz_r4-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-cz_r3-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r3-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 28.250984903785106 - y: -36.33979751545402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r4-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-ca_r5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-ca_r3-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-ca_WayPoint74 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint74 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-ca_WayPoint73 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint73 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-ca_r4-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r4-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 22.9220657349 - y: -9.20281600952 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r4-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-c0_r4-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-c0_r4-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r4-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 24.954082489 - y: -22.946100235 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r4-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-cz_r3-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-cz_r5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-cz_r4-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r4-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 26.945385691085104 - y: -36.55604125935402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r5-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-ca_r4-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-ca_WayPoint74 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint74 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-ca_r5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r5-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 21.6146430969 - y: -9.41316413879 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r5-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-c0_r5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-c0_r5-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r5-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 23.6351661682 - y: -23.1455612183 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r5-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-cz_r4-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-cz_r5-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r5-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 25.635498758785104 - y: -36.75929977375402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r6-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-ca_r7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-ca_WayPoint66 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint66 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-ca_r6-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r6-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 18.9433765411 - y: -9.84392356873 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r6-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-c0_r6-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-c0_r6-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r6-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 20.937664032 - y: -23.5692234039 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r6-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-cz_r7-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-cz_r6-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r6-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 22.915657755385105 - y: -37.193175811354024 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r7-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-ca_r8-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-ca_r6-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-ca_WayPoint56 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint56 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-ca_WayPoint66 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint66 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-ca_r7-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r7-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 17.6317653656 - y: -10.0511760712 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r7-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-c0_r7-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-c0_r7-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r7-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 19.6318664551 - y: -23.7595119476 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r7-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-cz_r8-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-cz_r6-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-cz_r7-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r7-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 21.636023279585103 - y: -37.37216140705402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r8-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-ca_r9-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-ca_r7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-ca_WayPoint56 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint56 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-ca_r8-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r8-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 16.2781791687 - y: -10.2434005737 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r8-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-c0_r8-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-c0_r8-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r8-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 18.3280582428 - y: -23.942522049 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r8-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-cz_r9-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-cz_r7-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-cz_r8-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r8-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 20.374644037685105 - y: -37.56874801585402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r9-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-ca_r10-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-ca_r8-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-ca_WayPoint63 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint63 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-ca_WayPoint56 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint56 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-ca_r9-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r9-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 14.9655475616 - y: -10.4636011124 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r9-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-c0_r9-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-c0_r9-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r9-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 16.9808979034 - y: -24.1296234131 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r9-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-cz_r10-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-cz_r8-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-cz_r9-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r9-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 18.982176538885106 - y: -37.76452590895402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r10-ca - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-ca_r9-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-ca - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-ca_WayPoint63 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint63 - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-ca_r10-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r10-ca - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 13.6513690948 - y: -10.6481618881 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r10-c0 - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-c0_r10-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-c0_r10-cb - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-cb - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r10-c0 - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 15.6664028168 - y: -24.3333568573 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r10-cz - pointset: riseholme_restrict - node: - edges: - - action: row_change - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-cz_r9-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-cz - recovery_behaviours_config: '' - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-cz_r10-cy - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-cy - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r10-cz - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 17.671026941785104 - y: -37.94117309525402 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r1-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-cb_r1-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-cb_r1-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r1-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 27.8867988586 - y: -15.4533720017 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r1-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-cy_r1-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r1-cy_r1-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r1-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r1-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 29.8985608799 - y: -29.1367585752 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r2-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-cb_r2-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-cb_r2-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r2-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 26.5407857894 - y: -15.6683950424 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r2-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-cy_r2-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r2-cy_r2-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r2-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r2-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 28.5803560001 - y: -29.3410718534 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r3-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-cy_r3-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-cy_r3-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r3-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 27.2458146794 - y: -29.5336988066 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r4-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-cy_r4-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-cy_r4-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r4-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 25.94973409 - y: -29.7510707472 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r5-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-cy_r5-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-cy_r5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r5-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 24.6353324635 - y: -29.952430496 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r3-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-cb_r3-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r3-cb_r3-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r3-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r3-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 25.2263717651 - y: -15.8644008637 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: robot_tall - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r4-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-cb_r4-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r4-cb_r4-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r4-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r4-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 23.938074112 - y: -16.0744581223 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r5-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-cb_r5-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r5-cb_r5-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r5-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r5-cb - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 22.60000000000001 - y: -16.2793626785 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r6-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-cy_r6-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-cy_r6-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r6-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 21.9266608937 - y: -30.3811996076 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r7-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-cy_r7-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-cy_r7-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r7-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 20.6339448673 - y: -30.5658366773 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r8-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-cy_r8-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-cy_r8-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r8-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 19.3513511402 - y: -30.7556350324 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r9-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-cy_r9-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-cy_r9-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r9-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 17.9815372211 - y: -30.947074661 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r10-cy - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-cy_r10-cz - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-cz - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-cy_r10-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r10-cy - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 16.6687148793 - y: -31.1372649763 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r6-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-cb_r6-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r6-cb_r6-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r6-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r6-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 19.9405202866 - y: -16.7065734863 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r7-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-cb_r7-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r7-cb_r7-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r7-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r7-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 18.6318159103 - y: -16.9053440094 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r8-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-cb_r8-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r8-cb_r8-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r8-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r8-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 17.3031187058 - y: -17.0929613114 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r9-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-cb_r9-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r9-cb_r9-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r9-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r9-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 15.9732227325 - y: -17.2966122627 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: r10-cb - pointset: riseholme_restrict - node: - edges: - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-cb_r10-c0 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-c0 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: row_traversal - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: r10-cb_r10-ca - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: r10-ca - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: r10-cb - parent_frame: map - pose: - orientation: - w: 0.756889507045039 - x: 0.0 - y: 0.0 - z: -0.6535428632653851 - position: - x: 14.6588859558 - y: -17.4907593727 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: robot_tall - restrictions_runtime: obstacleFree_1 - verts: - - x: 0.384511440992 - y: -0.640796363354 - - x: 0.526600658894 - y: -0.212510615587 - - x: 0.442932456732 - y: 0.35535877943 - - x: 0.183357924223 - y: 0.724464535713 - - x: -0.384511440992 - y: 0.640796363354 - - x: -0.526600658894 - y: 0.212510615587 - - x: -0.442932456732 - y: -0.35535877943 - - x: -0.183357924223 - y: -0.724464535713 -- meta: - map: riseholme - node: WayPoint144 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint144_WayPoint141 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint141 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint144_WayPoint143 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint143 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: WayPoint144_WayPoint67 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint67 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint144 - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 26.0 - y: -2.8000000000000114 - z: 0.0 - properties: - xy_goal_tolerance: 0.35 - yaw_goal_tolerance: 6.29 - restrictions_planning: 'True' - restrictions_runtime: 'True' - verts: - - x: 0.689999997616 - y: 0.287000000477 - - x: 0.287000000477 - y: 0.689999997616 - - x: -0.287000000477 - y: 0.689999997616 - - x: -0.689999997616 - y: 0.287000000477 - - x: -0.689999997616 - y: -0.287000000477 - - x: -0.287000000477 - y: -0.689999997616 - - x: 0.287000000477 - y: -0.689999997616 - - x: 0.689999997616 - y: -0.287000000477 -- meta: - map: riseholme - node: s0 - pointset: riseholme_restrict - node: - edges: - - action: NavigateToPose - action_type: geometry_msgs/PoseStamped - config: [] - edge_id: s0_WayPoint72 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint72 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: s0 - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 20.650000000000006 - y: 3.799999999999997 - z: 0.0 - properties: - xy_goal_tolerance: 0.35 - yaw_goal_tolerance: 6.29 - restrictions_planning: 'True' - restrictions_runtime: 'True' - verts: - - x: 0.689999997616 - y: 0.287000000477 - - x: 0.287000000477 - y: 0.689999997616 - - x: -0.287000000477 - y: 0.689999997616 - - x: -0.689999997616 - y: 0.287000000477 - - x: -0.689999997616 - y: -0.287000000477 - - x: -0.287000000477 - y: -0.689999997616 - - x: 0.287000000477 - y: -0.689999997616 - - x: 0.689999997616 - y: -0.287000000477 -pointset: riseholme_restrict -transformation: - child: topo_map - parent: map - rotation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - translation: - x: 0.0 - y: 0.0 - z: 0.0 diff --git a/topological_navigation/test/conftest.py b/topological_navigation/test/conftest.py new file mode 100644 index 00000000..99b2bcd2 --- /dev/null +++ b/topological_navigation/test/conftest.py @@ -0,0 +1,28 @@ +""" +Pytest configuration for topological_navigation tests. + +This file ensures that the topological_navigation package can be imported +during testing by adding the source directory to the Python path and +invalidating any stale module cache. +""" + +import sys +from pathlib import Path + +# The ROS package directory containing the actual Python package. +# Layout: topological_navigation/ (ROS pkg) -> topological_navigation/ (python pkg) +_pkg_dir = str(Path(__file__).resolve().parent.parent) + +# Insert BEFORE anything else to win over the outer __init__.py +if _pkg_dir not in sys.path: + sys.path.insert(0, _pkg_dir) + +# Invalidate the root cached module so Python re-discovers from the correct path. +# Only remove the root module – submodules will be imported fresh. +# We must not remove test.conftest itself. +_stale = 'topological_navigation' +if _stale in sys.modules: + _cached_file = getattr(sys.modules[_stale], '__file__', '') or '' + # Only invalidate if the cached module points to the wrong location + if 'topological_navigation/topological_navigation' not in _cached_file: + del sys.modules[_stale] diff --git a/topological_navigation/test/fixtures/README.md b/topological_navigation/test/fixtures/README.md new file mode 100644 index 00000000..8ce1f01f --- /dev/null +++ b/topological_navigation/test/fixtures/README.md @@ -0,0 +1,164 @@ +# Test Fixture YAML Files + +This directory contains topological map YAML fixtures for testing the localisation system. + +## Files + +### 1. simple_map.yaml +**Purpose:** Basic functionality testing with minimal complexity + +**Characteristics:** +- **Nodes:** 2 (WP1, WP2) +- **Edges:** 1 (WP1 → WP2) +- **Polygon shapes:** Square influence zones +- **Features:** + - Basic NavigateToPose action + - Simple node properties (xy_goal_tolerance, yaw_goal_tolerance) + - Edge properties (max_speed) + +**Use cases:** +- Basic graph construction tests +- Simple KD-tree queries +- Basic point-in-polygon checks +- Edge distance calculations with minimal edges + +### 2. complex_map.yaml +**Purpose:** Comprehensive testing with realistic map structure + +**Characteristics:** +- **Nodes:** 10 (Entry, Junction1, Junction2, Row1Start, Row1End, Row2Start, Row2End, Exit, NoGoZone, TopicLocaliseNode) +- **Edges:** Multiple edges forming a connected graph +- **Polygon shapes:** Square influence zones (various sizes) +- **Features:** + - Multiple edge types (NavigateToPose, RowOperation) + - Agricultural navigation patterns (row operations) + - Node tags (entry_point, row_entry, row_exit, exit_point, no_go) + - No-go node (NoGoZone with no_go: true property) + - Topic-based localization (TopicLocaliseNode with localise_by_topic JSON config) + - Roboflow integration properties + - Various node properties and semantics + +**Use cases:** +- Complex graph traversal +- Multiple node filtering (no-go, topic-based) +- Realistic localization scenarios +- Edge action testing +- Tag-based node queries +- Agricultural navigation testing + +### 3. polygon_shapes_map.yaml +**Purpose:** Point-in-polygon algorithm testing with diverse polygon geometries + +**Characteristics:** +- **Nodes:** 10 nodes with different polygon shapes +- **Edges:** None (focused on polygon testing) +- **Polygon shapes:** + 1. **TriangleNode** - Equilateral triangle (3 vertices) + 2. **PentagonNode** - Regular pentagon (5 vertices) + 3. **HexagonNode** - Regular hexagon (6 vertices) + 4. **LShapeNode** - Concave L-shaped polygon (6 vertices) + 5. **IrregularNode** - Asymmetric irregular polygon (5 vertices) + 6. **TinyNode** - Very small square (0.2m x 0.2m) for precision testing + 7. **LargeNode** - Very large square (20m x 20m) for scale testing + 8. **RectangleNode** - Non-square rectangle (6m x 2m) + 9. **EmptyVertsNode** - Empty verts list (edge case) + 10. **StarNode** - Complex concave star shape (10 vertices) + +**Use cases:** +- Point-in-polygon algorithm validation +- Convex polygon testing (triangle, pentagon, hexagon, rectangle) +- Concave polygon testing (L-shape, star) +- Edge case testing (empty verts, tiny polygon, large polygon) +- Irregular polygon testing +- Ray-casting algorithm correctness verification + +## Testing Guidelines + +### Unit Tests +When writing unit tests, use these fixtures as follows: + +1. **simple_map.yaml** - For basic functionality tests where you need minimal complexity +2. **complex_map.yaml** - For integration tests and realistic scenarios +3. **polygon_shapes_map.yaml** - For point-in-polygon specific tests + +### Example Usage + +```python +import yaml +from pathlib import Path + +# Load fixture +fixture_path = Path(__file__).parent / 'fixtures' / 'simple_map.yaml' +with open(fixture_path, 'r') as f: + map_data = yaml.safe_load(f) + +# Use in tests +def test_graph_construction(): + graph = build_graph_from_tmap(map_data) + assert graph.number_of_nodes() == 2 + assert graph.number_of_edges() == 1 +``` + +### Point-in-Polygon Test Cases + +The `polygon_shapes_map.yaml` fixture enables comprehensive testing: + +```python +# Test point inside triangle +pose_inside = create_pose(0.0, 0.5) # Inside TriangleNode +assert point_in_poly_nx(graph, 'TriangleNode', pose_inside) == True + +# Test point outside triangle +pose_outside = create_pose(5.0, 5.0) # Outside TriangleNode +assert point_in_poly_nx(graph, 'TriangleNode', pose_outside) == False + +# Test concave polygon (L-shape) +pose_in_concave = create_pose(0.5, 11.5) # Inside L-shape +assert point_in_poly_nx(graph, 'LShapeNode', pose_in_concave) == True + +# Test empty verts edge case +pose_any = create_pose(20.0, -10.0) +assert point_in_poly_nx(graph, 'EmptyVertsNode', pose_any) == False +``` + +## Maintenance + +When adding new fixtures: +1. Follow the existing YAML structure +2. Use descriptive node names +3. Document the purpose in this README +4. Ensure valid topological map format (schema2.json compliant) +5. Add corresponding test cases + +## Validation + +All fixtures should validate against `topological_navigation/config/schema2.json`. + +To validate manually: +```bash +# Install jsonschema if needed +pip install jsonschema pyyaml + +# Validate fixture +python -c " +import yaml +import json +from jsonschema import validate + +with open('topological_navigation/config/schema2.json') as f: + schema = json.load(f) + +with open('topological_navigation/test/fixtures/simple_map.yaml') as f: + data = yaml.safe_load(f) + +validate(instance=data, schema=schema) +print('Valid!') +" +``` + +## References + +- Requirements: `/home/ibrahim/.kiro/specs/localisation-networkx-refactor/requirements.md` +- Design: `/home/ibrahim/.kiro/specs/localisation-networkx-refactor/design.md` +- Schema: `topological_navigation/config/schema2.json` +- AGENTS.md: Project development guidelines diff --git a/topological_navigation/test/fixtures/complex_map.yaml b/topological_navigation/test/fixtures/complex_map.yaml new file mode 100644 index 00000000..98694c9f --- /dev/null +++ b/topological_navigation/test/fixtures/complex_map.yaml @@ -0,0 +1,426 @@ +meta: + last_updated: 01-01-2026_00-00-00 + origin: + latitude: 0.0 + longitude: 0.0 + altitude: 0.0 + +metric_map: complex_test_map +name: complex_test_map +pointset: complex_test_map + +transformation: + topo_frame_id: complex_test_map + parent: map + rotation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + translation: + x: 0.0 + y: 0.0 + z: 0.0 + +definitions: + default_bt: | + + + + + + + + + row_traversal_bt: | + + + + + + + + + +actions: + navigate_to_pose: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + + row_traversal: + composable: true + action_type: nav2_msgs.action.NavigateThroughPoses + action_server: /navigate_through_poses + action_goal_template: + poses: + - header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.row_traversal_bt}' + +nodes: + - meta: + map: complex_test_map + node: Entry + pointset: complex_test_map + tag: ['entry_point'] + node: + edges: + - action: navigate_to_pose + edge_id: Entry_Junction1 + node: Junction1 + properties: + max_speed: 0.8 + - action: navigate_to_pose + edge_id: Entry_Row1Start + node: Row1Start + name: Entry + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + semantics: entry + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: complex_test_map + node: Junction1 + pointset: complex_test_map + node: + edges: + - action: navigate_to_pose + edge_id: Junction1_Row1Start + node: Row1Start + - action: navigate_to_pose + edge_id: Junction1_Row2Start + node: Row2Start + - action: navigate_to_pose + edge_id: Junction1_Exit + node: Exit + name: Junction1 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 5.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.5 + verts: + - x: -1.5 + y: -1.5 + - x: 1.5 + y: -1.5 + - x: 1.5 + y: 1.5 + - x: -1.5 + y: 1.5 + + - meta: + map: complex_test_map + node: Row1Start + pointset: complex_test_map + tag: ['row_entry'] + node: + edges: + - action: row_traversal + edge_id: Row1Start_Row1End + node: Row1End + properties: + max_speed: 0.3 + row_type: vineyard + roboflow: + enabled: true + confidence: 0.7 + name: Row1Start + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: 0.707 + position: + x: 5.0 + y: 5.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.05 + semantics: row_entry + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: complex_test_map + node: Row1End + pointset: complex_test_map + tag: ['row_exit'] + node: + edges: + - action: navigate_to_pose + edge_id: Row1End_Junction2 + node: Junction2 + name: Row1End + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: 0.707 + position: + x: 15.0 + y: 5.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.2 + semantics: row_exit + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: complex_test_map + node: Row2Start + pointset: complex_test_map + tag: ['row_entry'] + node: + edges: + - action: row_traversal + edge_id: Row2Start_Row2End + node: Row2End + properties: + max_speed: 0.3 + row_type: vineyard + name: Row2Start + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: 0.707 + position: + x: 5.0 + y: -5.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.05 + semantics: row_entry + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: complex_test_map + node: Row2End + pointset: complex_test_map + tag: ['row_exit'] + node: + edges: + - action: navigate_to_pose + edge_id: Row2End_Junction2 + node: Junction2 + name: Row2End + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: 0.707 + position: + x: 15.0 + y: -5.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.2 + semantics: row_exit + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: complex_test_map + node: Junction2 + pointset: complex_test_map + node: + edges: + - action: navigate_to_pose + edge_id: Junction2_Exit + node: Exit + name: Junction2 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 20.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.5 + verts: + - x: -1.5 + y: -1.5 + - x: 1.5 + y: -1.5 + - x: 1.5 + y: 1.5 + - x: -1.5 + y: 1.5 + + - meta: + map: complex_test_map + node: Exit + pointset: complex_test_map + tag: ['exit_point'] + node: + edges: [] + name: Exit + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 25.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + semantics: exit + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: complex_test_map + node: NoGoZone + pointset: complex_test_map + tag: ['no_go'] + node: + edges: [] + name: NoGoZone + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 10.0 + y: 10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + no_go: true + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: complex_test_map + node: TopicLocaliseNode + pointset: complex_test_map + node: + edges: [] + localise_by_topic: '{"topic": "/special_zone", "field": "data", "val": "active", "localise_anywhere": true, "persistency": 5}' + name: TopicLocaliseNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 10.0 + y: -10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 diff --git a/topological_navigation/test/fixtures/mixed_actions_map.yaml b/topological_navigation/test/fixtures/mixed_actions_map.yaml new file mode 100644 index 00000000..4daddfcb --- /dev/null +++ b/topological_navigation/test/fixtures/mixed_actions_map.yaml @@ -0,0 +1,323 @@ +meta: + last_updated: 08-03-2026_12-00-00 + origin: + latitude: 53.268642038 + longitude: -0.524509505881 + altitude: 0 + +metric_map: mixed_test_map +name: mixed_test_map +pointset: mixed_test_map + +# All nodes default to topo_frame_id ("mixed_test_map") +transformation: + topo_frame_id: mixed_test_map + parent: map + rotation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + translation: + x: 0.0 + y: 0.0 + z: 0.0 + +definitions: + default_bt: | + + + + + + + + + goal_align_bt: | + + + + + + + + + row_traversal_bt: | + + + + + + + + + +actions: + navigate_to_pose: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + + navigate_to_poses: + composable: true + action_type: nav2_msgs.action.NavigateThroughPoses + action_server: /navigate_through_poses + action_goal_template: + poses: + - header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + + goal_align: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.goal_align_bt}' + + row_traversal: + composable: true + action_type: nav2_msgs.action.NavigateThroughPoses + action_server: /navigate_through_poses + action_goal_template: + poses: + - header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.row_traversal_bt}' + +nodes: + # N1 -- navigate_to_pose --> N2 -- navigate_to_pose --> N3 + # N3 -- row_traversal --> N4 -- row_traversal --> N5 + # N5 -- goal_align --> N6 + + - meta: + map: mixed_test_map + node: N1 + pointset: mixed_test_map + node: + edges: + - action: navigate_to_pose + edge_id: N1_N2 + node: N2 + name: N1 + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: mixed_test_map + node: N2 + pointset: mixed_test_map + node: + edges: + - action: navigate_to_pose + edge_id: N2_N3 + node: N3 + - action: navigate_to_pose + edge_id: N2_N1 + node: N1 + name: N2 + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 3.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: mixed_test_map + node: N3 + pointset: mixed_test_map + node: + edges: + - action: row_traversal + edge_id: N3_N4 + node: N4 + properties: + boundary_left: 0.4 + boundary_right: 0.6 + - action: navigate_to_pose + edge_id: N3_N2 + node: N2 + name: N3 + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 6.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: mixed_test_map + node: N4 + pointset: mixed_test_map + node: + edges: + - action: row_traversal + edge_id: N4_N5 + node: N5 + properties: + boundary_left: 0.4 + boundary_right: 0.6 + - action: row_traversal + edge_id: N4_N3 + node: N3 + properties: + boundary_left: 0.4 + boundary_right: 0.6 + name: N4 + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 9.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: mixed_test_map + node: N5 + pointset: mixed_test_map + node: + edges: + - action: goal_align + edge_id: N5_N6 + node: N6 + - action: row_traversal + edge_id: N5_N4 + node: N4 + name: N5 + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: 0.707 + position: + x: 12.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.02 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 + + - meta: + map: mixed_test_map + node: N6 + pointset: mixed_test_map + node: + edges: + - action: goal_align + edge_id: N6_N5 + node: N5 + name: N6 + nav_frame: map # override: use 'map' instead of topo_frame_id + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: 0.707 + position: + x: 15.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.02 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 diff --git a/topological_navigation/test/fixtures/polygon_shapes_map.yaml b/topological_navigation/test/fixtures/polygon_shapes_map.yaml new file mode 100644 index 00000000..5d45b4a3 --- /dev/null +++ b/topological_navigation/test/fixtures/polygon_shapes_map.yaml @@ -0,0 +1,372 @@ +meta: + last_updated: 01-01-2026_00-00-00 + origin: + latitude: 0.0 + longitude: 0.0 + altitude: 0.0 + +metric_map: polygon_shapes_test_map +name: polygon_shapes_test_map +pointset: polygon_shapes_test_map + +transformation: + topo_frame_id: polygon_shapes_test_map + parent: map + rotation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + translation: + x: 0.0 + y: 0.0 + z: 0.0 + +definitions: + default_bt: | + + + + + + + + + +actions: + navigate_to_pose: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + +nodes: + # Triangle polygon + - meta: + map: polygon_shapes_test_map + node: TriangleNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: TriangleNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: + - x: 0.0 + y: 2.0 + - x: -1.732 + y: -1.0 + - x: 1.732 + y: -1.0 + + # Pentagon polygon + - meta: + map: polygon_shapes_test_map + node: PentagonNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: PentagonNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 10.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: + - x: 0.0 + y: 2.0 + - x: 1.902 + y: 0.618 + - x: 1.176 + y: -1.618 + - x: -1.176 + y: -1.618 + - x: -1.902 + y: 0.618 + + # Hexagon polygon + - meta: + map: polygon_shapes_test_map + node: HexagonNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: HexagonNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 20.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: + - x: 1.0 + y: 0.0 + - x: 0.5 + y: 0.866 + - x: -0.5 + y: 0.866 + - x: -1.0 + y: 0.0 + - x: -0.5 + y: -0.866 + - x: 0.5 + y: -0.866 + + # L-shaped (concave) polygon + - meta: + map: polygon_shapes_test_map + node: LShapeNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: LShapeNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: 10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: + - x: 0.0 + y: 0.0 + - x: 2.0 + y: 0.0 + - x: 2.0 + y: 1.0 + - x: 1.0 + y: 1.0 + - x: 1.0 + y: 3.0 + - x: 0.0 + y: 3.0 + + # Irregular polygon (asymmetric) + - meta: + map: polygon_shapes_test_map + node: IrregularNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: IrregularNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 10.0 + y: 10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: + - x: 0.0 + y: 0.0 + - x: 3.0 + y: 0.5 + - x: 2.5 + y: 2.0 + - x: 1.0 + y: 2.5 + - x: -0.5 + y: 1.5 + + # Very small polygon (stress test) + - meta: + map: polygon_shapes_test_map + node: TinyNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: TinyNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 20.0 + y: 10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.05 + verts: + - x: -0.1 + y: -0.1 + - x: 0.1 + y: -0.1 + - x: 0.1 + y: 0.1 + - x: -0.1 + y: 0.1 + + # Large polygon (stress test) + - meta: + map: polygon_shapes_test_map + node: LargeNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: LargeNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: -10.0 + z: 0.0 + properties: + xy_goal_tolerance: 2.0 + verts: + - x: -10.0 + y: -10.0 + - x: 10.0 + y: -10.0 + - x: 10.0 + y: 10.0 + - x: -10.0 + y: 10.0 + + # Rectangle (non-square) + - meta: + map: polygon_shapes_test_map + node: RectangleNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: RectangleNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 10.0 + y: -10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: + - x: -3.0 + y: -1.0 + - x: 3.0 + y: -1.0 + - x: 3.0 + y: 1.0 + - x: -3.0 + y: 1.0 + + # Empty verts (edge case for testing) + - meta: + map: polygon_shapes_test_map + node: EmptyVertsNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: EmptyVertsNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 20.0 + y: -10.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: [] + + # Star-shaped polygon (complex concave) + - meta: + map: polygon_shapes_test_map + node: StarNode + pointset: polygon_shapes_test_map + node: + edges: [] + name: StarNode + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 30.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + verts: + - x: 0.0 + y: 2.0 + - x: 0.588 + y: 0.618 + - x: 1.902 + y: 0.618 + - x: 0.951 + y: -0.191 + - x: 1.176 + y: -1.618 + - x: 0.0 + y: -0.809 + - x: -1.176 + y: -1.618 + - x: -0.951 + y: -0.191 + - x: -1.902 + y: 0.618 + - x: -0.588 + y: 0.618 + diff --git a/topological_navigation/test/fixtures/simple_map.yaml b/topological_navigation/test/fixtures/simple_map.yaml new file mode 100644 index 00000000..63dc16b2 --- /dev/null +++ b/topological_navigation/test/fixtures/simple_map.yaml @@ -0,0 +1,113 @@ +meta: + last_updated: 01-01-2026_00-00-00 + origin: + latitude: 0.0 + longitude: 0.0 + altitude: 0.0 + +metric_map: simple_test_map +name: simple_test_map +pointset: simple_test_map + +transformation: + topo_frame_id: simple_test_map + parent: map + rotation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + translation: + x: 0.0 + y: 0.0 + z: 0.0 + +definitions: + default_bt: | + + + + + + + + + +actions: + navigate_to_pose: + composable: false + action_type: nav2_msgs.action.NavigateToPose + action_server: /navigate_to_pose + action_goal_template: + pose: + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + +nodes: + - meta: + map: simple_test_map + node: WP1 + pointset: simple_test_map + node: + edges: + - action: navigate_to_pose + edge_id: WP1_WP2 + node: WP2 + properties: + max_speed: 0.5 + name: WP1 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 1.0 + x: 0.0 + y: 0.0 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.3 + yaw_goal_tolerance: 0.1 + verts: + - x: -1.0 + y: -1.0 + - x: 1.0 + y: -1.0 + - x: 1.0 + y: 1.0 + - x: -1.0 + y: 1.0 + + - meta: + map: simple_test_map + node: WP2 + pointset: simple_test_map + node: + edges: [] + name: WP2 + nav_frame: '${transformation.topo_frame_id}' + pose: + orientation: + w: 0.707 + x: 0.0 + y: 0.0 + z: 0.707 + position: + x: 5.0 + y: 0.0 + z: 0.0 + properties: + xy_goal_tolerance: 0.5 + verts: + - x: -0.5 + y: -0.5 + - x: 0.5 + y: -0.5 + - x: 0.5 + y: 0.5 + - x: -0.5 + y: 0.5 diff --git a/topological_navigation/test/static_transformer.py b/topological_navigation/test/static_transformer.py index dc4bf50f..0b1d141b 100644 --- a/topological_navigation/test/static_transformer.py +++ b/topological_navigation/test/static_transformer.py @@ -10,7 +10,7 @@ def main(): broadcaster = tf2_ros.StaticTransformBroadcaster(node) static_transform_stamped = TransformStamped() - static_transform_stamped.header.frame_id = 'topo_map' + static_transform_stamped.header.frame_id = 'simple_test_map' static_transform_stamped.child_frame_id = 'base_link' static_transform_stamped.transform.translation.x = 0.0 diff --git a/topological_navigation/test/test_convert_tmap.py b/topological_navigation/test/test_convert_tmap.py new file mode 100644 index 00000000..15bafc7e --- /dev/null +++ b/topological_navigation/test/test_convert_tmap.py @@ -0,0 +1,383 @@ +"""Tests for convert_tmap module. + +Covers _normalise_action_type, _map_action_name, _discover_actions, +_build_actions_section, _build_definitions, _convert_node, +_convert_transformation, and convert_tmap. +""" + +import pytest + +from topological_navigation.convert_tmap import ( + _normalise_action_type, + _map_action_name, + _discover_actions, + _build_actions_section, + _build_definitions, + _convert_node, + _convert_transformation, + convert_tmap, + _BUILTIN_BT_DEFS, +) + + +# ---------- _normalise_action_type ------------------------------------ + +class TestNormaliseActionType: + """Tests for slash-to-dot conversion.""" + + def test_slash_to_dot(self): + assert _normalise_action_type("nav2_msgs/action/NavigateToPose") == \ + "nav2_msgs.action.NavigateToPose" + + def test_already_dotted(self): + assert _normalise_action_type("nav2_msgs.action.NavigateToPose") == \ + "nav2_msgs.action.NavigateToPose" + + def test_empty_string(self): + assert _normalise_action_type("") == "" + + +# ---------- _map_action_name ------------------------------------------ + +class TestMapActionName: + """Tests for CamelCase -> snake_case mapping.""" + + def test_known_actions(self): + assert _map_action_name("NavigateToPose") == "navigate_to_pose" + assert _map_action_name("RowOperation") == "row_traversal" + assert _map_action_name("GoalAlign") == "goal_align" + + def test_already_snake_case_known(self): + """Known lowercase forms also map correctly via the dict.""" + assert _map_action_name("RowTraversal") == "row_traversal" + + def test_unknown_camel_case(self): + """Unknown CamelCase names are converted to snake_case.""" + result = _map_action_name("CustomBehavior") + assert result == "custom_behavior" + + def test_unknown_single_word(self): + """Single-word names stay lowercase.""" + assert _map_action_name("stop") == "stop" + + +# ---------- _discover_actions ----------------------------------------- + +class TestDiscoverActions: + """Tests for action discovery from edge data.""" + + def _nodes(self, *actions): + """Build minimal node list with the given edge actions.""" + return [ + { + "node": { + "edges": [ + {"action": a, "action_type": "nav2_msgs/action/NavigateToPose"} + for a in actions + ] + } + } + ] + + def test_discovers_known_action(self): + discovered = _discover_actions(self._nodes("NavigateToPose")) + assert "navigate_to_pose" in discovered + + def test_deduplicates(self): + nodes = self._nodes("NavigateToPose", "NavigateToPose") + discovered = _discover_actions(nodes) + assert len([k for k in discovered if k == "navigate_to_pose"]) == 1 + + def test_multiple_actions(self): + nodes = self._nodes("NavigateToPose", "RowOperation") + discovered = _discover_actions(nodes) + assert "navigate_to_pose" in discovered + assert "row_traversal" in discovered + + def test_empty_nodes(self): + assert _discover_actions([]) == {} + + def test_unknown_action_uses_normalised_edge_type(self): + """Unknown actions fall back to the edge-level action_type.""" + nodes = [{"node": {"edges": [ + {"action": "MyCustomAction", "action_type": "my_pkg/action/Custom"} + ]}}] + disc = _discover_actions(nodes) + assert "my_custom_action" in disc + assert disc["my_custom_action"] == "my_pkg.action.Custom" + + def test_missing_action_type_defaults(self): + """Missing edge action_type defaults to NavigateToPose.""" + nodes = [{"node": {"edges": [{"action": "SomeUnknown"}]}}] + disc = _discover_actions(nodes) + assert disc["some_unknown"] == "nav2_msgs.action.NavigateToPose" + + +# ---------- _build_actions_section ------------------------------------ + +class TestBuildActionsSection: + """Tests for building the top-level actions dict.""" + + def test_structure(self): + discovered = {"navigate_to_pose": "nav2_msgs.action.NavigateToPose"} + actions = _build_actions_section(discovered, _BUILTIN_BT_DEFS) + assert "navigate_to_pose" in actions + entry = actions["navigate_to_pose"] + assert "composable" in entry + assert "action_type" in entry + assert "action_server" in entry + assert "action_goal_template" in entry + + def test_single_pose_action_template(self): + """Single-pose actions get a 'pose' key, not 'poses'.""" + discovered = {"navigate_to_pose": "nav2_msgs.action.NavigateToPose"} + section = _build_actions_section(discovered, _BUILTIN_BT_DEFS) + tpl = section["navigate_to_pose"]["action_goal_template"] + assert "pose" in tpl + assert "poses" not in tpl + + def test_multi_pose_action_template(self): + """Multi-pose actions get a 'poses' key.""" + discovered = {"row_traversal": "nav2_msgs.action.NavigateThroughPoses"} + section = _build_actions_section(discovered, _BUILTIN_BT_DEFS) + tpl = section["row_traversal"]["action_goal_template"] + assert "poses" in tpl + + def test_bt_reference_added(self): + """behavior_tree reference is added when definition exists.""" + discovered = {"navigate_to_pose": "nav2_msgs.action.NavigateToPose"} + section = _build_actions_section(discovered, _BUILTIN_BT_DEFS) + tpl = section["navigate_to_pose"]["action_goal_template"] + assert "behavior_tree" in tpl + + +# ---------- _build_definitions ---------------------------------------- + +class TestBuildDefinitions: + """Tests for building the definitions section.""" + + def test_builtin_definitions_used(self): + discovered = {"navigate_to_pose": "nav2_msgs.action.NavigateToPose"} + defs = _build_definitions(discovered, None, _BUILTIN_BT_DEFS) + assert "default_bt" in defs + + def test_row_traversal_bt_included(self): + discovered = {"row_traversal": "nav2_msgs.action.NavigateThroughPoses"} + defs = _build_definitions(discovered, None, _BUILTIN_BT_DEFS) + assert "row_traversal_bt" in defs + + def test_empty_discovered(self): + defs = _build_definitions({}, None, _BUILTIN_BT_DEFS) + assert isinstance(defs, dict) + + +# ---------- _convert_node --------------------------------------------- + +class TestConvertNode: + """Tests for converting a single node entry.""" + + @pytest.fixture + def old_node(self): + return { + "meta": { + "map": "test", + "node": "A", + "pointset": "test", + "tag": "old_tag", + }, + "node": { + "name": "A", + "pose": {"position": {"x": 1, "y": 2, "z": 0}, + "orientation": {"w": 1, "x": 0, "y": 0, "z": 0}}, + "edges": [ + { + "action": "NavigateToPose", + "action_type": "nav2_msgs/action/NavigateToPose", + "edge_id": "A_B", + "node": "B", + } + ], + "properties": {"xy_goal_tolerance": 0.3}, + "verts": [{"x": -1, "y": -1}, {"x": 1, "y": 1}], + "localise_by_topic": "some_topic", + "parent_frame": "map", + }, + } + + def test_action_type_removed(self, old_node): + """action_type must be removed from converted edges.""" + result = _convert_node(old_node) + for edge in result["node"]["edges"]: + assert "action_type" not in edge + + def test_action_name_mapped(self, old_node): + """Edge action names are converted to snake_case.""" + result = _convert_node(old_node) + assert result["node"]["edges"][0]["action"] == "navigate_to_pose" + + def test_localise_by_topic_removed(self, old_node): + """localise_by_topic is stripped from the node.""" + result = _convert_node(old_node) + assert "localise_by_topic" not in result["node"] + + def test_parent_frame_removed(self, old_node): + """parent_frame is stripped from the node.""" + result = _convert_node(old_node) + assert "parent_frame" not in result["node"] + + def test_tag_removed_from_meta(self, old_node): + """tag is stripped from the meta.""" + result = _convert_node(old_node) + assert "tag" not in result["meta"] + + def test_properties_preserved(self, old_node): + """Node properties are kept as-is.""" + result = _convert_node(old_node) + assert result["node"]["properties"]["xy_goal_tolerance"] == 0.3 + + def test_verts_preserved(self, old_node): + """Influence zone vertices are kept.""" + result = _convert_node(old_node) + assert len(result["node"]["verts"]) == 2 + + def test_core_fields(self, old_node): + """Name and pose are preserved.""" + result = _convert_node(old_node) + assert result["node"]["name"] == "A" + assert "pose" in result["node"] + + +# ---------- _convert_transformation ----------------------------------- + +class TestConvertTransformation: + """Tests for transformation block conversion.""" + + def test_child_renamed(self): + """'child' key becomes 'topo_frame_id'.""" + result = _convert_transformation({"child": "map"}) + assert result["topo_frame_id"] == "map" + assert "child" not in result + + def test_already_new_format(self): + """If topo_frame_id is already there (no child), it's preserved.""" + result = _convert_transformation({"topo_frame_id": "map"}) + assert result["topo_frame_id"] == "map" + + def test_extra_keys_preserved(self): + """Other transformation keys are kept.""" + result = _convert_transformation({ + "child": "map", + "rotation": {"w": 1.0}, + "translation": {"x": 0.0}, + }) + assert "rotation" in result + assert "translation" in result + + +# ---------- convert_tmap (integration) --------------------------------- + +class TestConvertTmap: + """Integration test for full tmap conversion.""" + + @pytest.fixture + def old_map(self): + return { + "meta": {"last_updated": "01-01-2026"}, + "metric_map": "test_map", + "name": "test_map", + "pointset": "test_map", + "transformation": { + "child": "test_map", + "parent": "map", + "rotation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + }, + "nodes": [ + { + "meta": {"map": "test_map", "node": "A", "pointset": "test_map"}, + "node": { + "name": "A", + "pose": {"position": {"x": 0, "y": 0, "z": 0}, + "orientation": {"w": 1, "x": 0, "y": 0, "z": 0}}, + "edges": [ + { + "action": "NavigateToPose", + "action_type": "nav2_msgs/action/NavigateToPose", + "edge_id": "A_B", + "node": "B", + }, + { + "action": "RowOperation", + "action_type": "nav2_msgs/action/NavigateThroughPoses", + "edge_id": "A_C", + "node": "C", + }, + ], + "verts": [{"x": -1, "y": -1}, {"x": 1, "y": 1}], + }, + }, + { + "meta": {"map": "test_map", "node": "B", "pointset": "test_map"}, + "node": { + "name": "B", + "pose": {"position": {"x": 5, "y": 0, "z": 0}, + "orientation": {"w": 1, "x": 0, "y": 0, "z": 0}}, + "edges": [], + }, + }, + { + "meta": {"map": "test_map", "node": "C", "pointset": "test_map"}, + "node": { + "name": "C", + "pose": {"position": {"x": 0, "y": 5, "z": 0}, + "orientation": {"w": 1, "x": 0, "y": 0, "z": 0}}, + "edges": [], + }, + }, + ], + } + + def test_top_level_keys(self, old_map): + """Converted map has expected top-level keys.""" + result = convert_tmap(old_map) + for key in ("meta", "transformation", "definitions", "actions", "nodes"): + assert key in result + + def test_transformation_converted(self, old_map): + """Transformation block has topo_frame_id.""" + result = convert_tmap(old_map) + assert "topo_frame_id" in result["transformation"] + + def test_actions_discovered(self, old_map): + """Both navigate_to_pose and row_traversal should be discovered.""" + result = convert_tmap(old_map) + assert "navigate_to_pose" in result["actions"] + assert "row_traversal" in result["actions"] + + def test_definitions_present(self, old_map): + """Definitions section should contain BT XML.""" + result = convert_tmap(old_map) + assert len(result["definitions"]) > 0 + + def test_node_count_preserved(self, old_map): + """All nodes are carried over.""" + result = convert_tmap(old_map) + assert len(result["nodes"]) == len(old_map["nodes"]) + + def test_edges_have_no_action_type(self, old_map): + """Converted edges must not contain action_type.""" + result = convert_tmap(old_map) + for node_entry in result["nodes"]: + for edge in node_entry["node"]["edges"]: + assert "action_type" not in edge + + def test_edge_actions_are_snake_case(self, old_map): + """Converted edge actions use snake_case.""" + result = convert_tmap(old_map) + edge = result["nodes"][0]["node"]["edges"][0] + assert edge["action"] == "navigate_to_pose" + + def test_empty_nodes(self): + """Converting a map with no nodes still produces valid output.""" + result = convert_tmap({"nodes": [], "transformation": {}}) + assert result["nodes"] == [] diff --git a/topological_navigation/test/test_localisation2.py b/topological_navigation/test/test_localisation2.py new file mode 100644 index 00000000..3a31163f --- /dev/null +++ b/topological_navigation/test/test_localisation2.py @@ -0,0 +1,415 @@ +"""Unit tests for localisation2 module. + +Tests cover the pure-logic parts of TopologicalNavLoc without starting a +full ROS 2 node. The tests mock rclpy so that no actual ROS 2 runtime +is required. + +Tested functionality: +- Import / module syntax +- Static message helpers (_make_string_msg, _make_float32_msg) +- _get_node_tag +- get_edge_distances_to_pose +- _publish_topics (latched vs non-latched) +- _map_callback graph/KD-tree construction +- localise_pose_cb +- _pose_callback throttle logic +""" + +# -- Path fix: the ROS package directory has an __init__.py that shadows +# the inner Python package of the same name. We must ensure the correct +# "topological_navigation" package (the inner one) is found first. +import sys as _sys +from pathlib import Path as _Path + +_src_dir = str(_Path(__file__).resolve().parent.parent) +if _src_dir not in _sys.path: + _sys.path.insert(0, _src_dir) + +# Evict stale cached root package (outer __init__.py) if necessary. +_tn = _sys.modules.get('topological_navigation') +if _tn is not None: + _f = getattr(_tn, '__file__', '') or '' + if 'topological_navigation/topological_navigation' not in _f: + del _sys.modules['topological_navigation'] + +import math +from pathlib import Path +from threading import Lock +from unittest.mock import MagicMock, patch, PropertyMock + +import numpy as np +import pytest +import yaml + +# We need geometry_msgs for Pose objects used in tests +from geometry_msgs.msg import Pose +from std_msgs.msg import String, Float32 + +from topological_navigation.networkx_utils import ( + build_graph_from_tmap, + build_kdtree_from_graph, +) + + +# --------------------------------------------------------------------------- +# Fixtures +# --------------------------------------------------------------------------- + +FIXTURE_DIR = Path(__file__).parent / 'fixtures' + + +@pytest.fixture +def simple_map_data(): + """Load simple 2-node map fixture.""" + with open(FIXTURE_DIR / 'simple_map.yaml') as f: + return yaml.safe_load(f) + + +@pytest.fixture +def complex_map_data(): + """Load complex 10-node map fixture.""" + with open(FIXTURE_DIR / 'complex_map.yaml') as f: + return yaml.safe_load(f) + + +@pytest.fixture +def simple_graph(simple_map_data): + """Build a NetworkX graph from the simple map.""" + return build_graph_from_tmap(simple_map_data) + + +@pytest.fixture +def simple_kdtree(simple_graph): + """Build a KD-tree from the simple graph.""" + return build_kdtree_from_graph(simple_graph) + + +def _make_pose(x=0.0, y=0.0, z=0.0): + """Helper: create a geometry_msgs/Pose at (x, y, z).""" + p = Pose() + p.position.x = float(x) + p.position.y = float(y) + p.position.z = float(z) + p.orientation.w = 1.0 + return p + + +def _make_loc_node(simple_map_data, simple_graph, simple_kdtree): + """Create a TopologicalNavLoc-like object without starting ROS. + + We patch __init__ to avoid rclpy.node.Node initialisation, then + manually set the attributes the methods under test rely on. + """ + from topological_navigation.scripts.localisation2 import TopologicalNavLoc + + with patch.object(TopologicalNavLoc, '__init__', lambda self, *a, **kw: None): + loc = TopologicalNavLoc.__new__(TopologicalNavLoc) + + # Minimal attributes for the methods under test + loc.tmap = simple_map_data + loc._graph = simple_graph + kdtree, names = simple_kdtree + loc._kdtree = kdtree + loc._kdtree_node_names = names + loc.nogos = [] + loc.names_by_topic = [] + loc.loc_by_topic = [] + loc.force_check = True + loc.current_pose = _make_pose() + + # Latched-mode state + loc.only_latched = True + loc.wpstr = "Unknown" + loc.closest_dist = 1e6 - 1 + loc.cnstr = "Unknown" + loc.nodetag = "Unknown" + loc.closest_edge_ids = [] + loc.closest_edge_dists = [] + loc.current_closest_node_name = "" + + # Thread lock (required by get_edge_distances_to_pose) + loc._map_lock = Lock() + + # Mock publishers + loc.wp_pub = MagicMock() + loc.wd_pub = MagicMock() + loc.cn_pub = MagicMock() + loc.ce_pub = MagicMock() + loc.tag_pub = MagicMock() + + # Mock logger + _logger = MagicMock() + loc.get_logger = MagicMock(return_value=_logger) + + return loc + + +# --------------------------------------------------------------------------- +# Test: module imports cleanly +# --------------------------------------------------------------------------- + +class TestImport: + """Verify that the module can be imported without errors.""" + + def test_import_module(self): + import topological_navigation.scripts.localisation2 # noqa: F401 + + def test_import_class(self): + from topological_navigation.scripts.localisation2 import TopologicalNavLoc # noqa: F401 + + def test_import_main(self): + from topological_navigation.scripts.localisation2 import main # noqa: F401 + + +# --------------------------------------------------------------------------- +# Test: static message helpers +# --------------------------------------------------------------------------- + +class TestMessageHelpers: + """Test _make_string_msg and _make_float32_msg.""" + + def test_make_string_msg(self): + from topological_navigation.scripts.localisation2 import TopologicalNavLoc + msg = TopologicalNavLoc._make_string_msg("hello") + assert isinstance(msg, String) + assert msg.data == "hello" + + def test_make_string_msg_empty(self): + from topological_navigation.scripts.localisation2 import TopologicalNavLoc + msg = TopologicalNavLoc._make_string_msg("") + assert msg.data == "" + + def test_make_float32_msg(self): + from topological_navigation.scripts.localisation2 import TopologicalNavLoc + msg = TopologicalNavLoc._make_float32_msg(3.14) + assert isinstance(msg, Float32) + assert abs(msg.data - 3.14) < 1e-6 + + def test_make_float32_msg_zero(self): + from topological_navigation.scripts.localisation2 import TopologicalNavLoc + msg = TopologicalNavLoc._make_float32_msg(0.0) + assert msg.data == 0.0 + + +# --------------------------------------------------------------------------- +# Test: _get_node_tag +# --------------------------------------------------------------------------- + +class TestGetNodeTag: + """Test the _get_node_tag helper.""" + + def test_tag_unknown_when_node_missing(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + assert loc._get_node_tag("nonexistent_node") == 'Unknown' + + def test_tag_unknown_when_no_tag_key(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + # WP1 in simple_map has no 'tag' in meta + assert loc._get_node_tag("WP1") == 'Unknown' + + def test_tag_returned_when_present(self, complex_map_data): + """complex_map Entry node has tag: ['entry_point'].""" + graph = build_graph_from_tmap(complex_map_data) + kdtree = build_kdtree_from_graph(graph) + + from topological_navigation.scripts.localisation2 import TopologicalNavLoc + with patch.object(TopologicalNavLoc, '__init__', lambda self, *a, **kw: None): + loc = TopologicalNavLoc.__new__(TopologicalNavLoc) + loc.tmap = complex_map_data + loc._graph = graph + loc.get_logger = MagicMock(return_value=MagicMock()) + + assert loc._get_node_tag("Entry") == 'entry_point' + + +# --------------------------------------------------------------------------- +# Test: get_edge_distances_to_pose +# --------------------------------------------------------------------------- + +class TestGetEdgeDistancesToPose: + """Test get_edge_distances_to_pose.""" + + def test_returns_results(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + pose = _make_pose(2.5, 1.0) + edge_ids, dists = loc.get_edge_distances_to_pose(pose) + assert len(edge_ids) > 0 + assert len(dists) > 0 + + def test_empty_when_no_graph(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + loc._graph = None + edge_ids, dists = loc.get_edge_distances_to_pose(_make_pose()) + assert edge_ids == [] + assert len(dists) == 0 + + +# --------------------------------------------------------------------------- +# Test: _publish_topics +# --------------------------------------------------------------------------- + +class TestPublishTopics: + """Test the _publish_topics method (latched and non-latched modes).""" + + def test_latched_publishes_on_change(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + loc.only_latched = True + + loc._publish_topics("WP1", 1.5, "WP1", ["e1"], [0.5], "tag1") + + loc.wp_pub.publish.assert_called_once() + loc.wd_pub.publish.assert_called_once() + loc.cn_pub.publish.assert_called_once() + loc.tag_pub.publish.assert_called_once() + + def test_latched_no_publish_when_same(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + loc.only_latched = True + + # First publish sets the state + loc._publish_topics("WP1", 1.5, "WP1", ["e1"], [0.5], "tag1") + loc.wp_pub.reset_mock() + loc.cn_pub.reset_mock() + + # Same values → no publish + loc._publish_topics("WP1", 1.5, "WP1", ["e1"], [0.5], "tag1") + loc.wp_pub.publish.assert_not_called() + loc.cn_pub.publish.assert_not_called() + + def test_non_latched_always_publishes(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + loc.only_latched = False + + loc._publish_topics("WP1", 1.5, "WP1", ["e1"], [0.5]) + loc.wp_pub.publish.assert_called_once() + + # Same values → still publishes + loc.wp_pub.reset_mock() + loc._publish_topics("WP1", 1.5, "WP1", ["e1"], [0.5]) + loc.wp_pub.publish.assert_called_once() + + def test_edge_ids_sorted_when_dists_equal(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + loc.only_latched = False + + loc._publish_topics("WP1", 1.0, "WP1", ["b_edge", "a_edge"], [1.0, 1.0]) + # After the call, stored edge_ids should be sorted + assert loc.closest_edge_ids == ["a_edge", "b_edge"] + + +# --------------------------------------------------------------------------- +# Test: _map_callback +# --------------------------------------------------------------------------- + +class TestMapCallback: + """Test _map_callback builds graph and KD-tree correctly.""" + + def _make_bare_loc(self): + """Create a bare TopologicalNavLoc without ROS init.""" + from topological_navigation.scripts.localisation2 import TopologicalNavLoc + + with patch.object(TopologicalNavLoc, '__init__', lambda self, *a, **kw: None): + loc = TopologicalNavLoc.__new__(TopologicalNavLoc) + + loc.rec_map = False + loc._graph = None + loc._kdtree = None + loc._kdtree_node_names = [] + loc.names_by_topic = [] + loc.nodes_by_topic = [] + loc.loc_by_topic = [] + loc.nogos = [] + loc.with_tags = False + loc._map_lock = Lock() + loc.get_logger = MagicMock(return_value=MagicMock()) + return loc + + def test_builds_graph_and_kdtree(self, simple_map_data): + loc = self._make_bare_loc() + + msg = String() + msg.data = yaml.dump(simple_map_data) + loc._map_callback(msg) + + assert loc.rec_map is True + assert loc._graph is not None + assert loc._graph.number_of_nodes() == 2 + assert loc._kdtree is not None + assert len(loc._kdtree_node_names) == 2 + assert loc.tmap_frame == 'simple_test_map' + + def test_update_replaces_graph(self, simple_map_data): + """Calling _map_callback again should update (not skip).""" + loc = self._make_bare_loc() + + msg = String() + msg.data = yaml.dump(simple_map_data) + loc._map_callback(msg) + assert loc.rec_map is True + old_graph = loc._graph + + # Second call should update, not skip + loc._map_callback(msg) + assert loc.rec_map is True + assert loc._graph is not None + assert loc._graph.number_of_nodes() == 2 + + def test_handles_bad_map_data(self): + loc = self._make_bare_loc() + + msg = String() + msg.data = yaml.dump({'transformation': {'topo_frame_id': 'test'}, 'nodes': []}) + loc._map_callback(msg) + + # Should not set rec_map because graph build returned None (empty nodes) + assert loc.rec_map is False + + +# --------------------------------------------------------------------------- +# Test: localise_pose_cb +# --------------------------------------------------------------------------- + +class TestLocalisePoseCb: + """Test the localise_pose service callback.""" + + def test_returns_nodes(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + + req = MagicMock() + req.pose = _make_pose(0.0, 0.0) + res = MagicMock() + res.current_node = '' + res.closest_node = '' + + result = loc.localise_pose_cb(req, res) + + # At origin, should be inside WP1's influence zone + assert result.current_node != 'none' + assert result.closest_node == 'WP1' + + def test_returns_none_when_no_graph(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + loc._graph = None + + req = MagicMock() + req.pose = _make_pose() + res = MagicMock() + + result = loc.localise_pose_cb(req, res) + assert result.current_node == 'none' + assert result.closest_node == 'none' + + def test_closest_node_far_from_all(self, simple_map_data, simple_graph, simple_kdtree): + loc = _make_loc_node(simple_map_data, simple_graph, simple_kdtree) + + req = MagicMock() + req.pose = _make_pose(100.0, 100.0) + res = MagicMock() + res.current_node = '' + res.closest_node = '' + + result = loc.localise_pose_cb(req, res) + # Should still return a closest node even if far away + assert result.closest_node in ('WP1', 'WP2') + # Should NOT be inside any influence zone + assert result.current_node == 'none' diff --git a/topological_navigation/test/test_navigation_graph.py b/topological_navigation/test/test_navigation_graph.py new file mode 100644 index 00000000..34c6b9dd --- /dev/null +++ b/topological_navigation/test/test_navigation_graph.py @@ -0,0 +1,597 @@ +"""Tests for ``navigation_graph.py``. + +Covers the pure-Python navigation-graph module: +- NavStateMachine transitions (valid and invalid) +- plan_route (A* via NetworkX) +- get_route_edges +- merge_action_segments +- compute_boundary_polygon +- get_route_distance +- ActionSegment dataclass +""" + +import math +import os + +import networkx as nx +import pytest +import yaml + +from topological_navigation.navigation_graph import ( + ACTION_TO_STATE, + ActionSegment, + NavState, + NavStateMachine, + VALID_TRANSITIONS, + compute_boundary_polygon, + get_route_distance, + get_route_edges, + merge_action_segments, + plan_route, +) +from topological_navigation.networkx_utils import build_graph_from_tmap + + +# ===================================================================== +# Fixtures +# ===================================================================== + + +def _make_node(name, x, y, z=0.0, verts=None, props=None): + """Tiny helper to build a graph node attribute dict.""" + return dict( + x=float(x), y=float(y), z=float(z), + name=name, parent_frame='map', + orientation={'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}, + verts=verts or [], properties=props or {}, + ) + + +@pytest.fixture +def simple_graph(): + """Linear graph: A -> B -> C -> D. + + A(0,0) --[Nav]--> B(2,0) --[Row]--> C(4,0) --[Row]--> D(6,0) + """ + G = nx.DiGraph() + G.add_node('A', **_make_node('A', 0, 0)) + G.add_node('B', **_make_node('B', 2, 0)) + G.add_node('C', **_make_node('C', 4, 0)) + G.add_node('D', **_make_node('D', 6, 0)) + + G.add_edge('A', 'B', edge_id='A_B', + action='NavigateToPose', + action_type='nav2_msgs/action/NavigateToPose', + properties={}, weight=1.0) + G.add_edge('B', 'C', edge_id='B_C', + action='row_traversal', + action_type='nav2_msgs/action/NavigateToPose', + properties={'boundary_left': 0.3, 'boundary_right': 0.4}, + weight=1.0) + G.add_edge('C', 'D', edge_id='C_D', + action='row_traversal', + action_type='nav2_msgs/action/NavigateToPose', + properties={}, weight=1.0) + return G + + +@pytest.fixture +def mixed_graph(): + """Graph with all three action types. + + N1 -[Nav]-> N2 -[Nav]-> N3 -[Row]-> N4 -[Row]-> N5 -[Align]-> N6 + """ + G = nx.DiGraph() + positions = { + 'N1': (0, 0), 'N2': (2, 0), 'N3': (4, 0), + 'N4': (6, 0), 'N5': (8, 0), 'N6': (10, 0), + } + for name, (x, y) in positions.items(): + G.add_node(name, **_make_node(name, x, y)) + + edges = [ + ('N1', 'N2', 'NavigateToPose'), + ('N2', 'N3', 'NavigateToPose'), + ('N3', 'N4', 'row_traversal'), + ('N4', 'N5', 'row_traversal'), + ('N5', 'N6', 'goal_align'), + ] + for src, tgt, action in edges: + G.add_edge(src, tgt, + edge_id='%s_%s' % (src, tgt), + action=action, + action_type='nav2_msgs/action/NavigateToPose', + properties={}, weight=1.0) + return G + + +@pytest.fixture +def mixed_actions_tmap(): + """Load the mixed_actions_map.yaml fixture as a dict.""" + here = os.path.dirname(__file__) + path = os.path.join(here, 'fixtures', 'mixed_actions_map.yaml') + with open(path, 'r') as f: + return yaml.safe_load(f) + + +# ===================================================================== +# NavStateMachine tests +# ===================================================================== + + +class TestNavStateMachine: + """Validate state machine transitions.""" + + def test_initial_state_is_idle(self): + sm = NavStateMachine() + assert sm.state == NavState.IDLE + + def test_valid_startup_sequence(self): + sm = NavStateMachine() + assert sm.transition(NavState.WAITING_FOR_MAP) + assert sm.transition(NavState.WAITING_FOR_LOCALISATION) + assert sm.transition(NavState.READY) + assert sm.state == NavState.READY + + def test_planning_to_executing(self): + sm = NavStateMachine() + sm.transition(NavState.WAITING_FOR_MAP) + sm.transition(NavState.WAITING_FOR_LOCALISATION) + sm.transition(NavState.READY) + assert sm.transition(NavState.PLANNING) + assert sm.transition(NavState.EXECUTING_NAVIGATE_TO_POSE) + assert sm.is_executing() + assert not sm.is_terminal() + + def test_execute_to_succeed_to_ready(self): + sm = NavStateMachine() + sm.transition(NavState.WAITING_FOR_MAP) + sm.transition(NavState.WAITING_FOR_LOCALISATION) + sm.transition(NavState.READY) + sm.transition(NavState.PLANNING) + sm.transition(NavState.EXECUTING_NAVIGATE_TO_POSE) + assert sm.transition(NavState.SUCCEEDED) + assert sm.is_terminal() + assert sm.reset() + assert sm.state == NavState.READY + + def test_execute_to_failed(self): + sm = NavStateMachine() + sm.transition(NavState.WAITING_FOR_MAP) + sm.transition(NavState.WAITING_FOR_LOCALISATION) + sm.transition(NavState.READY) + sm.transition(NavState.PLANNING) + sm.transition(NavState.EXECUTING_ROW_TRAVERSAL) + assert sm.transition(NavState.FAILED) + assert sm.is_terminal() + + def test_execute_to_cancelled(self): + sm = NavStateMachine() + sm.transition(NavState.WAITING_FOR_MAP) + sm.transition(NavState.WAITING_FOR_LOCALISATION) + sm.transition(NavState.READY) + sm.transition(NavState.PLANNING) + sm.transition(NavState.EXECUTING_GOAL_ALIGN) + assert sm.transition(NavState.CANCELLED) + assert sm.is_terminal() + + def test_execute_to_different_execute(self): + """Can transition directly between executing states (recovery/segment switch).""" + sm = NavStateMachine() + sm.transition(NavState.WAITING_FOR_MAP) + sm.transition(NavState.WAITING_FOR_LOCALISATION) + sm.transition(NavState.READY) + sm.transition(NavState.PLANNING) + sm.transition(NavState.EXECUTING_ROW_TRAVERSAL) + assert sm.transition(NavState.EXECUTING_NAVIGATE_TO_POSE) + + def test_invalid_transition_returns_false(self): + sm = NavStateMachine() + # IDLE -> READY is invalid (must go via WAITING_FOR_*) + assert not sm.transition(NavState.READY) + assert sm.state == NavState.IDLE + + def test_invalid_skip_waiting(self): + sm = NavStateMachine() + sm.transition(NavState.WAITING_FOR_MAP) + # Cannot skip WAITING_FOR_LOCALISATION + assert not sm.transition(NavState.READY) + + def test_switch_between_executing_states(self): + """Can move directly between executing states (segment switch).""" + sm = NavStateMachine() + sm.transition(NavState.WAITING_FOR_MAP) + sm.transition(NavState.WAITING_FOR_LOCALISATION) + sm.transition(NavState.READY) + sm.transition(NavState.PLANNING) + sm.transition(NavState.EXECUTING_NAVIGATE_TO_POSE) + assert sm.transition(NavState.EXECUTING_ROW_TRAVERSAL) + assert sm.transition(NavState.EXECUTING_GOAL_ALIGN) + assert sm.transition(NavState.EXECUTING_NAVIGATE_TO_POSE) + + def test_all_action_to_state_mappings_exist(self): + for action, state in ACTION_TO_STATE.items(): + assert isinstance(state, NavState) + assert 'EXECUTING' in state.value + + def test_all_states_have_transitions_defined(self): + for state in NavState: + assert state in VALID_TRANSITIONS + + +# ===================================================================== +# plan_route tests +# ===================================================================== + + +class TestPlanRoute: + """Route planning via NetworkX A*.""" + + def test_direct_route(self, simple_graph): + route = plan_route(simple_graph, 'A', 'D') + assert route == ['A', 'B', 'C', 'D'] + + def test_partial_route(self, simple_graph): + route = plan_route(simple_graph, 'B', 'D') + assert route == ['B', 'C', 'D'] + + def test_same_origin_target(self, simple_graph): + route = plan_route(simple_graph, 'A', 'A') + assert route == ['A'] + + def test_no_route_reverse(self, simple_graph): + """No backward edges -> no route D to A.""" + route = plan_route(simple_graph, 'D', 'A') + assert route is None + + def test_unknown_origin(self, simple_graph): + route = plan_route(simple_graph, 'X', 'D') + assert route is None + + def test_unknown_target(self, simple_graph): + route = plan_route(simple_graph, 'A', 'X') + assert route is None + + def test_avoid_edges(self, simple_graph): + """With A->B avoided, add alternative A->C to enable a bypass.""" + simple_graph.add_edge('A', 'C', edge_id='A_C', + action='NavigateToPose', properties={}, + weight=10.0) + route = plan_route(simple_graph, 'A', 'D', avoid_edges=['A_B']) + assert route is not None + assert route[0] == 'A' + # Must bypass B via the new A->C edge + assert 'C' in route + + def test_avoid_all_paths(self, simple_graph): + """If the only path's edges are all avoided, return None.""" + route = plan_route( + simple_graph, 'A', 'D', + avoid_edges=['A_B'], + ) + assert route is None + + def test_weighted_preference(self): + """A* should prefer the cheaper route.""" + G = nx.DiGraph() + G.add_node('S', **_make_node('S', 0, 0)) + G.add_node('M', **_make_node('M', 5, 0)) + G.add_node('T', **_make_node('T', 10, 0)) + G.add_node('D', **_make_node('D', 5, 5)) + G.add_edge('S', 'M', edge_id='SM', action='NavigateToPose', + properties={}, weight=1.0) + G.add_edge('M', 'T', edge_id='MT', action='NavigateToPose', + properties={}, weight=1.0) + G.add_edge('S', 'D', edge_id='SD', action='NavigateToPose', + properties={}, weight=100.0) + G.add_edge('D', 'T', edge_id='DT', action='NavigateToPose', + properties={}, weight=100.0) + route = plan_route(G, 'S', 'T') + assert route == ['S', 'M', 'T'] + + +# ===================================================================== +# get_route_edges tests +# ===================================================================== + + +class TestGetRouteEdges: + """Extract edge data from route node list.""" + + def test_full_route(self, simple_graph): + edges = get_route_edges(simple_graph, ['A', 'B', 'C', 'D']) + assert len(edges) == 3 + assert edges[0]['edge_id'] == 'A_B' + assert edges[0]['source'] == 'A' + assert edges[0]['target'] == 'B' + assert edges[2]['edge_id'] == 'C_D' + + def test_single_edge(self, simple_graph): + edges = get_route_edges(simple_graph, ['A', 'B']) + assert len(edges) == 1 + assert edges[0]['action'] == 'NavigateToPose' + + def test_empty_route(self, simple_graph): + assert get_route_edges(simple_graph, []) == [] + + def test_single_node(self, simple_graph): + assert get_route_edges(simple_graph, ['A']) == [] + + def test_actions_preserved(self, simple_graph): + edges = get_route_edges(simple_graph, ['A', 'B', 'C']) + assert edges[0]['action'] == 'NavigateToPose' + assert edges[1]['action'] == 'row_traversal' + + +# ===================================================================== +# merge_action_segments tests +# ===================================================================== + + +class TestMergeActionSegments: + """Merge consecutive same-action edges into segments.""" + + def test_mixed_actions(self, mixed_graph): + route = ['N1', 'N2', 'N3', 'N4', 'N5', 'N6'] + edges = get_route_edges(mixed_graph, route) + segments = merge_action_segments(edges) + + assert len(segments) == 3 + + # NavigateToPose x2 + assert segments[0].action_type == 'NavigateToPose' + assert segments[0].num_edges == 2 + assert segments[0].first_source == 'N1' + assert segments[0].last_target == 'N3' + + # row_traversal x2 + assert segments[1].action_type == 'row_traversal' + assert segments[1].num_edges == 2 + assert segments[1].first_source == 'N3' + assert segments[1].last_target == 'N5' + + # goal_align x1 + assert segments[2].action_type == 'goal_align' + assert segments[2].num_edges == 1 + assert segments[2].first_source == 'N5' + assert segments[2].last_target == 'N6' + + def test_single_action_type(self, simple_graph): + edges = get_route_edges(simple_graph, ['B', 'C', 'D']) + segments = merge_action_segments(edges) + assert len(segments) == 1 + assert segments[0].action_type == 'row_traversal' + assert segments[0].num_edges == 2 + + def test_empty_edges(self): + assert merge_action_segments([]) == [] + + def test_alternating_never_merges(self): + edges = [ + {'edge_id': 'e1', 'source': 'A', 'target': 'B', + 'action': 'NavigateToPose', 'properties': {}}, + {'edge_id': 'e2', 'source': 'B', 'target': 'C', + 'action': 'row_traversal', 'properties': {}}, + {'edge_id': 'e3', 'source': 'C', 'target': 'D', + 'action': 'NavigateToPose', 'properties': {}}, + ] + segments = merge_action_segments(edges) + assert len(segments) == 3 + assert all(s.num_edges == 1 for s in segments) + + def test_all_same_type_single_segment(self): + edges = [ + {'edge_id': 'e%d' % i, 'source': 'n%d' % i, + 'target': 'n%d' % (i + 1), 'action': 'goal_align', + 'properties': {}} + for i in range(5) + ] + segments = merge_action_segments(edges) + assert len(segments) == 1 + assert segments[0].num_edges == 5 + + def test_edge_ids_preserved(self, mixed_graph): + edges = get_route_edges(mixed_graph, ['N1', 'N2', 'N3']) + segments = merge_action_segments(edges) + assert segments[0].edge_ids == ['N1_N2', 'N2_N3'] + + +# ===================================================================== +# compute_boundary_polygon tests +# ===================================================================== + + +class TestComputeRowBoundary: + """Compute corridor polygons for RowTraversal segments.""" + + def test_single_edge_horizontal(self, simple_graph): + """B(2,0) -> C(4,0): horizontal edge, polygon should be a + rectangle centred along the x-axis.""" + edges = get_route_edges(simple_graph, ['B', 'C']) + segments = merge_action_segments(edges) + assert len(segments) == 1 + + poly = compute_boundary_polygon( + simple_graph, segments[0], + default_left=0.5, default_right=0.5, + ) + # Rectangle: 2 left + 2 right = 4 points + assert len(poly) == 4 + + # Left points (first half) should be above y=0 + left_y = [p[1] for p in poly[:2]] + assert all(y > 0 for y in left_y) + + # Right points (second half) should be below y=0 + right_y = [p[1] for p in poly[2:]] + assert all(y < 0 for y in right_y) + + def test_custom_distances_from_properties(self, simple_graph): + """B->C edge has boundary_left=0.3, boundary_right=0.4.""" + edges = get_route_edges(simple_graph, ['B', 'C']) + segments = merge_action_segments(edges) + + poly = compute_boundary_polygon(simple_graph, segments[0]) + + # Left offset should be 0.3 (y = +0.3 for horizontal edge) + assert abs(poly[0][1] - 0.3) < 0.01 + # Right offset should be -0.4 + assert abs(poly[3][1] + 0.4) < 0.01 + + def test_merged_edges_more_points(self, simple_graph): + """B->C->D: two edges merged -> 3 waypoints -> 6 points.""" + edges = get_route_edges(simple_graph, ['B', 'C', 'D']) + segments = merge_action_segments(edges) + + poly = compute_boundary_polygon( + simple_graph, segments[0], + default_left=0.5, default_right=0.5, + ) + assert len(poly) == 6 + + def test_empty_segment_returns_empty(self, simple_graph): + segment = ActionSegment(action_type='row_traversal') + poly = compute_boundary_polygon(simple_graph, segment) + assert poly == [] + + def test_polygon_is_closed_corridor(self, simple_graph): + """All left points should be on one side, right on the other.""" + edges = get_route_edges(simple_graph, ['B', 'C', 'D']) + segments = merge_action_segments(edges) + poly = compute_boundary_polygon( + simple_graph, segments[0], + default_left=1.0, default_right=1.0, + ) + # For horizontal edges, left = positive y, right = negative y + n_wp = 3 # three waypoints + left_pts = poly[:n_wp] + right_pts = poly[n_wp:] + assert all(p[1] > 0 for p in left_pts) + assert all(p[1] < 0 for p in right_pts) + + def test_diagonal_edge(self): + """45-degree edge: verify perpendicular offset direction.""" + G = nx.DiGraph() + G.add_node('P', **_make_node('P', 0, 0)) + G.add_node('Q', **_make_node('Q', 1, 1)) + G.add_edge('P', 'Q', edge_id='PQ', action='row_traversal', + properties={}, weight=1.0) + + edges = get_route_edges(G, ['P', 'Q']) + segments = merge_action_segments(edges) + poly = compute_boundary_polygon( + G, segments[0], default_left=1.0, default_right=1.0, + ) + assert len(poly) == 4 + + # Verify the width: distance from left to right at each + # waypoint should be left_dist + right_dist = 2.0 + for i in range(2): + lx, ly = poly[i] + rx, ry = poly[3 - i] + dist = math.hypot(rx - lx, ry - ly) + assert abs(dist - 2.0) < 0.01 + + +# ===================================================================== +# get_route_distance tests +# ===================================================================== + + +class TestGetRouteDistance: + """Total Euclidean distance along a route.""" + + def test_full_route(self, simple_graph): + dist = get_route_distance(simple_graph, ['A', 'B', 'C', 'D']) + assert abs(dist - 6.0) < 0.01 # 2 + 2 + 2 + + def test_single_edge(self, simple_graph): + dist = get_route_distance(simple_graph, ['A', 'B']) + assert abs(dist - 2.0) < 0.01 + + def test_single_node(self, simple_graph): + assert get_route_distance(simple_graph, ['A']) == 0.0 + + def test_empty_route(self, simple_graph): + assert get_route_distance(simple_graph, []) == 0.0 + + +# ===================================================================== +# ActionSegment dataclass tests +# ===================================================================== + + +class TestActionSegment: + """Properties and edge case handling.""" + + def test_empty_segment(self): + seg = ActionSegment(action_type='NavigateToPose') + assert seg.is_empty + assert seg.first_source is None + assert seg.last_target is None + assert seg.num_edges == 0 + + def test_populated_segment(self): + seg = ActionSegment( + action_type='row_traversal', + edge_ids=['e1', 'e2'], + source_nodes=['A', 'B'], + target_nodes=['B', 'C'], + edge_data=[{'edge_id': 'e1'}, {'edge_id': 'e2'}], + ) + assert not seg.is_empty + assert seg.first_source == 'A' + assert seg.last_target == 'C' + assert seg.num_edges == 2 + + +# ===================================================================== +# Integration: build_graph_from_tmap + plan_route +# ===================================================================== + + +class TestIntegrationWithBuildGraph: + """End-to-end: YAML fixture -> NetworkX graph -> route planning.""" + + def test_build_and_plan_route(self, mixed_actions_tmap): + graph = build_graph_from_tmap(mixed_actions_tmap) + assert graph is not None + assert graph.number_of_nodes() == 6 + + route = plan_route(graph, 'N1', 'N6') + assert route is not None + assert route[0] == 'N1' + assert route[-1] == 'N6' + + def test_merge_from_yaml(self, mixed_actions_tmap): + graph = build_graph_from_tmap(mixed_actions_tmap) + route = plan_route(graph, 'N1', 'N6') + edges = get_route_edges(graph, route) + segments = merge_action_segments(edges) + + # Expect 3 segments: Nav(2), Row(2), Align(1) + assert len(segments) == 3 + types = [s.action_type for s in segments] + assert types == ['NavigateToPose', 'row_traversal', 'goal_align'] + + def test_boundary_from_yaml_props(self, mixed_actions_tmap): + """The YAML defines boundary_left=0.4 and boundary_right=0.6 + on the first row_traversal edge (N3->N4).""" + graph = build_graph_from_tmap(mixed_actions_tmap) + route = plan_route(graph, 'N1', 'N6') + edges = get_route_edges(graph, route) + segments = merge_action_segments(edges) + + row_seg = [s for s in segments + if s.action_type == 'row_traversal'][0] + poly = compute_boundary_polygon(graph, row_seg) + + # 3 waypoints (N3, N4, N5) -> 6 polygon points + assert len(poly) == 6 + + # Verify asymmetric boundary (left=0.4, right=0.6) + # For horizontal edges: left_pts have positive y, right negative y + left_y = poly[0][1] + right_y = poly[5][1] + assert abs(left_y - 0.4) < 0.01 + assert abs(right_y + 0.6) < 0.01 diff --git a/topological_navigation/test/test_navigationcore.py b/topological_navigation/test/test_navigationcore.py index f747d28d..e0dde16b 100644 --- a/topological_navigation/test/test_navigationcore.py +++ b/topological_navigation/test/test_navigationcore.py @@ -8,6 +8,7 @@ import pytest import rclpy from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy from std_msgs.msg import String @launch_pytest.fixture @@ -23,18 +24,11 @@ def generate_test_description(): ), launch_ros.actions.Node( executable=sys.executable, - arguments=[str(path_to_test / '..' / 'topological_navigation'/'scripts' /'map_manager2.py'), str(path_to_test / 'conf' / 'network_autogen.tmap2.yaml')], + arguments=[str(path_to_test / '..' / 'topological_navigation'/'scripts' /'map_manager2.py'), str(path_to_test / 'fixtures' / 'simple_map.yaml')], additional_env={'PYTHONUNBUFFERED': '1'}, name='map_manager', output='screen', ), - launch_ros.actions.Node( - executable=sys.executable, - arguments=[str(path_to_test / '..' / 'topological_navigation'/'scripts'/'topological_transform_publisher.py')], - additional_env={'PYTHONUNBUFFERED': '1'}, - name='topological_transform_publisher', - output='screen', - ), launch_ros.actions.Node( executable=sys.executable, arguments=[str(path_to_test / '..' / 'topological_navigation'/ 'scripts'/'navigation2.py')], @@ -61,7 +55,10 @@ def test_check_if_topological_map_is_published(): msgs_received_flag = node.topmap_event_object.wait(timeout=40.0) assert msgs_received_flag, 'Did not receive topological map !' finally: - rclpy.shutdown() + try: + rclpy.shutdown() + except (RuntimeError, rclpy._rclpy_pybind11.RCLError): + pass @pytest.mark.launch(fixture=generate_test_description) def test_check_if_closest_node_is_published(): @@ -72,7 +69,10 @@ def test_check_if_closest_node_is_published(): msgs_received_flag = node.closest_node_event_object.wait(timeout=40.0) assert msgs_received_flag, 'Did not receive closest node info !' finally: - rclpy.shutdown() + try: + rclpy.shutdown() + except (RuntimeError, rclpy._rclpy_pybind11.RCLError): + pass @pytest.mark.launch(fixture=generate_test_description) def test_check_if_current_node_is_published(): @@ -83,7 +83,10 @@ def test_check_if_current_node_is_published(): msgs_received_flag = node.current_node_event_object.wait(timeout=40.0) assert msgs_received_flag, 'Did not receive current node info !' finally: - rclpy.shutdown() + try: + rclpy.shutdown() + except (RuntimeError, rclpy._rclpy_pybind11.RCLError): + pass class NavigationClient(Node): def __init__(self, name='test_node'): @@ -93,9 +96,21 @@ def __init__(self, name='test_node'): self.current_node_event_object = Event() def initialize(self): - self.topmap_sub = self.create_subscription(String, '/topological_map_2', self.topmap_sub_callback, 1) - self.closest_node_sub = self.create_subscription(String, 'closest_node', self.closest_node_callback, 1) - self.current_node_sub = self.create_subscription(String, 'current_node', self.current_node_callback, 1) + # Match the TRANSIENT_LOCAL durability used by the publishers + # (map_manager2, localisation2) so late-joining subscribers + # receive the last published message. + latched_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + self.topmap_sub = self.create_subscription( + String, '/topological_map_2', self.topmap_sub_callback, latched_qos) + self.closest_node_sub = self.create_subscription( + String, 'closest_node', self.closest_node_callback, latched_qos) + self.current_node_sub = self.create_subscription( + String, 'current_node', self.current_node_callback, latched_qos) self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) self.ros_spin_thread.start() diff --git a/topological_navigation/test/test_networkx_utils.py b/topological_navigation/test/test_networkx_utils.py new file mode 100644 index 00000000..98015fde --- /dev/null +++ b/topological_navigation/test/test_networkx_utils.py @@ -0,0 +1,1252 @@ +""" +Unit tests for networkx_utils module. + +Tests cover: +- Graph construction from topological map data +- KD-tree spatial indexing +- Point-in-polygon checks +- Edge distance calculations +- Localization logic +""" + +import pytest +import networkx as nx +import yaml +from pathlib import Path + +# Import the module to test +from topological_navigation.networkx_utils import build_graph_from_tmap + +# geometry_msgs is only available in a ROS 2 environment +try: + import geometry_msgs # noqa: F401 + _has_geometry_msgs = True +except ImportError: + _has_geometry_msgs = False + +_skip_no_geometry = pytest.mark.skipif( + not _has_geometry_msgs, reason='geometry_msgs not available' +) + + +# --------------------------------------------------------------------------- +# Module-level fixtures (available to all test classes) +# --------------------------------------------------------------------------- + +FIXTURE_DIR = Path(__file__).parent / 'fixtures' + + +@pytest.fixture +def simple_map_data(): + """Load simple map fixture.""" + with open(FIXTURE_DIR / 'simple_map.yaml', 'r') as f: + return yaml.safe_load(f) + + +@pytest.fixture +def complex_map_data(): + """Load complex map fixture.""" + with open(FIXTURE_DIR / 'complex_map.yaml', 'r') as f: + return yaml.safe_load(f) + + +class TestGraphConstruction: + """Tests for build_graph_from_tmap function.""" + + def test_simple_map_conversion(self, simple_map_data): + """Test conversion of simple map (2 nodes).""" + graph = build_graph_from_tmap(simple_map_data) + + assert graph is not None + assert isinstance(graph, nx.DiGraph) + assert graph.number_of_nodes() == 2 + assert graph.number_of_edges() == 1 + + # Check node names + assert 'WP1' in graph.nodes + assert 'WP2' in graph.nodes + + def test_complex_map_conversion(self, complex_map_data): + """Test conversion of complex map (10 nodes).""" + graph = build_graph_from_tmap(complex_map_data) + + assert graph is not None + assert isinstance(graph, nx.DiGraph) + assert graph.number_of_nodes() == 10 + assert graph.number_of_edges() > 0 + + # Check some node names + assert 'Entry' in graph.nodes + assert 'Exit' in graph.nodes + assert 'Junction1' in graph.nodes + + def test_node_attribute_preservation(self, simple_map_data): + """Test that node attributes are preserved correctly.""" + graph = build_graph_from_tmap(simple_map_data) + + # Check WP1 attributes + wp1_attrs = graph.nodes['WP1'] + assert wp1_attrs['name'] == 'WP1' + assert wp1_attrs['x'] == 0.0 + assert wp1_attrs['y'] == 0.0 + assert wp1_attrs['z'] == 0.0 + assert 'orientation' in wp1_attrs + assert wp1_attrs['orientation']['w'] == 1.0 + assert 'verts' in wp1_attrs + assert len(wp1_attrs['verts']) == 4 + assert wp1_attrs['parent_frame'] == 'map' + + # Check WP2 attributes + wp2_attrs = graph.nodes['WP2'] + assert wp2_attrs['x'] == 5.0 + assert wp2_attrs['y'] == 0.0 + + def test_edge_attribute_preservation(self, simple_map_data): + """Test that edge attributes are preserved correctly.""" + graph = build_graph_from_tmap(simple_map_data) + + # Check edge from WP1 to WP2 + assert graph.has_edge('WP1', 'WP2') + edge_attrs = graph.edges['WP1', 'WP2'] + + assert edge_attrs['edge_id'] == 'WP1_WP2' + assert edge_attrs['action'] == 'navigate_to_pose' + assert 'properties' in edge_attrs + assert 'weight' in edge_attrs + + def test_optional_properties_handling(self, complex_map_data): + """Test handling of optional properties dictionaries.""" + graph = build_graph_from_tmap(complex_map_data) + + # Check node with properties + entry_attrs = graph.nodes['Entry'] + assert 'properties' in entry_attrs + assert entry_attrs['properties']['xy_goal_tolerance'] == 0.3 + assert entry_attrs['properties']['semantics'] == 'entry' + + # Check edge with properties + edge_attrs = graph.edges['Row1Start', 'Row1End'] + assert 'properties' in edge_attrs + assert edge_attrs['properties']['max_speed'] == 0.3 + assert edge_attrs['properties']['row_type'] == 'vineyard' + + def test_missing_field_handling(self): + """Test handling of missing required fields.""" + # Map with missing 'nodes' field + invalid_map1 = {'name': 'test_map'} + assert build_graph_from_tmap(invalid_map1) is None + + # Map with invalid nodes structure + invalid_map2 = {'nodes': 'not_a_list'} + assert build_graph_from_tmap(invalid_map2) is None + + # Map with node missing 'name' -- only invalid node means 0 valid, + # so build_graph_from_tmap returns None. + invalid_map3 = { + 'nodes': [{ + 'node': { + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + }] + } + graph = build_graph_from_tmap(invalid_map3) + assert graph is None # No valid nodes -> returns None + + def test_invalid_data_handling(self): + """Test handling of completely invalid data.""" + # None input + assert build_graph_from_tmap(None) is None + + # String input + assert build_graph_from_tmap("invalid") is None + + # Empty dict + assert build_graph_from_tmap({}) is None + + def test_empty_map(self): + """Test handling of empty map (no nodes).""" + empty_map = {'nodes': []} + graph = build_graph_from_tmap(empty_map) + + # Empty nodes list -> 0 valid nodes -> returns None + assert graph is None + + def test_node_with_no_edges(self, simple_map_data): + """Test node with no outgoing edges.""" + graph = build_graph_from_tmap(simple_map_data) + + # WP2 has no edges + assert graph.out_degree('WP2') == 0 + assert list(graph.successors('WP2')) == [] + + def test_localise_by_topic_preservation(self, complex_map_data): + """Test that localise_by_topic configuration is preserved.""" + graph = build_graph_from_tmap(complex_map_data) + + # Check node with localise_by_topic + topic_node_attrs = graph.nodes['TopicLocaliseNode'] + assert 'localise_by_topic' in topic_node_attrs + assert topic_node_attrs['localise_by_topic'] != '' + assert '"topic"' in topic_node_attrs['localise_by_topic'] + + # Check node without localise_by_topic + entry_attrs = graph.nodes['Entry'] + assert entry_attrs['localise_by_topic'] == '' + + def test_meta_preservation(self, complex_map_data): + """Test that meta information is preserved.""" + graph = build_graph_from_tmap(complex_map_data) + + # Check node with tags + entry_attrs = graph.nodes['Entry'] + assert 'meta' in entry_attrs + assert 'tag' in entry_attrs['meta'] + assert 'entry_point' in entry_attrs['meta']['tag'] + + def test_edge_weight_default(self, simple_map_data): + """Test that edge weight defaults to 1.0.""" + graph = build_graph_from_tmap(simple_map_data) + + edge_attrs = graph.edges['WP1', 'WP2'] + assert edge_attrs['weight'] == 1.0 + + def test_graph_is_directed(self, simple_map_data): + """Test that returned graph is a DiGraph.""" + graph = build_graph_from_tmap(simple_map_data) + + assert isinstance(graph, nx.DiGraph) + assert graph.is_directed() + + def test_multiple_edges_from_node(self, complex_map_data): + """Test node with multiple outgoing edges.""" + graph = build_graph_from_tmap(complex_map_data) + + # Entry node has 2 edges + assert graph.out_degree('Entry') == 2 + successors = list(graph.successors('Entry')) + assert 'Junction1' in successors + assert 'Row1Start' in successors + + def test_self_loop_edge_handling(self): + """Test handling of self-loop edges (should be added to graph).""" + map_with_self_loop = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + }, + 'edges': [{ + 'edge_id': 'WP1_WP1', + 'node': 'WP1', + 'action': 'Wait' + }] + } + }] + } + + graph = build_graph_from_tmap(map_with_self_loop) + assert graph is not None + assert graph.has_edge('WP1', 'WP1') # Self-loop should be added + + +class TestErrorHandlingWithLogging: + """Tests for error handling with logging.""" + + class MockLogger: + """Mock logger for testing.""" + def __init__(self): + self.errors = [] + self.warnings = [] + + def error(self, msg): + self.errors.append(msg) + + def warning(self, msg): + self.warnings.append(msg) + + def test_invalid_map_type_logs_error(self): + """Test that invalid map type logs appropriate error.""" + logger = self.MockLogger() + result = build_graph_from_tmap("not a dict", logger=logger) + + assert result is None + assert len(logger.errors) == 1 + assert "Expected dictionary" in logger.errors[0] + + def test_missing_nodes_field_logs_error(self): + """Test that missing 'nodes' field logs error.""" + logger = self.MockLogger() + result = build_graph_from_tmap({'name': 'test'}, logger=logger) + + assert result is None + assert len(logger.errors) == 1 + assert "Missing required 'nodes' field" in logger.errors[0] + + def test_invalid_nodes_type_logs_error(self): + """Test that invalid 'nodes' type logs error.""" + logger = self.MockLogger() + result = build_graph_from_tmap({'nodes': 'not a list'}, logger=logger) + + assert result is None + assert len(logger.errors) == 1 + assert "'nodes' field must be a list" in logger.errors[0] + + def test_invalid_node_structure_logs_warning(self): + """Test that invalid node structure logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [ + 'not a dict', # Invalid node + { + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + } + ] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_nodes() == 1 # Only valid node added + assert len(logger.warnings) >= 1 + assert any("Expected dictionary" in w for w in logger.warnings) + + def test_missing_node_field_logs_warning(self): + """Test that missing 'node' field logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [ + {'meta': {}}, # Missing 'node' field + { + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + } + ] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_nodes() == 1 + assert len(logger.warnings) >= 1 + assert any("Missing 'node' field" in w for w in logger.warnings) + + def test_missing_name_field_logs_warning(self): + """Test that missing 'name' field logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + # Single invalid node -> 0 valid nodes -> returns None + assert result is None + assert len(logger.warnings) >= 1 + assert any("Missing required 'name' field" in w for w in logger.warnings) + + def test_missing_pose_field_logs_warning(self): + """Test that missing 'pose' field logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1' + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is None # No valid nodes -> returns None + assert len(logger.warnings) >= 1 + assert any("Missing required 'pose' field" in w for w in logger.warnings) + + def test_missing_position_logs_warning(self): + """Test that missing 'position' in pose logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is None # No valid nodes -> returns None + assert len(logger.warnings) >= 1 + assert any("Missing 'position' in pose" in w for w in logger.warnings) + + def test_missing_orientation_logs_warning(self): + """Test that missing 'orientation' in pose logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0} + } + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is None # No valid nodes -> returns None + assert len(logger.warnings) >= 1 + assert any("Missing 'orientation' in pose" in w for w in logger.warnings) + + def test_missing_position_coordinates_logs_warning(self): + """Test that missing position coordinates log warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0}, # Missing 'z' + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is None # No valid nodes -> returns None + assert len(logger.warnings) >= 1 + assert any("Missing position coordinates" in w for w in logger.warnings) + + def test_missing_orientation_components_logs_warning(self): + """Test that missing orientation components log warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0} # Missing 'w' + } + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is None # No valid nodes -> returns None + assert len(logger.warnings) >= 1 + assert any("Missing orientation components" in w for w in logger.warnings) + + def test_invalid_coordinate_type_logs_warning(self): + """Test that invalid coordinate types log warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 'not a number', 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is None # No valid nodes -> returns None + assert len(logger.warnings) >= 1 + assert any("Error converting coordinates to float" in w for w in logger.warnings) + + def test_invalid_edges_type_logs_warning(self): + """Test that invalid 'edges' type logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + }, + 'edges': 'not a list' + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_nodes() == 1 + assert len(logger.warnings) >= 1 + assert any("'edges' field must be a list" in w for w in logger.warnings) + + def test_invalid_edge_structure_logs_warning(self): + """Test that invalid edge structure logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + }, + 'edges': ['not a dict'] + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_nodes() == 1 + assert result.number_of_edges() == 0 + assert len(logger.warnings) >= 1 + assert any("Expected dictionary" in w for w in logger.warnings) + + def test_missing_edge_id_logs_warning(self): + """Test that missing edge_id logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + }, + 'edges': [{ + 'node': 'WP2', + 'action': 'NavigateToPose' + }] + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_edges() == 0 + assert len(logger.warnings) >= 1 + assert any("Missing 'edge_id'" in w for w in logger.warnings) + + def test_missing_edge_target_node_logs_warning(self): + """Test that missing edge target node logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + }, + 'edges': [{ + 'edge_id': 'edge1', + 'action': 'NavigateToPose' + }] + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_edges() == 0 + assert len(logger.warnings) >= 1 + assert any("Missing target 'node'" in w for w in logger.warnings) + + def test_missing_edge_action_logs_warning(self): + """Test that missing edge action logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + }, + 'edges': [{ + 'edge_id': 'edge1', + 'node': 'WP2' + }] + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_edges() == 0 + assert len(logger.warnings) >= 1 + assert any("Missing 'action'" in w for w in logger.warnings) + + def test_invalid_edge_properties_type_logs_warning(self): + """Test that invalid edge properties type logs warning.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + }, + 'edges': [{ + 'edge_id': 'edge1', + 'node': 'WP2', + 'action': 'NavigateToPose', + 'properties': 'not a dict' + }] + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_edges() == 1 + assert len(logger.warnings) >= 1 + assert any("'properties' must be a dictionary" in w for w in logger.warnings) + + def test_invalid_edge_weight_logs_warning(self): + """Test that invalid edge weight logs warning and uses default.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + }, + 'edges': [{ + 'edge_id': 'edge1', + 'node': 'WP2', + 'action': 'NavigateToPose', + 'properties': {'weight': 'not a number'} + }] + } + }] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is not None + assert result.number_of_edges() == 1 + edge_attrs = result.edges['WP1', 'WP2'] + assert edge_attrs['weight'] == 1.0 # Default weight + assert len(logger.warnings) >= 1 + assert any("Error converting weight to float" in w for w in logger.warnings) + + def test_no_valid_nodes_logs_error(self): + """Test that map with no valid nodes logs error.""" + logger = self.MockLogger() + invalid_map = { + 'nodes': [ + {'invalid': 'data'}, + {'node': {'invalid': 'structure'}} + ] + } + result = build_graph_from_tmap(invalid_map, logger=logger) + + assert result is None + assert len(logger.errors) >= 1 + assert any("No valid nodes found" in e for e in logger.errors) + + def test_unexpected_exception_logs_error(self): + """Test that unexpected exceptions are caught and logged.""" + logger = self.MockLogger() + + # Create a map that will cause an unexpected error during processing + # This is a bit tricky to trigger, but we can test the exception handler exists + invalid_map = { + 'nodes': [{ + 'node': { + 'name': 'WP1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + }] + } + + # This should succeed normally + result = build_graph_from_tmap(invalid_map, logger=logger) + assert result is not None + + def test_partial_map_processing(self): + """Test that valid nodes are processed even when some nodes are invalid.""" + logger = self.MockLogger() + mixed_map = { + 'nodes': [ + { + 'node': { + 'name': 'ValidNode1', + 'pose': { + 'position': {'x': 0, 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + }, + { + 'node': { + 'name': 'InvalidNode', + 'pose': { + 'position': {'x': 'invalid', 'y': 0, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + }, + { + 'node': { + 'name': 'ValidNode2', + 'pose': { + 'position': {'x': 5, 'y': 5, 'z': 0}, + 'orientation': {'x': 0, 'y': 0, 'z': 0, 'w': 1} + } + } + } + ] + } + + result = build_graph_from_tmap(mixed_map, logger=logger) + + assert result is not None + assert result.number_of_nodes() == 2 # Only valid nodes + assert 'ValidNode1' in result.nodes + assert 'ValidNode2' in result.nodes + assert 'InvalidNode' not in result.nodes + assert len(logger.warnings) >= 1 # Warning for invalid node + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) + + +class TestKDTreeConstruction: + """Tests for build_kdtree_from_graph function.""" + + @pytest.fixture + def simple_graph(self, simple_map_data): + """Build graph from simple map (uses module-level fixture).""" + return build_graph_from_tmap(simple_map_data) + + @pytest.fixture + def complex_graph(self, complex_map_data): + """Build graph from complex map (uses module-level fixture).""" + return build_graph_from_tmap(complex_map_data) + + def test_kdtree_construction_simple(self, simple_graph): + """Test KD-tree construction from simple graph.""" + from topological_navigation.networkx_utils import build_kdtree_from_graph + + kdtree, node_names = build_kdtree_from_graph(simple_graph) + + assert kdtree is not None + assert len(node_names) == 2 + assert 'WP1' in node_names + assert 'WP2' in node_names + + def test_kdtree_construction_complex(self, complex_graph): + """Test KD-tree construction from complex graph.""" + from topological_navigation.networkx_utils import build_kdtree_from_graph + + kdtree, node_names = build_kdtree_from_graph(complex_graph) + + assert kdtree is not None + assert len(node_names) == 10 + assert 'Entry' in node_names + assert 'Exit' in node_names + + def test_kdtree_none_graph(self): + """Test KD-tree construction with None graph.""" + from topological_navigation.networkx_utils import build_kdtree_from_graph + + kdtree, node_names = build_kdtree_from_graph(None) + + assert kdtree is None + assert node_names == [] + + def test_kdtree_empty_graph(self): + """Test KD-tree construction with empty graph.""" + from topological_navigation.networkx_utils import build_kdtree_from_graph + + empty_graph = nx.DiGraph() + kdtree, node_names = build_kdtree_from_graph(empty_graph) + + assert kdtree is None + assert node_names == [] + + +@_skip_no_geometry +class TestNearestNodeQuery: + """Tests for query_nearest_nodes function.""" + + @pytest.fixture + def simple_kdtree_setup(self, simple_map_data): + """Build graph and KD-tree from simple map.""" + from topological_navigation.networkx_utils import build_kdtree_from_graph + graph = build_graph_from_tmap(simple_map_data) + kdtree, node_names = build_kdtree_from_graph(graph) + return kdtree, node_names + + def test_query_single_nearest(self, simple_kdtree_setup): + """Test querying single nearest node.""" + from topological_navigation.networkx_utils import query_nearest_nodes + from geometry_msgs.msg import Pose + + kdtree, node_names = simple_kdtree_setup + + pose = Pose() + pose.position.x = 1.0 + pose.position.y = 0.0 + + results = query_nearest_nodes(kdtree, node_names, pose, k=1) + + assert len(results) == 1 + assert results[0]['node'] == 'WP1' + assert results[0]['dist'] > 0 + + def test_query_multiple_nearest(self, simple_kdtree_setup): + """Test querying multiple nearest nodes.""" + from topological_navigation.networkx_utils import query_nearest_nodes + from geometry_msgs.msg import Pose + + kdtree, node_names = simple_kdtree_setup + + pose = Pose() + pose.position.x = 2.5 + pose.position.y = 0.0 + + results = query_nearest_nodes(kdtree, node_names, pose, k=2) + + assert len(results) == 2 + assert all('node' in r and 'dist' in r for r in results) + + def test_query_none_kdtree(self): + """Test query with None KD-tree.""" + from topological_navigation.networkx_utils import query_nearest_nodes + from geometry_msgs.msg import Pose + + pose = Pose() + results = query_nearest_nodes(None, [], pose, k=1) + + assert results == [] + + +@_skip_no_geometry +class TestPointInPolygon: + """Tests for point_in_poly_nx function.""" + + @pytest.fixture + def polygon_graph(self): + """Create graph with polygon influence zones.""" + fixture_path = Path(__file__).parent / 'fixtures' / 'polygon_shapes_map.yaml' + with open(fixture_path, 'r') as f: + map_data = yaml.safe_load(f) + return build_graph_from_tmap(map_data) + + def test_point_inside_rectangle(self, polygon_graph): + """Test point inside RectangleNode influence zone. + + RectangleNode is at (10, -10) with verts [(-3,-1),(3,-1),(3,1),(-3,1)]. + So absolute point (10.5, -9.5) is relative (0.5, 0.5) -> inside. + """ + from topological_navigation.networkx_utils import point_in_poly_nx + from geometry_msgs.msg import Pose + + pose = Pose() + pose.position.x = 10.5 + pose.position.y = -9.5 + + result = point_in_poly_nx(polygon_graph, 'RectangleNode', pose) + assert result is True + + def test_point_outside_rectangle(self, polygon_graph): + """Test point outside RectangleNode influence zone.""" + from topological_navigation.networkx_utils import point_in_poly_nx + from geometry_msgs.msg import Pose + + pose = Pose() + pose.position.x = 50.0 + pose.position.y = 50.0 + + result = point_in_poly_nx(polygon_graph, 'RectangleNode', pose) + assert result is False + + def test_point_in_poly_none_graph(self): + """Test point-in-polygon with None graph.""" + from topological_navigation.networkx_utils import point_in_poly_nx + from geometry_msgs.msg import Pose + + pose = Pose() + result = point_in_poly_nx(None, 'node', pose) + + assert result is False + + +@_skip_no_geometry +class TestEdgeDistances: + """Tests for get_edge_distances_nx function.""" + + @pytest.fixture + def simple_graph(self, simple_map_data): + """Build graph from simple map (uses module-level fixture).""" + return build_graph_from_tmap(simple_map_data) + + def test_edge_distances_calculation(self, simple_graph): + """Test edge distance calculation.""" + from topological_navigation.networkx_utils import get_edge_distances_nx + from geometry_msgs.msg import Pose + + pose = Pose() + pose.position.x = 2.5 + pose.position.y = 1.0 + + edge_ids, distances = get_edge_distances_nx(simple_graph, pose) + + assert len(edge_ids) > 0 + assert len(distances) > 0 + assert len(edge_ids) == len(distances) + + def test_edge_distances_none_graph(self): + """Test edge distances with None graph.""" + from topological_navigation.networkx_utils import get_edge_distances_nx + from geometry_msgs.msg import Pose + import numpy as np + + pose = Pose() + edge_ids, distances = get_edge_distances_nx(None, pose) + + assert edge_ids == [] + assert len(distances) == 0 + + +@_skip_no_geometry +class TestCurrentNodeDetermination: + """Tests for determine_current_node function.""" + + @pytest.fixture + def localization_setup(self, simple_map_data): + """Build graph and KD-tree for localization.""" + from topological_navigation.networkx_utils import build_kdtree_from_graph + graph = build_graph_from_tmap(simple_map_data) + kdtree, node_names = build_kdtree_from_graph(graph) + return graph, kdtree, node_names + + def test_determine_current_inside_zone(self, localization_setup): + """Test determining current node when inside influence zone.""" + from topological_navigation.networkx_utils import determine_current_node + from geometry_msgs.msg import Pose + + graph, kdtree, node_names = localization_setup + + pose = Pose() + pose.position.x = 0.5 + pose.position.y = 0.5 + + current = determine_current_node(graph, kdtree, node_names, pose, [], []) + + assert current == 'WP1' + + def test_determine_current_outside_zones(self, localization_setup): + """Test determining current node when outside all zones.""" + from topological_navigation.networkx_utils import determine_current_node + from geometry_msgs.msg import Pose + + graph, kdtree, node_names = localization_setup + + pose = Pose() + pose.position.x = 100.0 + pose.position.y = 100.0 + + current = determine_current_node(graph, kdtree, node_names, pose, [], []) + + assert current == 'none' + + def test_determine_current_none_inputs(self): + """Test determine_current_node with None inputs.""" + from topological_navigation.networkx_utils import determine_current_node + from geometry_msgs.msg import Pose + + pose = Pose() + current = determine_current_node(None, None, [], pose, [], []) + + assert current == 'none' + + +@_skip_no_geometry +class TestClosestNodeDetermination: + """Tests for determine_closest_node function.""" + + @pytest.fixture + def localization_setup(self, simple_map_data): + """Build graph and KD-tree for localization.""" + from topological_navigation.networkx_utils import build_kdtree_from_graph + graph = build_graph_from_tmap(simple_map_data) + kdtree, node_names = build_kdtree_from_graph(graph) + return graph, kdtree, node_names + + def test_determine_closest_basic(self, localization_setup): + """Test determining closest node.""" + from topological_navigation.networkx_utils import determine_closest_node + from geometry_msgs.msg import Pose + + graph, kdtree, node_names = localization_setup + + pose = Pose() + pose.position.x = 1.0 + pose.position.y = 0.0 + + closest, dist = determine_closest_node(kdtree, node_names, graph, 'none', [], [], pose) + + assert closest == 'WP1' + assert dist > 0 + + def test_determine_closest_with_current(self, localization_setup): + """Test closest node when current node is set.""" + from topological_navigation.networkx_utils import determine_closest_node + from geometry_msgs.msg import Pose + + graph, kdtree, node_names = localization_setup + + pose = Pose() + pose.position.x = 0.5 + pose.position.y = 0.5 + + closest, dist = determine_closest_node(kdtree, node_names, graph, 'WP1', [], [], pose) + + assert closest == 'WP1' + + def test_determine_closest_none_kdtree(self): + """Test determine_closest_node with None KD-tree.""" + from topological_navigation.networkx_utils import determine_closest_node + from geometry_msgs.msg import Pose + + pose = Pose() + closest, dist = determine_closest_node(None, [], None, 'none', [], [], pose) + + assert closest == 'none' + assert dist == float('inf') + + +class TestTopicBasedLocalization: + """Tests for update_loc_by_topic_nx function.""" + + @pytest.fixture + def complex_graph(self, complex_map_data): + """Build graph from complex map.""" + return build_graph_from_tmap(complex_map_data) + + def test_update_loc_by_topic(self, complex_graph): + """Test extracting topic-based localization config.""" + from topological_navigation.networkx_utils import update_loc_by_topic_nx + + nodes_by_topic, names_by_topic = update_loc_by_topic_nx(complex_graph) + + assert len(nodes_by_topic) > 0 + assert len(names_by_topic) > 0 + assert 'TopicLocaliseNode' in names_by_topic + + def test_update_loc_by_topic_none_graph(self): + """Test update_loc_by_topic_nx with None graph.""" + from topological_navigation.networkx_utils import update_loc_by_topic_nx + + nodes, names = update_loc_by_topic_nx(None) + + assert nodes == [] + assert names == [] + + +# =========================================================================== +# Graph algorithm wrappers +# =========================================================================== + + +class TestComputeShortestPath: + """Tests for compute_shortest_path.""" + + @pytest.fixture + def weighted_graph(self): + """A -> B (1) -> C (2), A -> C (5).""" + from topological_navigation.networkx_utils import compute_shortest_path # noqa: F401 + import networkx as nx + G = nx.DiGraph() + G.add_edge('A', 'B', weight=1.0) + G.add_edge('B', 'C', weight=2.0) + G.add_edge('A', 'C', weight=5.0) + return G + + def test_shortest_via_intermediate(self, weighted_graph): + from topological_navigation.networkx_utils import compute_shortest_path + path = compute_shortest_path(weighted_graph, 'A', 'C') + assert path == ['A', 'B', 'C'] + + def test_same_node(self, weighted_graph): + from topological_navigation.networkx_utils import compute_shortest_path + assert compute_shortest_path(weighted_graph, 'A', 'A') == ['A'] + + def test_no_path(self, weighted_graph): + """C has no outgoing edges, so C->A should return [].""" + from topological_navigation.networkx_utils import compute_shortest_path + assert compute_shortest_path(weighted_graph, 'C', 'A') == [] + + def test_none_graph(self): + from topological_navigation.networkx_utils import compute_shortest_path + assert compute_shortest_path(None, 'A', 'B') == [] + + def test_missing_source(self, weighted_graph): + from topological_navigation.networkx_utils import compute_shortest_path + assert compute_shortest_path(weighted_graph, 'X', 'A') == [] + + def test_missing_target(self, weighted_graph): + from topological_navigation.networkx_utils import compute_shortest_path + assert compute_shortest_path(weighted_graph, 'A', 'X') == [] + + +class TestComputePathLength: + """Tests for compute_path_length.""" + + @pytest.fixture + def weighted_graph(self): + import networkx as nx + G = nx.DiGraph() + G.add_edge('A', 'B', weight=1.5) + G.add_edge('B', 'C', weight=2.5) + G.add_edge('A', 'C', weight=5.0) + return G + + def test_shortest_length(self, weighted_graph): + from topological_navigation.networkx_utils import compute_path_length + length = compute_path_length(weighted_graph, 'A', 'C') + assert length == pytest.approx(4.0) + + def test_same_node_zero(self, weighted_graph): + from topological_navigation.networkx_utils import compute_path_length + assert compute_path_length(weighted_graph, 'A', 'A') == 0.0 + + def test_no_path_inf(self, weighted_graph): + from topological_navigation.networkx_utils import compute_path_length + assert compute_path_length(weighted_graph, 'C', 'A') == float('inf') + + def test_none_graph_inf(self): + from topological_navigation.networkx_utils import compute_path_length + assert compute_path_length(None, 'A', 'B') == float('inf') + + +class TestCheckConnectivity: + """Tests for check_connectivity.""" + + @pytest.fixture + def directed_graph(self): + import networkx as nx + G = nx.DiGraph() + G.add_edge('A', 'B') + G.add_edge('B', 'C') + G.add_node('D') # isolated + return G + + def test_connected(self, directed_graph): + from topological_navigation.networkx_utils import check_connectivity + assert check_connectivity(directed_graph, 'A', 'C') is True + + def test_not_connected_reverse(self, directed_graph): + from topological_navigation.networkx_utils import check_connectivity + assert check_connectivity(directed_graph, 'C', 'A') is False + + def test_isolated_node(self, directed_graph): + from topological_navigation.networkx_utils import check_connectivity + assert check_connectivity(directed_graph, 'A', 'D') is False + + def test_same_node(self, directed_graph): + from topological_navigation.networkx_utils import check_connectivity + assert check_connectivity(directed_graph, 'A', 'A') is True + + def test_none_graph(self): + from topological_navigation.networkx_utils import check_connectivity + assert check_connectivity(None, 'A', 'B') is False + + def test_missing_node(self, directed_graph): + from topological_navigation.networkx_utils import check_connectivity + assert check_connectivity(directed_graph, 'X', 'A') is False + + +class TestGetNeighbors: + """Tests for get_neighbors.""" + + @pytest.fixture + def g(self): + import networkx as nx + G = nx.DiGraph() + G.add_edge('A', 'B') + G.add_edge('A', 'C') + G.add_edge('B', 'C') + return G + + def test_neighbors_of_hub(self, g): + from topological_navigation.networkx_utils import get_neighbors + assert sorted(get_neighbors(g, 'A')) == ['B', 'C'] + + def test_leaf_has_one_neighbor(self, g): + from topological_navigation.networkx_utils import get_neighbors + assert get_neighbors(g, 'B') == ['C'] + + def test_no_outgoing(self, g): + from topological_navigation.networkx_utils import get_neighbors + assert get_neighbors(g, 'C') == [] + + def test_none_graph(self): + from topological_navigation.networkx_utils import get_neighbors + assert get_neighbors(None, 'A') == [] + + def test_missing_node(self, g): + from topological_navigation.networkx_utils import get_neighbors + assert get_neighbors(g, 'X') == [] + + +# =========================================================================== +# normalize_action_name (navigation_graph) +# =========================================================================== + + +class TestNormalizeActionName: + """Tests for navigation_graph.normalize_action_name.""" + + def test_camel_to_canonical(self): + from topological_navigation.navigation_graph import normalize_action_name + assert normalize_action_name('NavigateToPose') == 'NavigateToPose' + + def test_snake_to_canonical(self): + from topological_navigation.navigation_graph import normalize_action_name + assert normalize_action_name('navigate_to_pose') == 'NavigateToPose' + + def test_row_traversal_cases(self): + from topological_navigation.navigation_graph import normalize_action_name + assert normalize_action_name('RowTraversal') == 'row_traversal' + assert normalize_action_name('row_traversal') == 'row_traversal' + + def test_goal_align_cases(self): + from topological_navigation.navigation_graph import normalize_action_name + assert normalize_action_name('GoalAlign') == 'goal_align' + assert normalize_action_name('goal_align') == 'goal_align' + + def test_unknown_returned_unchanged(self): + from topological_navigation.navigation_graph import normalize_action_name + assert normalize_action_name('SomethingNew') == 'SomethingNew' diff --git a/topological_navigation/test/test_tmap_utils.py b/topological_navigation/test/test_tmap_utils.py new file mode 100644 index 00000000..17bebf5f --- /dev/null +++ b/topological_navigation/test/test_tmap_utils.py @@ -0,0 +1,182 @@ +"""Tests for tmap_utils module. + +Covers CustomSafeLoader, NoAliasDumper, get_node_from_tmap2, +and get_edge_from_id_tmap2. +""" + +import yaml +import pytest + +from topological_navigation.tmap_utils import ( + CustomSafeLoader, + NoAliasDumper, + get_node_from_tmap2, + get_edge_from_id_tmap2, +) + + +# ---------- Fixtures -------------------------------------------------- + +@pytest.fixture +def sample_tmap(): + """Build a minimal tmap2 structure for testing.""" + return { + "nodes": [ + { + "node": { + "name": "WP1", + "pose": { + "position": {"x": 1.0, "y": 2.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}, + }, + "edges": [ + { + "edge_id": "WP1_WP2", + "node": "WP2", + "action": "navigate_to_pose", + }, + { + "edge_id": "WP1_WP3", + "node": "WP3", + "action": "row_traversal", + }, + ], + } + }, + { + "node": { + "name": "WP2", + "pose": { + "position": {"x": 5.0, "y": 0.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}, + }, + "edges": [], + } + }, + ] + } + + +# ---------- CustomSafeLoader ------------------------------------------ + +class TestCustomSafeLoader: + """Tests for CustomSafeLoader YAML loading.""" + + def test_int_pose_keys_converted_to_float(self): + """Int values for x, y, z, w are converted to float.""" + doc = "position:\n x: 1\n y: 2\n z: 0\n" + data = yaml.load(doc, Loader=CustomSafeLoader) + assert isinstance(data["position"]["x"], float) + assert isinstance(data["position"]["y"], float) + assert isinstance(data["position"]["z"], float) + + def test_float_values_remain_float(self): + """Already-float values remain unchanged.""" + doc = "x: 1.5\ny: 2.5\nz: 0.0\nw: 1.0\n" + data = yaml.load(doc, Loader=CustomSafeLoader) + assert data["x"] == 1.5 + assert data["w"] == 1.0 + + def test_tolerance_keys_converted(self): + """Tolerance keys with int values are converted to float.""" + doc = "xy_goal_tolerance: 1\nyaw_goal_tolerance: 2\n" + data = yaml.load(doc, Loader=CustomSafeLoader) + assert isinstance(data["xy_goal_tolerance"], float) + assert isinstance(data["yaw_goal_tolerance"], float) + + def test_non_pose_int_keys_unchanged(self): + """Keys other than pose/tolerance keep their original type.""" + doc = "capacity: 3\nname: test\n" + data = yaml.load(doc, Loader=CustomSafeLoader) + assert isinstance(data["capacity"], int) + assert isinstance(data["name"], str) + + def test_orientation_w_converted(self): + """Orientation quaternion w-component int is converted to float.""" + doc = "w: 1\n" + data = yaml.load(doc, Loader=CustomSafeLoader) + assert isinstance(data["w"], float) + assert data["w"] == 1.0 + + +# ---------- NoAliasDumper ---------------------------------------------- + +class TestNoAliasDumper: + """Tests for NoAliasDumper YAML output.""" + + def test_no_aliases_in_output(self): + """Duplicate objects are written without YAML anchors/aliases.""" + shared = {"x": 1.0, "y": 2.0} + data = {"a": shared, "b": shared} + output = yaml.dump(data, Dumper=NoAliasDumper) + assert "&" not in output + assert "*" not in output + + def test_roundtrip_preserves_data(self): + """Dump then load produces equivalent data.""" + data = {"nodes": [{"name": "A"}, {"name": "B"}]} + output = yaml.dump(data, Dumper=NoAliasDumper) + loaded = yaml.safe_load(output) + assert loaded == data + + +# ---------- get_node_from_tmap2 ---------------------------------------- + +class TestGetNodeFromTmap2: + """Tests for get_node_from_tmap2.""" + + def test_existing_node(self, sample_tmap): + """Returns the matching node entry dict.""" + result = get_node_from_tmap2(sample_tmap, "WP1") + assert result is not None + assert result["node"]["name"] == "WP1" + + def test_second_node(self, sample_tmap): + """Can find a node that is not first in the list.""" + result = get_node_from_tmap2(sample_tmap, "WP2") + assert result is not None + assert result["node"]["name"] == "WP2" + + def test_nonexistent_node(self, sample_tmap): + """Returns None when node name is not in the map.""" + result = get_node_from_tmap2(sample_tmap, "MISSING") + assert result is None + + def test_empty_map(self): + """Returns None for an empty map.""" + result = get_node_from_tmap2({"nodes": []}, "WP1") + assert result is None + + +# ---------- get_edge_from_id_tmap2 ------------------------------------- + +class TestGetEdgeFromIdTmap2: + """Tests for get_edge_from_id_tmap2.""" + + def test_existing_edge(self, sample_tmap): + """Returns the matching edge dict.""" + result = get_edge_from_id_tmap2(sample_tmap, "WP1", "WP1_WP2") + assert result is not None + assert result["edge_id"] == "WP1_WP2" + assert result["node"] == "WP2" + + def test_second_edge(self, sample_tmap): + """Can find an edge that is not the first one.""" + result = get_edge_from_id_tmap2(sample_tmap, "WP1", "WP1_WP3") + assert result is not None + assert result["edge_id"] == "WP1_WP3" + + def test_nonexistent_edge(self, sample_tmap): + """Returns None when edge_id doesn't exist on the node.""" + result = get_edge_from_id_tmap2(sample_tmap, "WP1", "MISSING_EDGE") + assert result is None + + def test_nonexistent_node(self, sample_tmap): + """Returns None when node doesn't exist.""" + result = get_edge_from_id_tmap2(sample_tmap, "MISSING", "WP1_WP2") + assert result is None + + def test_node_with_no_edges(self, sample_tmap): + """Returns None when the node has an empty edges list.""" + result = get_edge_from_id_tmap2(sample_tmap, "WP2", "anything") + assert result is None diff --git a/topological_navigation/test/test_validate_map.py b/topological_navigation/test/test_validate_map.py new file mode 100644 index 00000000..cb03d427 --- /dev/null +++ b/topological_navigation/test/test_validate_map.py @@ -0,0 +1,170 @@ +"""Tests for validate_map module. + +Covers find_schema_file, load_yaml_file, and validate_map. +""" + +import os +import tempfile +import textwrap + +import pytest +import yaml + +from topological_navigation.validate_map import ( + find_schema_file, + load_yaml_file, + validate_map, +) + + +# ---------- Helpers / fixtures ----------------------------------------- + +_FIXTURES = os.path.join(os.path.dirname(__file__), "fixtures") +_CONFIG = os.path.join(os.path.dirname(__file__), os.pardir, "config") +_SCHEMA = os.path.join(_CONFIG, "tmap-schema.yaml") +_SIMPLE_MAP = os.path.join(_FIXTURES, "simple_map.yaml") +_COMPLEX_MAP = os.path.join(_FIXTURES, "complex_map.yaml") + + +def _write_tmp_yaml(data, suffix=".yaml"): + """Write *data* to a temp file and return its path.""" + fd, path = tempfile.mkstemp(suffix=suffix) + with os.fdopen(fd, "w") as fh: + yaml.dump(data, fh) + return path + + +# ---------- find_schema_file ------------------------------------------- + +class TestFindSchemaFile: + """Tests for find_schema_file.""" + + def test_returns_path_or_none(self): + """Function returns a string path or None.""" + result = find_schema_file() + assert result is None or isinstance(result, str) + + def test_returned_path_exists_if_not_none(self): + """If a path is returned, the file must exist.""" + result = find_schema_file() + if result is not None: + assert os.path.isfile(result) + + +# ---------- load_yaml_file --------------------------------------------- + +class TestLoadYamlFile: + """Tests for load_yaml_file.""" + + def test_load_valid_yaml(self): + """Successfully loads a valid YAML file.""" + path = _write_tmp_yaml({"key": "value"}) + try: + data = load_yaml_file(path) + assert data == {"key": "value"} + finally: + os.unlink(path) + + def test_load_nonexistent_raises(self): + """Raises FileNotFoundError for a missing file.""" + with pytest.raises(FileNotFoundError): + load_yaml_file("/tmp/nonexistent_file_12345.yaml") + + def test_load_invalid_yaml_raises(self): + """Raises ValueError for syntactically broken YAML.""" + fd, path = tempfile.mkstemp(suffix=".yaml") + with os.fdopen(fd, "w") as fh: + fh.write(":\n - :\n bad: [unclosed\n") + try: + with pytest.raises(ValueError): + load_yaml_file(path) + finally: + os.unlink(path) + + +# ---------- validate_map ----------------------------------------------- + +class TestValidateMap: + """Tests for validate_map.""" + + @pytest.fixture(autouse=True) + def _skip_if_no_schema(self): + """Skip tests if schema file cannot be located.""" + if not os.path.isfile(_SCHEMA): + pytest.skip("tmap-schema.yaml not found") + + def test_simple_map_valid(self): + """simple_map.yaml should pass validation.""" + is_valid, msg = validate_map(_SIMPLE_MAP, _SCHEMA) + assert is_valid, msg + + def test_complex_map_valid(self): + """complex_map.yaml should pass validation.""" + is_valid, msg = validate_map(_COMPLEX_MAP, _SCHEMA) + assert is_valid, msg + + def test_verbose_flag(self, capsys): + """Verbose mode prints extra information.""" + validate_map(_SIMPLE_MAP, _SCHEMA, verbose=True) + captured = capsys.readouterr() + assert "Using schema" in captured.out + assert "Validating map" in captured.out + + def test_nonexistent_map(self): + """Validation fails gracefully for missing map file.""" + is_valid, msg = validate_map("/tmp/nonexistent.yaml", _SCHEMA) + assert not is_valid + assert "Error" in msg or "not found" in msg.lower() + + def test_invalid_map_content(self): + """Validation fails for a map that violates the schema.""" + bad = _write_tmp_yaml({"not_a_valid_map": True}) + try: + is_valid, _ = validate_map(bad, _SCHEMA) + assert not is_valid + finally: + os.unlink(bad) + + def test_schema_not_found(self): + """Returns failure when schema file cannot be found.""" + is_valid, msg = validate_map( + _SIMPLE_MAP, "/tmp/no_such_schema.yaml" + ) + assert not is_valid + + def test_schema_auto_detect(self): + """If schema is None, the function tries to auto-detect.""" + # We just verify it doesn't crash; the result depends on the + # runtime environment. + is_valid, msg = validate_map(_SIMPLE_MAP, schema_file=None) + # Either it found the schema and validated, or it didn't + assert isinstance(is_valid, bool) + assert isinstance(msg, str) + + def test_duplicate_node_names_warning(self): + """Duplicate node names generate a warning in the message.""" + data = yaml.safe_load(open(_SIMPLE_MAP)) + # Duplicate the first node + dup = dict(data["nodes"][0]) + data["nodes"].append(dup) + path = _write_tmp_yaml(data) + try: + is_valid, msg = validate_map(path, _SCHEMA) + # Should still be schema-valid but contain a warning + if is_valid: + assert "Duplicate" in msg or "Warning" in msg + finally: + os.unlink(path) + + def test_edge_to_nonexistent_node_warning(self): + """An edge targeting a missing node produces a warning.""" + data = yaml.safe_load(open(_SIMPLE_MAP)) + # Point the first edge to a non-existent node + data["nodes"][0]["node"]["edges"][0]["node"] = "GHOST" + path = _write_tmp_yaml(data) + try: + is_valid, msg = validate_map(path, _SCHEMA) + if is_valid: + assert "non-existent" in msg.lower() or "Warning" in msg + finally: + os.unlink(path) diff --git a/topological_navigation/tests/README.md b/topological_navigation/tests/README.md deleted file mode 100644 index 9b738a52..00000000 --- a/topological_navigation/tests/README.md +++ /dev/null @@ -1,171 +0,0 @@ -# Topological Navigation tests - -The topological navigation tests are designed to optimise the parameter set of the DWA planner and to check the functionalitites of topological navigation in its execute policy mode. The test are run automatically on our Jenkins server but can also be run locally on your machine to test a new paremeterset. - -## Run test - -Test for your whole worksapce can be run with `catkin_make test` which assumes that `strands_navigation` is present in your ros workspace. If you only want to run the test described here, use `catkin_make test --pkg topological_navigation`. This will only run the critical topological navigation tests. A set of supplementary tests can be run with: - -``` -roslaunch topological_navigation navigation_scenarios.test -rosrun topological_navigation topological_navigation_tester_supplementary.py -``` - -To run the testes from an installed version of topological_navigation do: - -``` -roslaunch topological_navigation navigation_scenarios.test -``` - -and - -``` -rosrun topological_navigation topological_navigation_tester_critical.py -``` - -for the critical tests or - -``` -rosrun topological_navigation topological_navigation_tester_supplementary.py -``` - -for the supplementary tests to test parameter sets. - -This will start all the tests and report the result to the terminal and a log file. - -## Test Scenarios, and Infrastructure - -### Infrastructure - -In order to run the tests, the commands above will start the following nodes: - -* `strands_morse` using the move_base_arena environment -* `mongodb_store` in test mode, creating a new and empty temporary datacentre -* `scitos_2d_navigation` with the correct slam map for the tests -* A static transform publisher with the transformation from `map` to `world` -* `topological_navigation`: - * `monitored_navigation` with all recovery behaviours disabled - * `map_manager.py` - * `fremenserver` - * `localisation.py` - * `navigation.py` - * `execute_policy_server.py` - * `visualise_map.py` - * `travel_time_estimator.py` - * `topological_prediction.py` - * `get_simple_policy.py` -* The test scneario server which is responsible for the test control -* The ros test file `topological_navigation_tester.py` - -### Test Scenarios - -All the test scnearios are based on a simple topological map that has to have a Node called `Start` and a node called `End`. The robot will always be teleported (in simulation) or pushed (on the real robot) to this node and then use policy execution to traverse the edges towards the node called `End`. If the node `End` cannot be reached, the navigation will fail and the scneario_server will trigger the graceful death behaviour, trying to navigate the robot back to the node `Start`. - -The topological maps used for the tests presented here can be found in `strands_morse/mba/maps` and are loaded into the datacentre (if they haven't already been inserted) by the scanario_server. Currently run tests are: - -**Static:** - -* System critical: - * `mb_test0`: Traversing a 2m wide l-shaped corridor. -* Supplementary: - * `mb_test1`: The robot starting 10cm away from a wall facing it straight on (0 degrees) - * `mb_test2`: The robot starting 10cm away from a wall facing it turned to the left (-45 degrees) - * `mb_test3`: The robot starting 10cm away from a wall facing it turned to the right (+45 degrees) - * `mb_test4`: Traversing a 1.55m wide straight corridor with .55m wide chairs on one side. - * `mb_test5`: Traversing a 2.1m wide straight corridor with .55m wide chairs on both sides. - * `mb_test6`: Cross a 80cm wide door. - * `mb_test7`: Cross a 70cm wide door. - * `mb_test8`: The robot is trapped in a corner and has to reach a goal behind it. - * `mb_test9`: Traverse a 1m wide straight corridor. - * `mb_test10`: Traverse a 1m wide l-shaped corridor. - * `mb_test11`: Wheelchair blocking an intermediate node in open space. - * `mb_test12`: Wheelchair blocking final node in open space. Graceful fail has to be tru to pass this test as the node itself can never be reached precisely. - * `mb_test13`: Human blocking an intermediate node in open space. - * `mb_test14`: Human blocking final node in open space. Graceful fail has to be tru to pass this test as the node itself can never be reached precisely. - * `mb_test15`: Non-SLAM map chairs on one side of 2m wide l-shaped corridor. - * `mb_test16`: Non-SLAM map wheelchairs on one side of 2m wide l-shaped corridor. - * `mb_test17`: Non-SLAM map wheelchairs block 2m wide l-shaped corridor after intermediate waypoint. Graceful fail has to be tru to pass this test as the node itself can never be reached. - * `mb_test18`: Non-SLAM map static humans block 2m wide l-shaped corridor after intermediate waypoint. Graceful fail has to be tru to pass this test as the node itself can never be reached. - -**Dynamic** - -Coming soon - -### Scenario Server control - -The scenario server can also be used without the unit test file for tests on the real robot. Running - -``` -roslaunch topological_navigation navigation_scenarios.test robot:=true map_dir:="path_to_topologicalmaps" -``` - -will start only the scneario server, the simple policy generation, and the joypad control. This assumes that everything on the robot, up to topological navigation, is running. The `map_dir` argument specifies a directory which holds the topological maps that you want to use for testing. If this is given, the maps will automatically inserted into the datacentre. If the maps have been inserted previously, the server will print a warning and skip the insertion. If the `map_dir` argument is omitted, no maps are inserted. The scneario server then offers 3 simple services: - -* `/scneario_server/load ` expects a string which is the name of the topological map you want to test. Keep in mind that this has to have the node `Start` and `End`. -* `/scenario_server/reset` is an empty service and is called to reset the data recording and the robot position. The robot position, however, cannot as easily be changed in real life as it can be in simulation, hence the robot has to be pushed to the starting node and then confirmed via the `A` button on the joypad. The reasoning behind having to push the robot is that the starting position might not be easily reachable via `move_base`. The server will print `+++ Please push the robot to 'Start' +++` where `Start` is the name of the starting node, until the node is reached. Once the node is reached, the server will print `+++ Please confirm correct positioning with A button on joypad: distance 0.65m 180.06deg +++` where distance represents the distance of the current `/robot_pose` to the metric coordinates of the node. In simulation this will just teleport the robot to the correct node. -* `/scneario_server/start` starts the policy execution. Returns - - ``` -bool nav_success -bool graceful_fail -bool human_success -float64 min_distance_to_human -float64 mean_speed -float64 distance_travelled -float64 travel_time - ``` - -**Joypad control** - -For convenience, a joypad control of the `reset` and `start` service is provided. The `load` service still has to be called manually to tell the server which map to use. The joypad control then offers an easy way to interact with the scenario server during the tests: - -* `A`: Toggle between `start` and `reset`. -* `B`: Confirm current selection. - -If in doubt, press `A` and look for the output on the terminal. - -**Creating Topological maps for testing** - -The topological map used for each test has to be a different one and the nodes have to conform to a specific naming scheme. By default, the start node has to be called `Start` and the goal has to be called `End`. This can be changed in `topologcial_navigation/tests/conf/scenario_server.yaml`. The easiest way to create these maps is: - -1. Start topological navigation with `roslaunch topological_navigation topologocal_navigation_empty_map.launch map:=` where `map_name` will be the name of your new map and cannot be the name of an existing one. -1. Drive the robot to the positions of the nodes you want to create and use the `add_rm_node` interactive marker in rviz to create a new node. -1. Use the interactive `edges` marker in rviz to delete unwanted edges. -1. Rename the start and end node to `Start` and `End` using `rosrun topological_utils rename_node ` -1. This map can now be loaded with `/scenario_server/load ` - - -**Creating scnearios with obstacles that are not in the SLAM map** - -For this purpose, there are `Chair`s, `OfficeChair`s, `WheelChair`s, and `StaticHuman`s present in the current test environment that can be positioned via topological nodes. The exact obstacle types are defined in the `conf/scenario_server.yaml` under `obstacle_types`; The names used have to be the variable name of the obstacle in the morse environment. Obstacle types have to be lower case, node names can be camel case to enhance readability. Currently, in the test environment, each object has 10 instances so you can only use 10 of any single object. The Objects are positioned according to topological nodes following a naming scheme: `ObstacleStaticHuman1` for example positions a static human model on this node. `Obstacle` is the `obstacle_node_prefix` defined in the `conf/scneario_server.yaml`, `StaticHuman` is the identifier of the obstacle type (if this omitted, an arbitrary object will be used), and `1` is just an arbitrary number to make the node name unique. When creating the topoligocal map, make sure that all edges from and to obstacle nodes are removed. Additionally, the nodes need to define a `localise_by_topic` json string so the robot will never localise itself based on these nodes. In the current simulation, we use the charging topic, because the robot will never charge and hence never localise itself at these nodes. We have to use an existing topic otherwise topological navigation fails. Example node: - -``` -- meta: - map: mb_arena - node: ObstacleStaticHuman1 - pointset: mb_test16 - node: - edges: [] - localise_by_topic: '{"topic": "battery_state", "field": "charging", "val": true}' - map: mb_arena - name: ObstacleStaticHuman1 - pointset: mb_test16 - pose: - orientation: - w: 0.816770076752 - ... - position: - x: -4.58532047272 - ... - verts: - - x: 0.689999997616 - y: 0.287000000477 - ... - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 -``` - -Where the important bit is the `localise_by_topic: '{"topic": "battery_state", "field": "charging", "val": true}'` entry. - -After the map has been loaded the obstacles will be spawned at (or better moved to) the respective nodes before the robot starts navigating. Before each test the arena is cleared to make sure that no obstacles linger. - diff --git a/topological_navigation/tests/conf/scenario_server.yaml b/topological_navigation/tests/conf/scenario_server.yaml deleted file mode 100644 index e9618d95..00000000 --- a/topological_navigation/tests/conf/scenario_server.yaml +++ /dev/null @@ -1,10 +0,0 @@ -robot_start_node: Start -robot_goal_node: End -obstacle_node_prefix: Obstacle -obstacle_types: - - chair - - officechair - - wheelchair - - statichuman -success_metrics: - nav_timeout: 60.0 # 0 for inf diff --git a/topological_navigation/tests/conf/test.tmap2 b/topological_navigation/tests/conf/test.tmap2 deleted file mode 100644 index ef103ad0..00000000 --- a/topological_navigation/tests/conf/test.tmap2 +++ /dev/null @@ -1,130 +0,0 @@ -meta: - last_updated: 2022-03-02_10-07-31 -metric_map: map_2d -name: new_map -nodes: -- meta: - map: map_2d - node: WayPoint1 - pointset: new_map - node: - edges: - - action: NavigateToPose - action_type: move_base_msgs/MoveBaseGoal - config: &id001 [] - edge_id: WayPoint1_WayPoint2 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint2 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint1 - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 0.0 - y: 0.0 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: 'True' - verts: - - x: 0.692909649383465 - y: 0.2870125742738173 - - x: 0.2870125742738174 - y: 0.692909649383465 - - x: -0.2870125742738173 - y: 0.692909649383465 - - x: -0.692909649383465 - y: 0.28701257427381743 - - x: -0.6929096493834651 - y: -0.28701257427381727 - - x: -0.28701257427381777 - y: -0.6929096493834649 - - x: 0.2870125742738169 - y: -0.6929096493834652 - - x: 0.6929096493834649 - y: -0.28701257427381777 -- meta: - map: map_2d - node: WayPoint2 - pointset: new_map - node: - edges: - - action: NavigateToPose - action_type: move_base_msgs/MoveBaseGoal - config: *id001 - edge_id: WayPoint2_WayPoint1 - fail_policy: fail - fluid_navigation: true - goal: - target_pose: - header: - frame_id: $node.parent_frame - pose: $node.pose - node: WayPoint1 - recovery_behaviours_config: '' - restrictions_planning: 'True' - restrictions_runtime: 'True' - localise_by_topic: '' - name: WayPoint2 - parent_frame: map - pose: - orientation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - position: - x: 10.0 - y: 10.0 - z: 0.0 - properties: - xy_goal_tolerance: 0.3 - yaw_goal_tolerance: 0.1 - restrictions_planning: 'True' - restrictions_runtime: 'True' - verts: - - x: 0.692909649383465 - y: 0.2870125742738173 - - x: 0.2870125742738174 - y: 0.692909649383465 - - x: -0.2870125742738173 - y: 0.692909649383465 - - x: -0.692909649383465 - y: 0.28701257427381743 - - x: -0.6929096493834651 - y: -0.28701257427381727 - - x: -0.28701257427381777 - y: -0.6929096493834649 - - x: 0.2870125742738169 - y: -0.6929096493834652 - - x: 0.6929096493834649 - y: -0.28701257427381777 -pointset: new_map -transformation: - child: topo_map - parent: map - rotation: - w: 1.0 - x: 0.0 - y: 0.0 - z: 0.0 - translation: - x: 0.0 - y: 0.0 - z: 0.0 diff --git a/topological_navigation/tests/map_manager.test b/topological_navigation/tests/map_manager.test deleted file mode 100644 index de63bccd..00000000 --- a/topological_navigation/tests/map_manager.test +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/topological_navigation/tests/map_manager_tester.py b/topological_navigation/tests/map_manager_tester.py deleted file mode 100755 index d343e8a0..00000000 --- a/topological_navigation/tests/map_manager_tester.py +++ /dev/null @@ -1,61 +0,0 @@ -#!/usr/bin/env python -""" -Created on Fri Jan 22 10:34:58 2021 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -######################################################################################################### -import unittest, rostest -import rospy, json - -from std_msgs.msg import String -from topological_navigation_msgs.srv import GetEdgesBetweenNodes, GetEdgesBetweenNodesRequest - -PKG = "topological_navigation" - - -class MapManagerTester(unittest.TestCase): - - def __init__(self, *args): - super(MapManagerTester, self).__init__(*args) - rospy.init_node('map_manager_tester') - - self.map_received = False - rospy.Subscriber("/topological_map_2", String, self.map_callback) - - - def map_callback(self, msg): - self.tmap2 = json.loads(msg.data) - self.map_received = True - - - def test_map_manager(self): - - rospy.sleep(5.0) - self.assertTrue(self.map_received) - - try: - s = rospy.ServiceProxy("/topological_map_manager2/get_edges_between_nodes", GetEdgesBetweenNodes) - s.wait_for_service(timeout=5.0) - - req = GetEdgesBetweenNodesRequest() - req.nodea = "WayPoint1" - req.nodeb = "WayPoint2" - res = s(req) - - self.assertGreater(len(res.ids_a_to_b), 0) - self.assertGreater(len(res.ids_b_to_a), 0) - - self.assertEqual(res.ids_a_to_b[0], "WayPoint1_WayPoint2") - self.assertEqual(res.ids_b_to_a[0], "WayPoint2_WayPoint1") - - except (rospy.ServiceException, rospy.ROSInterruptException) as e: - rospy.logfatal(e) - self.assertTrue(False) -######################################################################################################### - - -######################################################################################################### -if __name__ == "__main__": - rostest.rosrun(PKG, "map_manager_tester", MapManagerTester) -######################################################################################################### \ No newline at end of file diff --git a/topological_navigation/tests/navigation_scenarios.test b/topological_navigation/tests/navigation_scenarios.test deleted file mode 100644 index f2b13f85..00000000 --- a/topological_navigation/tests/navigation_scenarios.test +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_navigation/tests/navigation_test_joypad_control.py b/topological_navigation/tests/navigation_test_joypad_control.py deleted file mode 100755 index 489a6571..00000000 --- a/topological_navigation/tests/navigation_test_joypad_control.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy -from scitos_teleop.msg import action_buttons -from topological_navigation_msgs.srv import RunTopoNavTestScenario -from std_srvs.srv import Empty - - -class JoyPadControl(object): - _start = "start" - _reset = "reset" - - def __init__(self, name): - rospy.loginfo("Starting %s ..." % name) - self.action = self._start - rospy.Subscriber("/teleop_joystick/action_buttons", action_buttons, self.callback, queue_size=1) - rospy.loginfo("... done") - - def callback(self, msg): - if msg.A: - self.action = self._start if self.action == self._reset else self._reset - rospy.loginfo("+++ Current action '%s'. Press 'B' to confirm or 'A' to toggle. +++" % self.action) - elif msg.B: - if self.action == self._start: - try: - s = rospy.ServiceProxy("/scenario_server/start", RunTopoNavTestScenario) - s.wait_for_service() - rospy.loginfo(s()) - except (rospy.ServiceException, rospy.ROSInterruptException) as e: - rospy.logwarn(e) - elif self.action == self._reset: - try: - s = rospy.ServiceProxy("/scenario_server/reset", Empty) - s.wait_for_service() - s() - except (rospy.ServiceException, rospy.ROSInterruptException) as e: - rospy.logwarn(e) - - -if __name__ == "__main__": - rospy.init_node("navigation_test_joypad_control") - JoyPadControl(rospy.get_name()) - rospy.spin() diff --git a/topological_navigation/tests/scenario_server.py b/topological_navigation/tests/scenario_server.py deleted file mode 100755 index 95ce618f..00000000 --- a/topological_navigation/tests/scenario_server.py +++ /dev/null @@ -1,388 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -""" -Created on Tue Jul 14 11:10:28 2015 - -@author: cdondrup -""" - -import socket -import rospy -from std_srvs.srv import Empty, EmptyResponse -from std_msgs.msg import Header, String -import yaml -import numpy as np -from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, PoseStamped, Point -import actionlib -import actionlib_msgs -from topological_navigation.msg import GotoNodeAction, GotoNodeGoal -from topological_navigation_msgs.msg import ExecutePolicyModeAction, ExecutePolicyModeGoal -from topological_navigation_msgs.srv import LoadTopoNavTestScenario, LoadTopoNavTestScenarioResponse, RunTopoNavTestScenario, RunTopoNavTestScenarioResponse, GetRouteTo -from roslib.packages import find_resource -import time -from topological_navigation_msgs.srv import GetTopologicalMap, GetTopologicalMapRequest -from tf import TransformListener -import tf.transformations as trans -from topological_navigation.load_maps_from_yaml import YamlMapLoader -from scitos_teleop.msg import action_buttons -from scitos_msgs.srv import EnableMotors, ResetBarrierStop, ResetMotorStop - -HOST = 'localhost' -PORT = 4000 -PKG = "topological_navigation" - - -class ScenarioServer(object): - _id = 0 - _loaded = False - _robot_poses = [] - _distances = [0,0] - - def __init__(self, name): - rospy.loginfo("Starting scenario server") - self.robot = rospy.get_param("~robot", False) - conf_file = find_resource("topological_navigation", "scenario_server.yaml")[0] - rospy.loginfo("Reading config file: %s ..." % conf_file) - with open(conf_file,'r') as f: - conf = yaml.load(f) - self._robot_start_node = conf["robot_start_node"] - self._robot_goal_node = conf["robot_goal_node"] - self._obstacle_node_prefix = conf["obstacle_node_prefix"] - self._obstacle_types = conf["obstacle_types"] - self._timeout = conf["success_metrics"]["nav_timeout"] - rospy.loginfo(" ... done") - self._insert_maps() - self.tf = TransformListener() - rospy.Service("~load", LoadTopoNavTestScenario, self.load_scenario) - self.reset_pose = self.reset_pose_robot if self.robot else self.reset_pose_sim - rospy.Service("~reset", Empty, self.reset) - rospy.Service("~start", RunTopoNavTestScenario, self.start) - rospy.loginfo("All done") - - def _insert_maps(self): - map_dir = rospy.get_param("~map_dir", "") - rospy.loginfo("Inserting maps into datacentre...") - if map_dir == "": - rospy.logwarn("No 'map_dir' given. Will not insert maps into datacentre and assume they are present already.") - return - y = YamlMapLoader() - y.insert_maps(y.read_maps(map_dir)) - rospy.loginfo("... done") - - def robot_callback(self, msg): - self._robot_poses.append(msg) - - def _connect_port(self, port): - """ Establish the connection with the given MORSE port""" - sock = None - - for res in socket.getaddrinfo(HOST, port, socket.AF_UNSPEC, socket.SOCK_STREAM): - af, socktype, proto, canonname, sa = res - try: - sock = socket.socket(af, socktype, proto) - except socket.error: - sock = None - continue - try: - sock.connect(sa) - except socket.error: - sock.close() - sock = None - continue - break - - return sock - - def _translate(self, msg, target_tf): - t = self.tf.getLatestCommonTime(target_tf, msg.header.frame_id) - msg.header.stamp=t - new_pose=self.tf.transformPose(target_tf,msg) - return new_pose - - def _clear_costmaps(self): - try: - s = rospy.ServiceProxy("move_base/clear_costmaps", Empty) - s.wait_for_service() - s() - except rospy.ServiceException as e: - rospy.logerr(e) - - def _init_nav(self, pose): - pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=1) - rospy.sleep(1.0) - initialPose = PoseWithCovarianceStamped() - initialPose.header = pose.header - initialPose.pose.pose = pose.pose - p_cov = np.array([0.0]*36) - initialPose.pose.covariance = tuple(p_cov.ravel().tolist()) - pub.publish(initialPose) - - def _get_set_object_pose_command(self, agent, id, pose): - new_pose = self._translate( - msg=pose, - target_tf="/world" - ) - return 'id%d simulation set_object_pose ["%s", "[%f, %f, 0.1]", "[%f, %f, %f, %f]"]\n' \ - % (id, agent, new_pose.pose.position.x, new_pose.pose.position.y, \ - new_pose.pose.orientation.w, new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z) - - def robot_start_dist(self, msg): - self._distances = self._get_pose_distance(msg, self._robot_start_pose.pose) - - def reset_pose_robot(self): - rospy.loginfo("Enabling freerun ...") - try: - s = rospy.ServiceProxy("enable_motors", EnableMotors) - s.wait_for_service() - s(False) - except (rospy.ROSInterruptException, rospy.ServiceException) as e: - rospy.logwarn(e) - rospy.loginfo("... enabled") - - while self._robot_start_node != rospy.wait_for_message("current_node", String).data and not rospy.is_shutdown(): - rospy.loginfo("+++ Please push the robot to '%s' +++" % self._robot_start_node) - rospy.sleep(1) - rospy.loginfo("+++ Robot at '%s' +++" % self._robot_start_node) - - while not rospy.is_shutdown(): - sub = rospy.Subscriber("robot_pose", Pose, self.robot_start_dist) - rospy.loginfo("+++ Please confirm correct positioning with A button on joypad: distance %.2fm %.2fdeg +++" %(self._distances[0], self._distances[1])) - if self._robot_start_node != rospy.wait_for_message("current_node", String).data: - self.reset_pose() - return - try: - if rospy.wait_for_message("teleop_joystick/action_buttons", action_buttons, timeout=1.).A: - break - except rospy.ROSException: - pass - sub.unregister() - sub = None - rospy.loginfo("+++ Position confirmed +++") - - rospy.loginfo("Enabling motors, resetting motor stop and barrier stopped ...") - try: - s = rospy.ServiceProxy("enable_motors", EnableMotors) - s.wait_for_service() - s(True) - s = rospy.ServiceProxy("reset_motorstop", ResetMotorStop) - s.wait_for_service() - s() - s = rospy.ServiceProxy("reset_barrier_stop", ResetBarrierStop) - s.wait_for_service() - s() - except (rospy.ROSInterruptException, rospy.ServiceException) as e: - rospy.logwarn(e) - rospy.loginfo("... enabled and reset") - - def _send_socket(self, sock, agent, pose): - sock.send( - self._get_set_object_pose_command( - agent, - self._id, - pose - ) - ) - res = sock.recv(4096) - self._id += 1 - if "FAILURE" in res: - raise Exception(res) - - def _get_quaternion_distance(self, q1, q2): - q1 = (q1.x, q1.y, q1.z, q1.w) - q2 = (q2.x, q2.y, q2.z, q2.w) - return (trans.euler_from_quaternion(q1)[2] - trans.euler_from_quaternion(q2)[2]) * 180/np.pi - - def _get_euclidean_distance(self, p1, p2): - return np.sqrt(np.power(p2.x - p1.x, 2) + np.power(p2.y - p1.y, 2)) - - def _get_pose_distance(self, p1, p2): - e = self._get_euclidean_distance(p1.position, p2.position) - q = self._get_quaternion_distance(p1.orientation, p2.orientation) - return e, q - - def reset_pose_sim(self): - sock = self._connect_port(PORT) - if not sock: - raise Exception("Could not create socket connection to morse") - - # Clean up whole scene - # Create pose outside of map - clear_pose = PoseStamped( - header=Header(stamp=rospy.Time.now(), frame_id="map"), - pose=Pose(position=Point(x=20, y=20, z=0.01)) - ) - - # Moving obstacles to clear_pose - for obstacle in self._obstacle_types: - for idx in range(10): - self._send_socket(sock, obstacle+str(idx), clear_pose) - - # Setting robot and obstacles to correct position - self._send_socket(sock, "robot", self._robot_start_pose) - - if len(self._obstacle_poses) > 0: - for idx, d in enumerate(self._obstacle_poses): - try: - obstacle = max([x for x in self._obstacle_types if x in d["name"]], key=len) - except ValueError: - rospy.logwarn("No obstacle specified for obstacle node '%s', will use '%s'." % (d["name"], self._obstacle_types[0])) - obstacle = self._obstacle_types[0] - rospy.loginfo("Adding obstacle %s%i" % (obstacle,idx)) - self._send_socket(sock, obstacle+str(idx), d["pose"]) - else: - rospy.logwarn("No nodes starting with '%s' found in map. Assuming test without obstacles." % self._obstacle_node_prefix) - - sock.close() - - while not rospy.is_shutdown(): - rpose = rospy.wait_for_message("robot_pose", Pose) - rospy.loginfo("Setting initial amcl pose ...") - self._init_nav(self._robot_start_pose) - dist = self._get_pose_distance(rpose, self._robot_start_pose.pose) - if dist[0] < 0.1 and dist[1] < 10: - break - rospy.sleep(0.2) -# self._robot_start_pose.header.stamp = rospy.Time.now() - rospy.loginfo(" ... pose set.") - - def reset(self, req): - if not self._loaded: - rospy.logfatal("No scenario loaded!") - return EmptyResponse() - - self.client.cancel_all_goals() - - rospy.loginfo("Resetting robot position...") - - self.reset_pose() - - self._clear_costmaps() - rospy.loginfo("... reset successful") - return EmptyResponse() - - def graceful_fail(self): - res = False - closest_node = rospy.wait_for_message("closest_node", String).data - rospy.loginfo("Closest node: %s" % closest_node) - if closest_node != self._robot_start_node: - rospy.loginfo("Using policy execution from %s to %s" % (closest_node, self._robot_start_node)) - s = rospy.ServiceProxy("get_simple_policy/get_route_to", GetRouteTo) - s.wait_for_service() - policy = s(self._robot_start_node) - self.client.send_goal(ExecutePolicyModeGoal(route=policy.route)) - self.client.wait_for_result(timeout=rospy.Duration(self._timeout)) - res = self.client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED - self.client.cancel_all_goals() - else: - rospy.loginfo("Using topo nav from %s to %s" % (closest_node, self._robot_start_node)) - rospy.loginfo("Starting topo nav client...") - client = actionlib.SimpleActionClient("topological_navigation", GotoNodeAction) - client.wait_for_server() - rospy.loginfo(" ... done") - client.send_goal(GotoNodeGoal(target=self._robot_start_node)) - client.wait_for_result(timeout=rospy.Duration(self._timeout)) - res = client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED - client.cancel_all_goals() - return res - - def start(self, req): - rospy.loginfo("Starting test ...") - - if not self._loaded: - rospy.logfatal("No scenario loaded!") - return RunTopoNavTestScenarioResponse(False, False) - - self._robot_poses = [] - grace_res = False - sub = rospy.Subscriber("robot_pose", Pose, self.robot_callback) - rospy.loginfo("Sending goal to policy execution ...") - print(self._policy.route) - self.client.send_goal(ExecutePolicyModeGoal(route=self._policy.route)) - t = time.time() - rospy.loginfo("... waiting for result ...") - print(self._timeout) - nav_timeout = not self.client.wait_for_result(timeout=rospy.Duration(self._timeout)) - elapsed = time.time() - t - res = self.client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED - rospy.loginfo("... policy execution finished") - self.client.cancel_all_goals() - if not res: - rospy.loginfo("Attempting graceful death ...") - grace_res = self.graceful_fail() - rospy.loginfo("... dead") - sub.unregister() - sub = None - distance = self.get_distance_travelled(self._robot_poses) - rospy.loginfo("... test done") - return RunTopoNavTestScenarioResponse( - nav_success=res, - nav_timeout=nav_timeout, - graceful_fail=grace_res, - human_success=False, - min_distance_to_human=0, - distance_travelled=distance, - travel_time=elapsed, - mean_speed=distance/elapsed - ) - - def get_distance_travelled(self, poses): - distance = 0.0 - for idx in range(len(poses))[1:]: - distance += self._get_euclidean_distance(poses[idx].position, poses[idx-1].position) - return distance - - def load_scenario(self, req): - # No try except to have exception break the test - s = rospy.ServiceProxy("/topological_map_manager/switch_topological_map", GetTopologicalMap) - s.wait_for_service() - topo_map = s(GetTopologicalMapRequest(pointset=req.pointset)).map - - self.pointset = req.pointset - - self._robot_start_pose = None - self._obstacle_poses = [] - found_end_node = False - for node in topo_map.nodes: - if node.name == self._robot_start_node: - self._robot_start_pose = PoseStamped( - header=Header(stamp=rospy.Time.now(), frame_id="map"), - pose=node.pose - ) - elif node.name == self._robot_goal_node: - found_end_node = True - elif node.name.startswith(self._obstacle_node_prefix): - self._obstacle_poses.append({ - "name": node.name.lower(), - "pose": PoseStamped( - header=Header(stamp=rospy.Time.now(), frame_id="map"), - pose=node.pose - ) - }) - - if self._robot_start_pose == None: - raise Exception("Topological map '%s' does not contain start node '%s'." % (req.pointset, self._robot_start_node)) - if not found_end_node: - raise Exception("Topological map '%s' does not contain goal node '%s'." % (req.pointset, self._robot_goal_node)) - - rospy.loginfo("Starting policy execution client...") - # Has to be done here because the policy execution server waits for a topo map. - self.client = actionlib.SimpleActionClient("topological_navigation/execute_policy_mode", ExecutePolicyModeAction) - self.client.wait_for_server() - rospy.loginfo(" ... started") - - self._loaded = True - self.reset(None) - - # No try except to have exception break the test - rospy.loginfo("Getting route ...") - s = rospy.ServiceProxy("get_simple_policy/get_route_to", GetRouteTo) - s.wait_for_service() - self._policy = s(self._robot_goal_node) - rospy.loginfo(" ... done") - - return LoadTopoNavTestScenarioResponse() - -if __name__ == "__main__": - rospy.init_node("scenario_server") - s = ScenarioServer(rospy.get_name()) - rospy.spin() diff --git a/topological_navigation/tests/topological_navigation_tester_critical.py b/topological_navigation/tests/topological_navigation_tester_critical.py deleted file mode 100755 index 5db7d5ef..00000000 --- a/topological_navigation/tests/topological_navigation_tester_critical.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python - -import unittest -import rospy -from topological_navigation_msgs.srv import LoadTopoNavTestScenario, RunTopoNavTestScenario - - -PKG = 'topological_navigation' - - -class TestTopologicalNavigation(unittest.TestCase): - _map_name = 'mb_test' - - def __init__(self, *args): - super(self.__class__, self).__init__(*args) - rospy.init_node('topological_navigation_tester') - - def _run(self, map_name): - try: - l = rospy.ServiceProxy("/scenario_server/load", LoadTopoNavTestScenario) - l.wait_for_service(timeout=60.) - l(map_name) - s = rospy.ServiceProxy("/scenario_server/start", RunTopoNavTestScenario) - s.wait_for_service(timeout=60.) - res = s() - except (rospy.ServiceException, rospy.ROSInterruptException) as e: - rospy.logfatal(e) - self.assertTrue(False) - return res - - def test_static_l_shaped_corridor_2m(self): - res = self._run(self._map_name+str(0)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - -if __name__ == '__main__': - import rostest - - rostest.rosrun(PKG, 'topological_navigation_tester', TestTopologicalNavigation) - diff --git a/topological_navigation/tests/topological_navigation_tester_supplementary.py b/topological_navigation/tests/topological_navigation_tester_supplementary.py deleted file mode 100755 index 19b82acc..00000000 --- a/topological_navigation/tests/topological_navigation_tester_supplementary.py +++ /dev/null @@ -1,126 +0,0 @@ -#!/usr/bin/env python - -import unittest -import rospy -from topological_navigation_msgs.srv import LoadTopoNavTestScenario, RunTopoNavTestScenario - - -PKG = 'topological_navigation' - - -class TestTopologicalNavigation(unittest.TestCase): - _map_name = 'mb_test' - - def __init__(self, *args): - super(self.__class__, self).__init__(*args) - rospy.init_node('topological_navigation_tester') - - def _run(self, map_name): - try: - l = rospy.ServiceProxy("/scenario_server/load", LoadTopoNavTestScenario) - l.wait_for_service(timeout=60.) - l(map_name) - s = rospy.ServiceProxy("/scenario_server/start", RunTopoNavTestScenario) - s.wait_for_service(timeout=60.) - res = s() - except (rospy.ServiceException, rospy.ROSInterruptException) as e: - rospy.logfatal(e) - self.assertTrue(False) - return res - - def test_static_facing_wall_10cm_distance_0_degrees_goal_behind(self): - res = self._run(self._map_name+str(1)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_facing_wall_10cm_distance_minus_45_degrees_goal_behind(self): - res = self._run(self._map_name+str(2)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_facing_wall_10cm_distance_plus_45_degrees_goal_behind(self): - res = self._run(self._map_name+str(3)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_155cm_corridor_with_55cm_chairs_on_one_side(self): - res = self._run(self._map_name+str(4)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_210cm_corridor_with_55cm_chairs_on_both_sides(self): - res = self._run(self._map_name+str(5)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_80cm_wide_door(self): - res = self._run(self._map_name+str(6)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_70cm_wide_door(self): - res = self._run(self._map_name+str(7)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_trapped_in_corner(self): - res = self._run(self._map_name+str(8)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_1m_striaght_corridor(self): - res = self._run(self._map_name+str(9)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_1m_l_shaped_corridor(self): - res = self._run(self._map_name+str(10)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_wheelchair_on_intermediate_point(self): - res = self._run(self._map_name+str(11)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_wheelchair_on_end_point(self): - res = self._run(self._map_name+str(12)) - rospy.loginfo(res) - self.assertTrue(res.graceful_fail and not res.nav_timeout) # Cannot reach final node and navigation should fail without timeout - - def test_static_human_on_intermediate_point(self): - res = self._run(self._map_name+str(13)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_human_on_end_point(self): - res = self._run(self._map_name+str(14)) - rospy.loginfo(res) - self.assertTrue(res.graceful_fail and not res.nav_timeout) # Cannot reach final node and navigation should fail without timeout - - def test_static_chairs_on_one_side_of_corridor(self): - res = self._run(self._map_name+str(15)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_wheelchairs_on_one_side_of_corridor(self): - res = self._run(self._map_name+str(16)) - rospy.loginfo(res) - self.assertTrue(res.nav_success) - - def test_static_corridor_blocked_by_wheelchairs(self): - res = self._run(self._map_name+str(17)) - rospy.loginfo(res) - self.assertTrue(res.graceful_fail and not res.nav_timeout) # Cannot reach final node and navigation should fail without timeout - - def test_static_corridor_blocked_by_humans(self): - res = self._run(self._map_name+str(18)) - rospy.loginfo(res) - self.assertTrue(res.graceful_fail and not res.nav_timeout) # Cannot reach final node and navigation should fail without timeout - - -if __name__ == '__main__': - import rostest - - rostest.rosrun(PKG, 'topological_navigation_tester', TestTopologicalNavigation) - diff --git a/topological_navigation/tests/travel_time_estimator.test b/topological_navigation/tests/travel_time_estimator.test deleted file mode 100644 index 8736f20b..00000000 --- a/topological_navigation/tests/travel_time_estimator.test +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/topological_navigation/tests/travel_time_tester.py b/topological_navigation/tests/travel_time_tester.py deleted file mode 100755 index c035c003..00000000 --- a/topological_navigation/tests/travel_time_tester.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python - -import sys -import unittest -import topological_navigation.testing -import rospy -import math -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation.publisher import map_publisher -from topological_navigation.publisher import map_publisher -from topological_navigation_msgs.srv import EstimateTravelTime - - -class TestTravelTimeEstimator(unittest.TestCase): -# class TestTravelTimeEstimator(): - def __init__(self, *args): - super(TestTravelTimeEstimator, self).__init__(*args) - rospy.init_node('test_travel_time_estimator') - - def test_travel_time_estimator(self): - - # create a test topological map - width = 5 - height = 5 - nodeSeparation = 10.0 - - test_nodes = topological_navigation.testing.create_cross_map(width = width, height = height, nodeSeparation = nodeSeparation) - self.assertEqual(len(test_nodes), width + height - 1) - startIndex = -(width/2) - endIndex = (width/2) - startNode = test_nodes['h_%s' % startIndex] - self.assertIsNotNone(startNode) - endNode = test_nodes['h_%s' % endIndex] - self.assertIsNotNone(endNode) - - # locally check distance - dist = math.hypot((startNode.pose.position.x-endNode.pose.position.x),(startNode.pose.position.y-endNode.pose.position.y)) - self.assertEqual(dist, (width - 1) * nodeSeparation ) - - # now insert the map into the database - msg_store = MessageStoreProxy(collection='topological_maps') - - meta = {} - meta['map'] = 'test_travel_time_estimator_map' - meta['pointset'] = 'test_travel_time_estimator_map' - - for (nodeName, node) in test_nodes.items(): - meta["node"] = nodeName - node.map = meta['map'] - node.pointset = meta['pointset'] - msg_store.insert(node,meta) - - # and publish the map - ps = map_publisher('test_travel_time_estimator_map') - - # now wait for the distance service - time_srv_name = 'topological_navigation/travel_time_estimator' - rospy.wait_for_service(time_srv_name, timeout=10) - time_srv = rospy.ServiceProxy(time_srv_name, EstimateTravelTime) - time_estimate = time_srv(startNode.name, endNode.name).travel_time - # the time should be (at least for now) at least as long as the straight-line distance at 1m/s - self.assertGreaterEqual(time_estimate.to_sec(), (width - 1) * nodeSeparation) - - -if __name__ == '__main__': - import rostest - PKG = 'topological_navigation' - rostest.rosrun(PKG, 'test_travel_time_estimator', TestTravelTimeEstimator) - diff --git a/topological_navigation/topological_navigation/convert_tmap.py b/topological_navigation/topological_navigation/convert_tmap.py new file mode 100644 index 00000000..df6f29cd --- /dev/null +++ b/topological_navigation/topological_navigation/convert_tmap.py @@ -0,0 +1,556 @@ +#!/usr/bin/env python +"""Convert legacy topological maps to the new map-driven format. + +The old format stores ``action_type`` on every edge and uses CamelCase +action names (e.g. ``NavigateToPose``, ``RowOperation``). Nodes carry +``localise_by_topic`` and ``parent_frame`` directly. There is no +top-level ``definitions`` or ``actions`` section. + +The new format centralises action configuration in top-level +``definitions`` (BT XML blobs) and ``actions`` (mapping action names +to Nav2 servers, types, composability, and goal templates). Edges +reference action names only; ``action_type`` is removed. Node-level +``localise_by_topic`` and ``parent_frame`` are dropped (handled +elsewhere in the new architecture). + +Usage:: + + ros2 run topological_navigation convert_tmap.py input.yaml -o output.tmap2.yaml + ros2 run topological_navigation convert_tmap.py input.yaml # writes to stdout + +The script auto-discovers which action types appear in edges and +generates default ``actions`` entries. Custom BT XML can be supplied +via ``--bt-dir`` pointing to a directory of ``.xml`` files whose +stems become definition names. +""" + +import argparse +import os +import re +import sys + +import yaml + + +# ===================================================================== +# Action name mapping (old CamelCase -> new snake_case keys) +# ===================================================================== + +_ACTION_NAME_MAP = { + 'NavigateToPose': 'navigate_to_pose', + 'NavigateThroughPoses': 'navigate_through_poses', + 'FollowWaypoints': 'follow_waypoints', + 'RowOperation': 'row_traversal', + 'RowTraversal': 'row_traversal', + 'GoalAlign': 'goal_align', +} + +# Default action_type (ROS 2 dotted import) for each new action name. +_DEFAULT_ACTION_TYPES = { + 'navigate_to_pose': 'nav2_msgs.action.NavigateToPose', + 'navigate_through_poses': 'nav2_msgs.action.NavigateThroughPoses', + 'follow_waypoints': 'nav2_msgs.action.FollowWaypoints', + 'row_traversal': 'nav2_msgs.action.NavigateThroughPoses', + 'goal_align': 'nav2_msgs.action.NavigateToPose', +} + +_DEFAULT_ACTION_SERVERS = { + 'navigate_to_pose': '/navigate_to_pose', + 'navigate_through_poses': '/navigate_through_poses', + 'follow_waypoints': '/follow_waypoints', + 'row_traversal': '/navigate_through_poses', + 'goal_align': '/navigate_to_pose', +} + +# Multi-waypoint actions are composable; single-pose actions are not. +_DEFAULT_COMPOSABLE = { + 'navigate_to_pose': False, + 'navigate_through_poses': True, + 'follow_waypoints': True, + 'row_traversal': True, + 'goal_align': False, +} + +# Goal template: single-pose vs multi-pose +_SINGLE_POSE_ACTIONS = {'navigate_to_pose', 'goal_align'} +_MULTI_POSE_ACTIONS = { + 'navigate_through_poses', 'follow_waypoints', 'row_traversal', +} + + +# ===================================================================== +# Default BT XML templates +# ===================================================================== + +_DEFAULT_BT = """\ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +""" + +_ROW_TRAVERSAL_BT = """\ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +""" + +# Which BT definition name to use for each action +_ACTION_BT_MAP = { + 'navigate_to_pose': 'default_bt', + 'navigate_through_poses': 'default_bt', + 'follow_waypoints': 'default_bt', + 'goal_align': 'goal_align_bt', + 'row_traversal': 'row_traversal_bt', +} + +# Built-in BT definitions (used when --bt-dir is not supplied) +_BUILTIN_BT_DEFS = { + 'default_bt': _DEFAULT_BT, + 'goal_align_bt': _DEFAULT_BT, # same XML, different name + 'row_traversal_bt': _ROW_TRAVERSAL_BT, +} + + +# ===================================================================== +# Conversion helpers +# ===================================================================== + +def _normalise_action_type(old_type): + """Convert ``nav2_msgs/action/X`` (slash) to ``nav2_msgs.action.X`` (dot).""" + if '/' in old_type: + return old_type.replace('/', '.') + return old_type + + +def _map_action_name(old_name): + """Map old CamelCase action name to new snake_case name.""" + if old_name in _ACTION_NAME_MAP: + return _ACTION_NAME_MAP[old_name] + # Fallback: convert CamelCase to snake_case + s1 = re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', old_name) + return re.sub(r'([a-z0-9])([A-Z])', r'\1_\2', s1).lower() + + +def _discover_actions(nodes): + """Scan all edges and return a dict of unique action configs. + + Returns ``{new_action_name: action_type_dotted}`` for every + distinct ``(action, action_type)`` pair found in edges. + + If the new action name has a known default action_type (e.g. + ``row_traversal`` -> ``NavigateThroughPoses``), that default + takes priority over the old edge-level ``action_type``, which + was often incorrect in legacy maps. + """ + actions = {} # new_name -> dotted action_type + for node_entry in nodes: + nd = node_entry.get('node', {}) + for edge in nd.get('edges', []): + old_name = edge.get('action', '') + old_type = edge.get('action_type', '') + new_name = _map_action_name(old_name) + + if new_name in actions: + continue + + # Prefer the known default for this action name; fall + # back to the old edge-level type only for unknown actions. + if new_name in _DEFAULT_ACTION_TYPES: + actions[new_name] = _DEFAULT_ACTION_TYPES[new_name] + elif old_type: + actions[new_name] = _normalise_action_type(old_type) + else: + actions[new_name] = 'nav2_msgs.action.NavigateToPose' + + return actions + + +def _build_actions_section(discovered, bt_defs): + """Build the top-level ``actions`` dict from discovered actions. + + Parameters + ---------- + discovered : dict + ``{action_name: dotted_action_type}`` from ``_discover_actions``. + bt_defs : dict + ``{bt_name: xml_string}`` of available BT definitions. + """ + actions = {} + for name, action_type in sorted(discovered.items()): + bt_name = _ACTION_BT_MAP.get(name) + # If no predefined BT mapping, pick a reasonable default + if bt_name is None: + bt_name = 'default_bt' + + # Determine goal template based on action type. + # The template mirrors the ROS 2 goal structure: pose/poses + # contain PoseStamped dicts with header and pose sub-fields. + _pose_stamped_tpl = { + 'header': {'frame_id': '${node.nav_frame}'}, + 'pose': '${node.pose}', + } + if name in _SINGLE_POSE_ACTIONS: + goal_tpl = { + 'pose': dict(_pose_stamped_tpl), + } + elif name in _MULTI_POSE_ACTIONS: + goal_tpl = { + 'poses': [dict(_pose_stamped_tpl)], + } + else: + # Guess from action_type name + type_tail = action_type.rsplit('.', 1)[-1] + if 'Through' in type_tail or 'Waypoint' in type_tail: + goal_tpl = {'poses': [dict(_pose_stamped_tpl)]} + else: + goal_tpl = {'pose': dict(_pose_stamped_tpl)} + + if bt_name in bt_defs: + goal_tpl['behavior_tree'] = '${definitions.%s}' % bt_name + + actions[name] = { + 'composable': _DEFAULT_COMPOSABLE.get(name, False), + 'action_type': action_type, + 'action_server': _DEFAULT_ACTION_SERVERS.get( + name, '/' + name, + ), + 'action_goal_template': goal_tpl, + } + return actions + + +def _build_definitions(discovered, bt_dir, bt_defs): + """Build the ``definitions`` dict. + + If ``bt_dir`` is given, load ``.xml`` files from it. + Otherwise, use built-in defaults for referenced BT names. + """ + needed = set() + for name in discovered: + bt_name = _ACTION_BT_MAP.get(name, 'default_bt') + needed.add(bt_name) + + definitions = {} + + if bt_dir and os.path.isdir(bt_dir): + for fname in sorted(os.listdir(bt_dir)): + if fname.endswith('.xml'): + stem = fname[:-4] + path = os.path.join(bt_dir, fname) + with open(path, 'r') as fh: + definitions[stem] = fh.read() + # Also add any needed builtins not found on disk + for bt_name in needed: + if bt_name not in definitions and bt_name in bt_defs: + definitions[bt_name] = bt_defs[bt_name] + else: + for bt_name in sorted(needed): + if bt_name in bt_defs: + definitions[bt_name] = bt_defs[bt_name] + + return definitions + + +def _convert_node(node_entry): + """Convert a single node entry from old to new format. + + - Removes ``action_type`` from edges. + - Maps edge ``action`` names to snake_case. + - Removes ``localise_by_topic`` and ``parent_frame`` from the node. + - Removes ``tag`` from meta. + """ + new_entry = {} + + # -- meta -------------------------------------------------------- + old_meta = node_entry.get('meta', {}) + new_meta = { + 'map': old_meta.get('map', ''), + 'node': old_meta.get('node', ''), + 'pointset': old_meta.get('pointset', ''), + } + new_entry['meta'] = new_meta + + # -- node -------------------------------------------------------- + old_node = node_entry.get('node', {}) + new_node = {} + + # Edges: convert action names, drop action_type + new_edges = [] + for edge in old_node.get('edges', []): + new_edge = { + 'action': _map_action_name(edge.get('action', '')), + 'edge_id': edge.get('edge_id', ''), + 'node': edge.get('node', ''), + } + props = edge.get('properties') + if props: + new_edge['properties'] = props + new_edges.append(new_edge) + new_node['edges'] = new_edges + + # Core fields + new_node['name'] = old_node.get('name', '') + new_node['pose'] = old_node.get('pose', {}) + + # Properties (keep as-is) + props = old_node.get('properties') + if props: + new_node['properties'] = props + + # Verts (influence zone) + verts = old_node.get('verts') + if verts: + new_node['verts'] = verts + + new_entry['node'] = new_node + return new_entry + + +def _convert_transformation(old_tf): + """Convert transformation block. + + Renames ``child`` to ``topo_frame_id`` if present. + """ + new_tf = {} + for k, v in old_tf.items(): + if k == 'child': + new_tf['topo_frame_id'] = v + else: + new_tf[k] = v + # Ensure topo_frame_id exists + if 'topo_frame_id' not in new_tf: + new_tf['topo_frame_id'] = old_tf.get( + 'child', old_tf.get('topo_frame_id', ''), + ) + return new_tf + + +def convert_tmap(old_data, bt_dir=None): + """Convert an old-format topological map dict to the new format. + + Parameters + ---------- + old_data : dict + Parsed YAML of an old-format topological map. + bt_dir : str or None + Optional path to a directory of ``.xml`` BT files. + + Returns + ------- + dict + New-format topological map. + """ + new_data = {} + + # -- Top-level metadata ------------------------------------------ + new_data['meta'] = old_data.get('meta', {}) + + for key in ('metric_map', 'name', 'pointset'): + if key in old_data: + new_data[key] = old_data[key] + + # -- Transformation ---------------------------------------------- + old_tf = old_data.get('transformation', {}) + new_data['transformation'] = _convert_transformation(old_tf) + + # -- Discover actions from edges --------------------------------- + old_nodes = old_data.get('nodes', []) + discovered = _discover_actions(old_nodes) + + # -- Definitions (BT XML) --------------------------------------- + definitions = _build_definitions( + discovered, bt_dir, _BUILTIN_BT_DEFS, + ) + new_data['definitions'] = definitions + + # -- Actions ----------------------------------------------------- + new_data['actions'] = _build_actions_section( + discovered, definitions, + ) + + # -- Nodes ------------------------------------------------------- + new_data['nodes'] = [_convert_node(n) for n in old_nodes] + + return new_data + + +# ===================================================================== +# CLI +# ===================================================================== + +def main(args=None): + """Entry point for ``convert_tmap.py``.""" + parser = argparse.ArgumentParser( + description=( + 'Convert a legacy topological map YAML to the new ' + 'map-driven format with definitions and actions sections.' + ), + ) + parser.add_argument( + 'input', + help='Path to the old-format topological map YAML file.', + ) + parser.add_argument( + '-o', '--output', + default=None, + help=( + 'Output file path. If omitted, writes to stdout. ' + 'Recommended extension: .tmap2.yaml' + ), + ) + parser.add_argument( + '--bt-dir', + default=None, + help=( + 'Directory containing .xml BT files to embed as ' + 'definitions. File stems become definition names ' + '(e.g. default_bt.xml -> definitions.default_bt).' + ), + ) + parser.add_argument( + '--no-definitions', + action='store_true', + help=( + 'Omit the definitions section (no inline BT XML). ' + 'Useful if BTs are managed externally.' + ), + ) + + parsed = parser.parse_args(args) + + # -- Load input -------------------------------------------------- + input_path = parsed.input + if not os.path.isfile(input_path): + print("Error: file not found: %s" % input_path, file=sys.stderr) + sys.exit(1) + + with open(input_path, 'r') as fh: + old_data = yaml.safe_load(fh) + + if not isinstance(old_data, dict): + print("Error: input is not a valid YAML mapping", file=sys.stderr) + sys.exit(1) + + # Quick check: if it already has 'actions', it may already be new format + if 'actions' in old_data and 'definitions' in old_data: + print( + "Warning: input already has 'actions' and 'definitions' " + "sections -- it may already be in the new format.", + file=sys.stderr, + ) + + # -- Convert ----------------------------------------------------- + new_data = convert_tmap(old_data, bt_dir=parsed.bt_dir) + + if parsed.no_definitions: + new_data.pop('definitions', None) + # Remove BT refs from action templates + for act_cfg in new_data.get('actions', {}).values(): + tpl = act_cfg.get('action_goal_template', {}) + tpl.pop('behavior_tree', None) + + # -- Output ------------------------------------------------------ + output_yaml = yaml.dump( + new_data, + default_flow_style=False, + sort_keys=False, + allow_unicode=True, + width=120, + ) + + if parsed.output: + out_dir = os.path.dirname(parsed.output) + if out_dir: + os.makedirs(out_dir, exist_ok=True) + with open(parsed.output, 'w') as fh: + fh.write(output_yaml) + print( + "Converted: %s -> %s" % (input_path, parsed.output), + file=sys.stderr, + ) + else: + sys.stdout.write(output_yaml) + + +if __name__ == '__main__': + main() diff --git a/topological_navigation/topological_navigation/edge_action_manager.py b/topological_navigation/topological_navigation/edge_action_manager.py deleted file mode 100644 index dc02c713..00000000 --- a/topological_navigation/topological_navigation/edge_action_manager.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python -""" -Created on Tue Apr 13 22:02:24 2021 -@author: Adam Binch (abinch@sagarobotics.com) - -""" -######################################################################################################### -import json, yaml -import operator, collections, copy - -from functools import reduce # forward compatibility for Python 3 -# from rospy_message_converter import message_converter -from actionlib_msgs.msg import GoalStatus - - -def _import(location, name): - mod = __import__(location, fromlist=[name]) - return getattr(mod, name) - - -class dict_tools(object): - - - def get_paths_from_nested_dict(self, nested): - paths = list(self.nested_dict_iter(nested)) - return [{"keys": item[0], "value": item[1]} for item in paths] - - - def nested_dict_iter(self, nested, prefix=""): - """ - Recursively loops through a nested dictionary. - For each inner-most value generates the list of keys needed to access it. - """ - for key, value in nested.items(): - path = "{},{}".format(prefix, key) - if isinstance(value, collections.Mapping): - for inner_key, inner_value in self.nested_dict_iter(value, path): - yield inner_key, inner_value - else: - yield path[1:].split(","), value - - - def getFromDict(self, dataDict, mapList): - return reduce(operator.getitem, mapList, dataDict) - - - def setInDict(self, dataDict, mapList, value): - self.getFromDict(dataDict, mapList[:-1])[mapList[-1]] = value - return dataDict -######################################################################################################### - - -######################################################################################################### -class EdgeActionManager(object): - - - def __init__(self): - - self.client = None - self.current_action = "none" - self.dt = dict_tools() - - - def initialise(self, edge, destination_node, origin_node=None): - - self.edge = yaml.safe_load(json.dumps(edge)) # no unicode in edge - self.destination_node = destination_node - self.origin_node = origin_node - - rospy.loginfo("Edge Action Manager: Processing edge {}".format(self.edge["edge_id"])) - - self.action_name = self.edge["action"] - if self.action_name != self.current_action: - self.preempt() - - action_type = self.edge["action_type"] - items = action_type.split("/") - package = items[0] - action_spec = items[1][:-4] + "Action" - - rospy.loginfo("Edge Action Manager: Importing {} from {}.msg".format(action_spec, package)) - action = _import(package+".msg", action_spec) - - rospy.loginfo("Edge Action Manager: Creating a {} client".format(self.action_name.upper())) - self.client = actionlib.SimpleActionClient(self.action_name, action) - self.client.wait_for_server() - - rospy.loginfo("Edge Action Manager: Constructing the goal") - self.construct_goal(action_type, copy.deepcopy(self.edge["goal"])) - - - def preempt(self): - - if self.client is not None: - status = self.client.get_state() - if status == GoalStatus.PENDING or status == GoalStatus.ACTIVE: - self.client.cancel_all_goals() - - - def construct_goal(self, action_type, goal_args): - - paths = self.dt.get_paths_from_nested_dict(goal_args) - - for item in paths: - value = item["value"] - - if isinstance(value, str): - - if value.startswith("$"): - _property = self.dt.getFromDict(self.destination_node, value[1:].split(".")) - goal_args = self.dt.setInDict(goal_args, item["keys"], _property) - - elif value.startswith("+") and self.origin_node is not None: - _property = self.dt.getFromDict(self.origin_node, value[1:].split(".")) - goal_args = self.dt.setInDict(goal_args, item["keys"], _property) - - self.goal = message_converter.convert_dictionary_to_ros_message(action_type, goal_args) - - - def execute(self): - - rospy.loginfo("Edge Action Manager: Executing the action...") - self.client.send_goal(self.goal) - self.current_action = self.action_name -######################################################################################################### \ No newline at end of file diff --git a/topological_navigation/topological_navigation/edge_action_manager2.py b/topological_navigation/topological_navigation/edge_action_manager2.py deleted file mode 100644 index 93af6631..00000000 --- a/topological_navigation/topological_navigation/edge_action_manager2.py +++ /dev/null @@ -1,1364 +0,0 @@ -#!/usr/bin/env python -""" -Created on Tue Nov 5 22:02:24 2023 -@author: Geesara Kulathunga (ggeesara@gmail.com) - -""" -######################################################################################################### -import json, yaml -import operator, collections, copy -import rclpy -import numpy as np -from functools import reduce # forward compatibility for Python 3 -from rclpy.action import ActionClient -from action_msgs.msg import GoalStatus -from nav2_msgs.action import NavigateThroughPoses, NavigateToPose, FollowWaypoints, ComputePathToPose, ComputePathThroughPoses -from geometry_msgs.msg import PoseStamped -from nav_msgs.msg import Path -from nav_msgs.msg import Odometry -from topological_navigation.route_search2 import TopologicalRouteSearch2 -import math - -# from rospy_message_converter import message_converter -# from actionlib_msgs.msg import GoalStatus -from std_msgs.msg import Header -from builtin_interfaces.msg import Time -from action_msgs.msg import GoalStatus -from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy, QoSDurabilityPolicy -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup -from unique_identifier_msgs.msg import UUID -from rclpy.executors import SingleThreadedExecutor -from topological_navigation.scripts.actions_bt import ActionsType -from topological_navigation.scripts.param_processing import ParameterUpdaterNode -from topological_navigation.scripts.in_row_operations import RowOperations -from std_msgs.msg import String - - -try: - from collections.abc import Mapping -except ImportError: - from collections import Mapping - - -def _import(location, name): - mod = __import__(location, fromlist=[name]) - return getattr(mod, name) - - -class dict_tools(object): - - - def get_paths_from_nested_dict(self, nested): - paths = list(self.nested_dict_iter(nested)) - return [{"keys": item[0], "value": item[1]} for item in paths] - - - def nested_dict_iter(self, nested, prefix=""): - """ - Recursively loops through a nested dictionary. - For each inner-most value generates the list of keys needed to access it. - """ - for key, value in nested.items(): - path = "{},{}".format(prefix, key) - if isinstance(value, Mapping): - for inner_key, inner_value in self.nested_dict_iter(value, path): - yield inner_key, inner_value - else: - yield path[1:].split(","), value - - - def getFromDict(self, dataDict, mapList): - return reduce(operator.getitem, mapList, dataDict) - - - def setInDict(self, dataDict, mapList, value): - self.getFromDict(dataDict, mapList[:-1])[mapList[-1]] = value - return dataDict - - -class TopoNavEdgeActionMsg(): - - def __init__(self,): - pass - - - def setAction(self, action): - self.action = action - - - def setNavGoal(self, nav_goal): - self.nav_goal = nav_goal - - - def setSideEdges(self, side_edges, target_frame_id): - self.side_edges = side_edges - self.target_frame_id = target_frame_id - - - def getBoundary(self, ): - path = Path() - header = Header() - header.frame_id = self.target_frame_id - path.header = header - if(len(self.side_edges)>0): - for key, val in self.side_edges.items(): - for pose in val: - path.poses.append(pose) - return path - - - def getTargetFrameId(self, ): - return self.target_frame_id - - - def getAction(self): - return self.action - - - def getNavGoal(self): - return self.nav_goal - - - def setControlPluginParams(self, control_plugin_params): - self.control_plugin_params = control_plugin_params - - -######################################################################################################### - - - -######################################################################################################### -class EdgeActionManager(rclpy.node.Node): - - - def __init__(self, name="edge_action_manager"): - super().__init__(name) - self.client = None - self.current_action = "none" - self.dt = dict_tools() - - - def init(self, ACTIONS, route_search, update_params_control_server, inrow_step_size=3.0, intermediate_dis=0.7): - self.ACTIONS = ACTIONS - self.route_search = route_search - self.goal_handle = None - self.goal_response = None - self.internal_executor = SingleThreadedExecutor() - self.latching_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, reliability=ReliabilityPolicy.RELIABLE) - self.bt_trees = {} - self.in_row_operation = False - self.control_server_configs = [] - self.inrow_step_size = inrow_step_size - self.is_row_boundary_published = False - self.intermediate_dis = intermediate_dis - self.nav2_client_callback_group = MutuallyExclusiveCallbackGroup() - - self.update_params_control_server = update_params_control_server - self.current_robot_pose = None - self.is_inside_tunnel = False - self.odom_sub = self.create_subscription(Odometry, '/odometry/global', self.odom_callback, - QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT)) - self.get_current_node_sub = self.create_subscription(String, 'closest_node', self.set_current_pose - , QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT)) - self.robot_nav_area_sub = self.create_subscription(String, '/robot_navigation_area', self.nav_area_callback, 10) - - self.boundary_publisher = self.create_publisher(Path, '/boundary_checker', qos_profile=self.latching_qos) - self.robot_current_status_pub = self.create_publisher(String, '/robot_operation_current_status', qos_profile=self.latching_qos) - self.current_dest = self.create_publisher(String, '/topological_navigation/current_destination', qos_profile=self.latching_qos) - self.target_edge_path_pub = self.create_publisher(Path, "/target_edge_path", qos_profile=self.latching_qos) - self.center_node_pose_pub = self.create_publisher(PoseStamped, "/center_node/pose", qos_profile=self.latching_qos) - - self.robot_current_behavior_pub = None - self.current_node = None - self.action_status = 0 - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE - self.executor_nav_client = SingleThreadedExecutor() - - - def set_current_pose(self, msg): - self.current_node = msg.data - - - def odom_callback(self, msg): - self.current_robot_pose = msg.pose - - - def nav_area_callback(self, msg): - self.nav_area = msg.data - self.is_inside_tunnel = (self.nav_area == 'INSIDE_POLYTUNNEL') - - - def get_nav_action_server_status(self, ): - return self.ACTIONS.status_mapping - - - def get_status_msg(self, status_code): - try: - return self.ACTIONS.status_mapping[status_code] - except Exception as e: - self.get_logger().error("Status code is invalid {}".format(status_code)) - return self.ACTIONS.status_mapping[0] - - def get_goal_cancel_error_msg(self, status_code): - try: - return self.ACTIONS.goal_cancel_error_codes[status_code] - except Exception as e: - self.get_logger().error("Goal cancel code {}".format(status_code)) - return self.ACTIONS.goal_cancel_error_codes[0] - - def _is_waypoint_name(self, name: str) -> bool: - n = (name or "").lower() - return ("waypoint" in n) or n.startswith("wp") or ("_wp" in n) - - def _is_row_node_name(self, name: str) -> bool: - # Uses your existing constant that you already rely on elsewhere - # (you used startswith(ROW_COLUMN_START_INDEX) in execute_row_operation_action) - if not name: - return False - n = str(name) - return n.split("-")[-1].startswith(self.ACTIONS.ROW_COLUMN_START_INDEX) and (not self._is_waypoint_name(n)) - - def _yaw_from_quat(self, q) -> float: - # q: dict with x,y,z,w - x, y, z, w = q["x"], q["y"], q["z"], q["w"] - siny = 2.0 * (w * z + x * y) - cosy = 1.0 - 2.0 * (y * y + z * z) - return math.atan2(siny, cosy) - - def _set_pose_yaw(self, pose_stamped: PoseStamped, yaw: float): - pose_stamped.pose.orientation.x = 0.0 - pose_stamped.pose.orientation.y = 0.0 - pose_stamped.pose.orientation.z = math.sin(yaw / 2.0) - pose_stamped.pose.orientation.w = math.cos(yaw / 2.0) - - - def _adjust_orientations_for_next_wp(self, poses_dict): - """ - Adjusts the orientation of each pose to face the next one in the sequence. - The final pose's orientation is left unchanged. - """ - # Check if there are enough poses to perform the calculation - if len(poses_dict) < 2: - self.get_logger().warn("[_adjust_orientations_for_next_wp] Not enough poses to adjust orientations. Returning original poses.") - return poses_dict - - # 1. Create a flat list of the actual pose dictionaries. - # Sorting by key ensures the poses are in the correct sequential order. - flat_poses = [poses_dict[key][0] for key in sorted(poses_dict.keys())] - - # 2. Iterate up to the second-to-last pose to update its orientation - for i in range(len(flat_poses) - 1): - current_pos = flat_poses[i]['target_pose']['pose']['position'] - next_pos = flat_poses[i+1]['target_pose']['pose']['position'] - - # Calculate the yaw angle to face the next pose - delta_x = next_pos['x'] - current_pos['x'] - delta_y = next_pos['y'] - current_pos['y'] - yaw = math.atan2(delta_y, delta_x) - - # 3. Convert yaw to a quaternion and modify the orientation directly in the pose dictionary. - orientation = flat_poses[i]['target_pose']['pose']['orientation'] - orientation['w'] = math.cos(yaw / 2.0) - orientation['x'] = 0.0 - orientation['y'] = 0.0 - orientation['z'] = math.sin(yaw / 2.0) - self.get_logger().info("[_adjust_orientations_for_next_wp] Adjusted orientation for pose {} to face next goal".format(i)) - - return poses_dict - - def _process_and_segment_edges(self, edge, destination_node, origin_node, is_execpolicy): - """ - Processes a list of edges, groups them into segments based on the policy. - """ - self.get_logger().warn(f"[_process_and_segment_edges] - {'EXECUTE-POLICY' if is_execpolicy else 'GO-TO-NODE'}") - - poses, actions, edge_ids = {}, {}, {} - segment = 0 - previous_action = "" - total_edges = len(edge) - - for index in range(total_edges): - edge_i, dest_i, origin_i = edge[index], destination_node[index], origin_node[index] - edge_i = yaml.safe_load(json.dumps(edge_i)) - current_action_base = edge_i["action"] - - # Determine the effective current action (common logic) - if index < (total_edges - 1): - next_edge_id = edge[index + 1]["edge_id"] - current_action = self.get_goal_align_if(edge_i["edge_id"], current_action_base, next_edge_id) - self.get_logger().debug(f"[_process_and_segment_edges] - intermediate edge {edge_i['edge_id']} -> action {current_action}") - else: - current_action = self.get_goal_align_if(edge_i["edge_id"], current_action_base) - self.get_logger().debug(f"[_process_and_segment_edges] - final edge {edge_i['edge_id']} -> action {current_action}") - - # Determine segment number based on the policy (the only differing logic) - if not is_execpolicy: - # Group by action: increment segment only when action changes - if previous_action != current_action: segment += 1 - else: - # One edge per segment - segment = index - - # Construct and append data (common logic) - self.get_logger().info(f"Processing edge {edge_i['edge_id']} for segment {segment}") - intermediate_goal = self.construct_goal(copy.deepcopy(edge_i["goal"]), dest_i, origin_i) - - if segment not in poses: - self.get_logger().debug(f"[_process_and_segment_edges] - Creating new segment {segment}") - poses[segment], actions[segment], edge_ids[segment] = [], [], [] - - poses[segment].append(intermediate_goal) - actions[segment].append(current_action) - edge_ids[segment].append(edge_i["edge_id"]) - - previous_action = current_action - - return poses, actions, edge_ids - - - def initialise(self, bt_trees, edge, destination_node, origin_node=None, - action_name=None, package="nav2_msgs.action", in_row_operation=False, is_execpolicy=False): - - self.bt_trees = bt_trees - self.in_row_operation = in_row_operation - if self.in_row_operation: - try: - from robot_behavior_msg.msg import RobotBehavior - self.robot_current_behavior_pub = self.create_publisher(RobotBehavior, '/robot_current_behavior', qos_profile=self.latching_qos) - except Exception as e: - self.get_logger().error("robot_behavior_msg.msg.RobotBehavior message type is not defined") - - if action_name is not None: - self.action_name = action_name - else: - self.edge = yaml.safe_load(json.dumps(edge)) # no unicode in edge - self.action_name = self.edge["action"] - - if (self.action_name == self.ACTIONS.ROW_TRAVERSAL or self.action_name == self.ACTIONS.ROW_CHANGE): - self.action_name = self.ACTIONS.NAVIGATE_TO_POSE #TODO change this to actual - - if self.action_name != self.current_action: - self.preempt() - - self.package = package - self.set_nav_client() - self.action_status = 0 - - if self.action_name == self.ACTIONS.NAVIGATE_TO_POSE: - self.destination_node = destination_node - self.origin_node = origin_node - self.get_logger().info(f"Processing edge {self.edge['edge_id']}") - self.get_logger().info("Constructing the goal") - target_pose = self.construct_goal(copy.deepcopy(edge["goal"]), destination_node, origin_node) - self.action_msgs = self.construct_navigate_to_pose_goal(target_pose) - - if self.action_name == self.ACTIONS.NAVIGATE_THROUGH_POSES: - - self.destination_node_str = {} - for item in destination_node: - name = item['node']['name'] - x_coord = item['node']['pose']['position']['x'] - y_coord = item['node']['pose']['position']['y'] - coordinates = (x_coord, y_coord) - self.destination_node_str[coordinates] = name - self.get_logger().warn(f"Destination nodes: {self.destination_node_str}") - - poses, actions, edge_ids = self._process_and_segment_edges(edge, destination_node, origin_node, is_execpolicy) - if is_execpolicy: poses = self._adjust_orientations_for_next_wp(poses) - - self.destination_node = destination_node[-1] - self.action_msgs, self.control_server_configs = self.construct_navigate_through_poses_goal(poses, actions, edge_ids, is_execpolicy=is_execpolicy) - - return True - - - def get_goal_align_if(self, edge_id, current_action, next_edge_id=None): - edges = edge_id.split("_") - if next_edge_id is not None: - next_edge_ids = next_edge_id.split("_") - if len(next_edge_ids) == 2: - next_goal_stage = next_edge_ids[1].split("-") - if len(next_goal_stage) == 2: - if (next_goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX) or \ - (next_goal_stage[1] not in self.ACTIONS.GOAL_ALIGN_GOAL): - return current_action - elif len(next_goal_stage) == 1: - if current_action == self.ACTIONS.ROW_TRAVERSAL: - return current_action - if len(edges) == 2: - goal = edges[1] - goal_stage = goal.split("-") - if len(goal_stage) == 2: - if goal_stage[1] in self.ACTIONS.GOAL_ALIGN_INDEX and not self.is_inside_tunnel: - return self.ACTIONS.GOAL_ALIGN - - return current_action - - - def set_nav_client(self): - self.action_server_name = self.get_action_server_name(self.action_name) - self.get_logger().info("Importing {} from {}".format(self.action_name, self.package)) - try: - action = _import(self.package, self.action_name) - except Exception as e: - self.get_logger().error("Still does not support action type: {}".format(self.action_name)) - return False - - self.get_logger().info("Creating a {} client".format(self.action_server_name)) - self.action = action - self.client = ActionClient(self, self.action, self.action_server_name, callback_group=self.nav2_client_callback_group) - - - def feedback_callback(self, feedback_msg): - self.nav_client_feedback = feedback_msg - return - - - def preempt_feedback_callback(self, feedback_msg): - self.nav_client_preempt_feedback = feedback_msg - self.get_logger().info("preempt: {} ".format(self.nav_client_preempt_feedback)) - return - - - def get_action_server_name(self, action_name): - action_topic = "" - for char in action_name: - if char.isupper(): - if(len(action_topic)>1): - action_topic += "_" - action_topic += char.lower() - else: - action_topic += char.lower() - else: - action_topic += char - return action_topic - - - def get_state(self,): - return self.action_status - - - def preempt(self, timeout_secs=2.0): - if self.client is not None: - if not self.client.server_is_ready(): - self.get_logger().info("Waiting for the action server {}...".format(self.action_server_name)) - self.client.wait_for_server(timeout_sec=2) - if not self.client.server_is_ready(): - self.get_logger().info("action server {} not responding ... can not perform any action".format(self.action_server_name)) - return True - if self.goal_handle is None: - self.get_logger().info("There is no goal to stop it is already cancelled with status {}".format(self.action_status)) - return True - - counter = 0 - - try: - cancel_future = self.goal_handle.cancel_goal_async() - rclpy.spin_until_future_complete(self, cancel_future, timeout_sec=5.0) - self.get_logger().info("Waiting till terminating the current preemption") - self.action_status = 5 - self.get_logger().info("The goal cancel error code {} ".format(self.get_goal_cancel_error_msg(cancel_future.result().return_code))) - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_NATURAL_STATE - self.publish_robot_current_status_msg(self.ACTIONS.NAVIGATE_THROUGH_POSES, self.robot_current_status) - return True - except Exception as e: - self.get_logger().error("Something wrong with Nav2 Control server {} while preempting {}".format(e, self.action_server_name)) - return True - - - def construct_goal(self, goal_args, destination_node, origin_node): - paths = self.dt.get_paths_from_nested_dict(goal_args) - for item in paths: - value = item["value"] - if isinstance(value, str): - if value.startswith("$"): - _property = self.dt.getFromDict(destination_node, value[1:].split(".")) - goal_args = self.dt.setInDict(goal_args, item["keys"], _property) - - elif value.startswith("+") and origin_node is not None: - _property = self.dt.getFromDict(origin_node, value[1:].split(".")) - goal_args = self.dt.setInDict(goal_args, item["keys"], _property) - goal = goal_args - return goal - - - def construct_navigate_to_pose_goal(self, goal): - action_msgs = self.get_navigate_to_pose_goal(goal["target_pose"]["header"]["frame_id"], goal["target_pose"]["pose"]) - return action_msgs - - - def get_navigate_to_pose_goal(self, frame_id, goal): - nav_goal = NavigateToPose.Goal() - target_pose = self.create_pose_stamped_msg(frame_id, goal) - nav_goal.pose = target_pose - action_msgs = [] - action_msg = TopoNavEdgeActionMsg() - self.get_logger().info("Action: {}".format(self.ACTIONS.NAVIGATE_TO_POSE)) - if(self.ACTIONS.NAVIGATE_TO_POSE in self.bt_trees): - nav_goal.behavior_tree = self.bt_trees[self.ACTIONS.NAVIGATE_TO_POSE] - self.get_logger().info("Bt_tree: {}".format(self.bt_trees[self.ACTIONS.NAVIGATE_TO_POSE])) - - action_msg.setAction(self.ACTIONS.NAVIGATE_TO_POSE) - action_msg.setNavGoal(nav_goal) - action_msgs.append(action_msg) - return action_msgs - - - def construct_navigate_through_poses_goal(self, goals, actions, edge_ids, is_execpolicy=False): - action_msgs, control_server_configs = self.get_navigate_through_poses_goal(goals, actions, edge_ids, is_execpolicy=is_execpolicy) - return action_msgs, control_server_configs - - - def check_edges_area_same(self, side_edges): - edge_poses = [] - if(len(side_edges) >= 2): - for key, val in side_edges.items(): - for pose in val: - edge_poses.append(np.array([pose.pose.position.x, pose.pose.position.y])) - if(len(edge_poses) == 2): - if(np.linalg.norm(edge_poses[0]-edge_poses[1]) < 0.001): - return True - return False - - def check_target_is_same(self, node1, node2): - target1 = np.array([node1["pose"]["position"]["x"], node1["pose"]["position"]["y"]]) - target2 = np.array([node2["pose"]["position"]["x"], node2["pose"]["position"]["y"]]) - return np.linalg.norm(target1 - target2) < 0.001 - - def two_smallest_indices(self, lst): - if len(lst) < 1: - return [] - if len(lst) < 2: - return [0] - lst_copy = lst[:] - min_index1 = lst_copy.index(min(lst_copy)) - lst_copy[min_index1] = float('inf') - min_index2 = lst_copy.index(min(lst_copy)) - return [min_index1, min_index2] - - def extract_number(self, s): - return float(s.split('-')[0][1:]) - - def publish_target_edges_as_path(self, selected_edges_dict): - """ - Convert selected edges dictionary to a ROS Path and publish. - Safely handles placeholder values like '$node.pose' by substituting - with actual node pose from selected_edges_dict['node']['pose']. - """ - path_msg = Path() - node_info = selected_edges_dict.get("node", {}) - node_pose = node_info.get("pose", {}) - parent_frame = node_info.get("parent_frame", "map") # fallback to 'map' - - path_msg.header.frame_id = parent_frame - path_msg.header.stamp = self.get_clock().now().to_msg() - - if not node_pose: - self.get_logger().warn("Node pose missing, cannot publish path") - return - - valid_poses_count = 0 - - for edge in node_info.get("edges", []): - target_pose_data = edge.get("goal", {}).get("target_pose", {}) - - # Check if pose is placeholder - if target_pose_data.get("pose") == "$node.pose": - pose_source = node_pose - elif isinstance(target_pose_data.get("pose"), dict): - pose_source = target_pose_data["pose"] - else: - self.get_logger().warn( - f"Skipping edge {edge.get('edge_id')} due to invalid target_pose" - ) - continue - - # Create PoseStamped - try: - pose_msg = PoseStamped() - pose_msg.header.frame_id = parent_frame - pose_msg.header.stamp = self.get_clock().now().to_msg() - pose_msg.pose.position.x = pose_source["position"]["x"] - pose_msg.pose.position.y = pose_source["position"]["y"] - pose_msg.pose.position.z = pose_source["position"]["z"] - pose_msg.pose.orientation.x = pose_source["orientation"]["x"] - pose_msg.pose.orientation.y = pose_source["orientation"]["y"] - pose_msg.pose.orientation.z = pose_source["orientation"]["z"] - pose_msg.pose.orientation.w = pose_source["orientation"]["w"] - except KeyError as e: - self.get_logger().warn( - f"Skipping edge {edge.get('edge_id')} due to missing key: {e}" - ) - continue - - path_msg.poses.append(pose_msg) - valid_poses_count += 1 - - self.get_logger().info(f"Publishing Path with {valid_poses_count} poses") - self.target_edge_path_pub.publish(path_msg) - - def _publish_empty_boundary(self, frame_id: str): - """Publish an empty boundary path.""" - empty = Path() - empty.header.frame_id = frame_id - empty.header.stamp = self.get_clock().now().to_msg() - self.boundary_publisher.publish(empty) - - def _get_row_center_node(self, edge_id: str): - """ - Parse edge_id to find the row center node. - Returns: (center_node, tag_id, target_row_edge_id) or (None, None, None) on failure. - """ - try: - # edge_id = r7.5-c20_r7.5-c19 - target_row_edge_id_raw = edge_id.split("_")[0] # e.g. 'r7.5-c20' - tag_id = target_row_edge_id_raw.split("-")[1] # e.g. 'c20' - # print trget_row_edge_id_raw, tag_id - self.get_logger().info(f"[_get_row_center_node] Parsed edge_id='{edge_id}' to target_row_edge_id_raw='{target_row_edge_id_raw}', tag_id='{tag_id}'") - # Force ROW_START_INDEX by replacing the last character - # e.g. 'r7.5-c20' -> 'r7.5-c2a' if ROW_START_INDEX='a' - target_row_edge_id = target_row_edge_id_raw.split("c", 1)[0] + "c" + self.ACTIONS.ROW_START_INDEX # target_row_edge_id_raw[:-1] + self.ACTIONS.ROW_START_INDEX - tag_id = tag_id[0] + self.ACTIONS.ROW_START_INDEX - #print target_row_edge_id, tag_id - self.get_logger().info(f"[_get_row_center_node] Adjusted to target_row_edge_id='{target_row_edge_id}', tag_id='{tag_id}'") - except Exception as e: - self.get_logger().error(f"[_get_row_center_node] Failed to parse edge_id='{edge_id}': {e}") - return None, None, None - - cen = self.route_search.get_node_from_tmap2(target_row_edge_id) - if not cen or "node" not in cen or "pose" not in cen["node"]: - self.get_logger().error(f"[_get_row_center_node] Could not resolve '{target_row_edge_id}'") - return None, None, None - - return cen, tag_id, target_row_edge_id - - def _collect_boundary_candidates(self, cen, target_row_edge_id: str): - """ - Collect candidate boundary nodes from connected WayPoint nodes. - Returns: dict of node_id -> (pose_dict, xy_np) - """ - candidates = {} - children = self.route_search.get_connected_nodes_tmap2(cen) or [] - - for next_edge in children: - if not next_edge.startswith(self.ACTIONS.OUTSIDE_EDGE_START_INDEX): - continue - - next_edge_node = self.route_search.get_node_from_tmap2(next_edge) - if not next_edge_node or "node" not in next_edge_node: - continue - - for edge_info in next_edge_node["node"].get("edges", []): - node_id = edge_info.get("node", "") - if not node_id or not isinstance(node_id, str): - continue - # Must contain GOAL_ALIGN_INDEX and not be the target row itself - if self.ACTIONS.GOAL_ALIGN_INDEX[0] not in node_id: - continue - if target_row_edge_id in node_id: - continue - if self._is_waypoint_name(node_id): - continue - - node_obj = self.route_search.get_node_from_tmap2(node_id) - if not node_obj or "node" not in node_obj or "pose" not in node_obj["node"]: - continue - - pose = node_obj["node"]["pose"] - xy = np.array([pose["position"]["x"], pose["position"]["y"]], dtype=float) - candidates[node_id] = (pose, xy) - - return candidates - - def _select_boundary_nodes(self, candidates, center_xy, row_dir, tag_id): - """ - Select left and right boundary nodes relative to the row direction. - Returns: list of selected node_ids (up to 2). - """ - scored = [] # (side_sign, dist, node_id) - - for node_id, (_pose, txy) in candidates.items(): - if tag_id not in node_id: - continue - - v = txy - center_xy - dist = float(np.linalg.norm(v)) - if dist < 1e-6: - continue - - # Reject nodes too aligned with row direction (want perpendicular/lateral nodes) - cosang = abs(float(np.dot(row_dir, v) / dist)) - if cosang > 0.6: - continue - - # Determine which side (left=positive, right=negative) - crossz = float(row_dir[0] * v[1] - row_dir[1] * v[0]) - side = 1.0 if crossz > 0.0 else -1.0 - scored.append((side, dist, node_id)) - - if not scored: - return [] - - # Pick closest on each side - left = min((c for c in scored if c[0] > 0.0), default=None, key=lambda x: x[1]) - right = min((c for c in scored if c[0] < 0.0), default=None, key=lambda x: x[1]) - - picked = [] - if left: - picked.append(left[2]) - if right: - picked.append(right[2]) - - # Fallback: fill up to 2 from remaining candidates - if len(picked) < 2: - for _, _, nid in sorted(scored, key=lambda x: x[1]): - if nid not in picked: - picked.append(nid) - if len(picked) == 2: - break - - return picked - - def _select_last_row_goal(self, nodes): - """Select the last goal that is a ROW node (not a waypoint).""" - selected = None - for g in nodes: - try: - p = g["target_pose"]["pose"]["position"] - xy = (p["x"], p["y"]) - name = getattr(self, "destination_node_str", {}).get(xy) - if name and self._is_row_node_name(name): - selected = g - except Exception: - continue - return selected if selected else nodes[-1] - - def _handle_row_operation(self, nodes, edge_id, action_msg): - """ - Build row boundaries and return (action, action_msg). - Simplified logic for selecting boundary nodes. - """ - # Validate inputs - if not nodes: - self.get_logger().error("[_handle_row_operation] nodes list is empty") - self._publish_empty_boundary("map") - action_msg.setSideEdges({}, "map") - return self.ACTIONS.ROW_OPERATION, action_msg - - frame_id = nodes[0]["target_pose"]["header"].get("frame_id", "map") - - if not edge_id: - self.get_logger().error("[_handle_row_operation] edge_id is empty") - self._publish_empty_boundary(frame_id) - action_msg.setSideEdges({}, frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg - - # Get row center node - cen, tag_id, target_row_edge_id = self._get_row_center_node(edge_id) - if not cen: - self._publish_empty_boundary(frame_id) - action_msg.setSideEdges({}, frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg - - self.get_logger().info(f"[_handle_row_operation] center={target_row_edge_id}, tag={tag_id}") - - # get target_row_edge_id pose and orientation - target_row_edge_node = self.route_search.get_node_from_tmap2(target_row_edge_id) - if target_row_edge_node and "node" in target_row_edge_node: - target_row_edge_pose = target_row_edge_node["node"]["pose"] - self.get_logger().info(f"Target Row Edge Pose: {target_row_edge_pose}") - else: - self.get_logger().warn(f"[_handle_row_operation] Could not find node for target_row_edge_id: {target_row_edge_id}") - - try: - # print out cen info - # self.get_logger().info(f"Center Node Info: {cen}") - # get target edges from cen and publish as pose stamped message - node_info = cen.get("node", {}) - node_pose = node_info.get("pose", {}) - parent_frame = node_info.get("parent_frame", "map") - # get the node IDs from cen edges - selected_edges_dict = {"node": {"parent_frame": parent_frame, "pose": node_pose, "edges": []}} - # print node pose - self.get_logger().info(f"Center Node Pose: {node_pose}") - - msg = PoseStamped() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = 'map' - msg.pose.position.x = node_pose["position"]["x"] - msg.pose.position.y = node_pose["position"]["y"] - msg.pose.position.z = node_pose["position"]["z"] - msg.pose.orientation.x = node_pose["orientation"]["x"] - msg.pose.orientation.y = node_pose["orientation"]["y"] - msg.pose.orientation.z = node_pose["orientation"]["z"] - msg.pose.orientation.w = node_pose["orientation"]["w"] - self.center_node_pose_pub.publish(msg) - self.get_logger().info('Published latched Center Node Pose') - - except Exception as e: - self.get_logger().error(f"[_handle_row_operation] Failed to extract center node info: {e}") - - # Collect boundary candidates - candidates = self._collect_boundary_candidates(cen, target_row_edge_id) - if not candidates: - self.get_logger().error("[_handle_row_operation] No boundary candidates found") - self._publish_empty_boundary(frame_id) - action_msg.setSideEdges({}, frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg - - # Select last row goal and compute row direction - selected_last_node = self._select_last_row_goal(nodes) - cpos = cen["node"]["pose"]["position"] - lpos = selected_last_node["target_pose"]["pose"]["position"] - - # Avoid using center as last goal - if np.linalg.norm(np.array([lpos["x"] - cpos["x"], lpos["y"] - cpos["y"]])) < 1e-6: - selected_last_node = nodes[0] - lpos = selected_last_node["target_pose"]["pose"]["position"] - - dx, dy = lpos["x"] - cpos["x"], lpos["y"] - cpos["y"] - row_yaw = math.atan2(dy, dx) if abs(dx) + abs(dy) > 1e-6 else self._yaw_from_quat(cen["node"]["pose"]["orientation"]) - row_dir = np.array([math.cos(row_yaw), math.sin(row_yaw)], dtype=float) - center_xy = np.array([cpos["x"], cpos["y"]], dtype=float) - - # Select boundary nodes - picked = self._select_boundary_nodes(candidates, center_xy, row_dir, tag_id) - if not picked: - self.get_logger().error("[_handle_row_operation] No valid boundary nodes selected") - self._publish_empty_boundary(frame_id) - action_msg.setSideEdges({}, frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg - - # Build selected_edges with poses aligned to row direction - self.selected_edges = {} - for child in picked: - child_node = self.route_search.get_node_from_tmap2(child) - if not child_node or "node" not in child_node: - continue - - side_pose = self.get_intermediate_pose(cen, child_node, frame_id) - side_last_pose = self.get_last_intermediate_pose(side_pose, cen, selected_last_node) - - # self.get_logger().info(f"Side Pose: {side_pose}") - self.get_logger().info(f"Side Last Pose: {side_last_pose}") - self.get_logger().info(f"Row Yaw: {row_yaw}") - self.get_logger().info(f"Child Node ID: {child}") - # how to print the node orientation?? - self.get_logger().info(f"Child Node Orientation: {child_node['node']['pose']['orientation']}") - # print node orientation as yaw - child_yaw = self._yaw_from_quat(child_node["node"]["pose"]["orientation"]) - self.get_logger().info(f"Child Node Yaw: {child_yaw}") - - self._set_pose_yaw(side_pose, child_yaw) - self._set_pose_yaw(side_last_pose, child_yaw) - self.selected_edges[child] = [side_pose, side_last_pose] - - # Mirror if only one wall found - if len(self.selected_edges) == 1: - self.get_logger().info("[_handle_row_operation] Single wall found, creating mirror") - self.selected_edges["side_wall"] = self.get_intermediate_poses_interpolated( - self.selected_edges, cen, selected_last_node - ) - for ps in self.selected_edges["side_wall"]: - self._set_pose_yaw(ps, child_yaw) - - if not self.selected_edges: - self.get_logger().error("[_handle_row_operation] No edges built") - self._publish_empty_boundary(frame_id) - action_msg.setSideEdges({}, frame_id) - return self.ACTIONS.ROW_OPERATION, action_msg - - # Publish boundary - action_msg.setSideEdges(self.selected_edges, frame_id) - if not self.is_row_boundary_published: - boundary_info = action_msg.getBoundary() - boundary_info.header.stamp = self.get_clock().now().to_msg() - self.boundary_publisher.publish(boundary_info) - self.is_row_boundary_published = True - - return self.ACTIONS.ROW_OPERATION, action_msg - - def get_navigate_through_poses_goal(self, poses, actions, edge_ids, is_execpolicy=False): - - - - control_server_configs = {} - action_msgs = [] - self.is_row_boundary_published = False - edge_action_is_valid = True - - - # ================================================================== - # IF in Execute-Policy Mode: relaxed yaw tolerance for all segments - # ================================================================== - if is_execpolicy: - control_server_configs = [] - - self.get_logger().debug("[get_navigate_through_poses_goal] Executing with Execute-policy logic.") - - for seg_i, nodes in poses.items(): - nav_goal = NavigateThroughPoses.Goal() - for pose in nodes: - target_pose = self.create_pose_stamped_msg(pose["target_pose"]["header"]["frame_id"], pose["target_pose"]["pose"]) - nav_goal.poses.append(target_pose) - - action = actions[seg_i][0] - edge_id = edge_ids[seg_i][0] - action_msg = TopoNavEdgeActionMsg() - - self.get_logger().info("seg: {}, action: {}, edge id: {} ".format(seg_i, action, edge_id)) - - - self.get_logger().info(f"Segment {seg_i} -- action {action}") - current_config = None - if action in self.ACTIONS.bt_tree_with_control_server_config: - controller_plugin = self.ACTIONS.bt_tree_with_control_server_config[action] - current_config = self.ACTIONS.planner_with_goal_checker_config[controller_plugin].copy() - - if action == self.ACTIONS.NAVIGATE_TO_POSE and seg_i < len(poses) - 1: - self.get_logger().info(f"Segment {seg_i} is intermediate NAVIGATE_TO_POSE. Relaxing yaw tolerance.") - current_config['goal_checker.yaw_goal_tolerance'] = 2 * math.pi - - control_server_configs.append(current_config) - - if current_config: - self.get_logger().warn(f"Segment {seg_i} | XY tol: {current_config['goal_checker.xy_goal_tolerance']} | Yaw tol: {current_config['goal_checker.yaw_goal_tolerance']}") - - - if action in self.bt_trees: - nav_goal.behavior_tree = self.bt_trees[action] - edge_action_is_valid = True - - if action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation and nodes: - self.get_logger().warn(f"Segment {seg_i}: ROW_TRAVERSAL with in_row_operation") - action, action_msg = self._handle_row_operation(nodes, edge_id, action_msg) - - self.get_logger().info(" Action {} Bt_tree : {}".format(action, nav_goal.behavior_tree)) - if edge_action_is_valid: - self.get_logger().warn(f"Segment {seg_i} edge_action_is_valid") - action_msg.setAction(action) - action_msg.setNavGoal(nav_goal) - action_msgs.append(action_msg) - - # ================================================================== - # ELSE (Go-To-Node Mode): Use the original logic - # ================================================================== - else: - - self.get_logger().debug("[get_navigate_through_poses_goal] Executing with Go-To-Node logic.") - - for seg_i, nodes in poses.items(): - nav_goal = NavigateThroughPoses.Goal() - for pose in nodes: - target_pose = self.create_pose_stamped_msg(pose["target_pose"]["header"]["frame_id"], pose["target_pose"]["pose"]) - nav_goal.poses.append(target_pose) - - action = actions[seg_i][0] - edge_id = edge_ids[seg_i][0] - action_msg = TopoNavEdgeActionMsg() - - self.get_logger().info("seg: {}, action: {}, edge id: {} ".format(seg_i, action, edge_id)) - - if action in self.ACTIONS.bt_tree_with_control_server_config: - controller_plugin = self.ACTIONS.bt_tree_with_control_server_config[action] - control_server_configs[action] = self.ACTIONS.planner_with_goal_checker_config[controller_plugin] - if action in self.bt_trees: - nav_goal.behavior_tree = self.bt_trees[action] - edge_action_is_valid = True - - if action == self.ACTIONS.ROW_TRAVERSAL and self.in_row_operation and nodes: - self.get_logger().warn(f"Segment {seg_i}: ROW_TRAVERSAL with in_row_operation") - action, action_msg = self._handle_row_operation(nodes, edge_id, action_msg) - - self.get_logger().info(" Action {} Bt_tree : {}".format(action, nav_goal.behavior_tree)) - if edge_action_is_valid: - action_msg.setAction(action) - action_msg.setNavGoal(nav_goal) - action_msgs.append(action_msg) - - self.get_logger().debug("[get_navigate_through_poses_goal] - Control server configs: {}".format(control_server_configs)) - return action_msgs, control_server_configs - - - def get_intermediate_poses_interpolated(self, side_poses, center_pose, last_pose): - - header = Header() - # header.stamp = self.get_clock().now().to_msg() - header.frame_id = last_pose["target_pose"]["header"]["frame_id"] - - side_edge_key = list(side_poses)[0] - side_front = side_poses[side_edge_key][0].pose.position - side_back = side_poses[side_edge_key][1].pose.position - center_pose = center_pose["node"]["pose"] - last_pose = last_pose["target_pose"]['pose'] - - side_front_opposite = PoseStamped() - side_front_opposite.header = header - side_front_opposite.pose.position.x = center_pose["position"]["x"] + -1.0*(side_front.x-center_pose["position"]["x"]) - side_front_opposite.pose.position.y = center_pose["position"]["y"] + -1.0*(side_front.y-center_pose["position"]["y"]) - side_front_opposite.pose.position.z = center_pose["position"]["z"] - side_front_opposite.pose.orientation.w = center_pose["orientation"]["w"] - side_front_opposite.pose.orientation.x = center_pose["orientation"]["x"] - side_front_opposite.pose.orientation.y = center_pose["orientation"]["y"] - side_front_opposite.pose.orientation.z = center_pose["orientation"]["z"] - - side_back_opposite = PoseStamped() - side_back_opposite.header = header - side_back_opposite.pose.position.x = last_pose["position"]["x"] + -1.0*(side_back.x-last_pose["position"]["x"]) - side_back_opposite.pose.position.y = last_pose["position"]["y"] + -1.0*(side_back.y-last_pose["position"]["y"]) - side_back_opposite.pose.position.z = last_pose["position"]["z"] - side_back_opposite.pose.orientation.w = last_pose["orientation"]["w"] - side_back_opposite.pose.orientation.x = last_pose["orientation"]["x"] - side_back_opposite.pose.orientation.y = last_pose["orientation"]["y"] - side_back_opposite.pose.orientation.z = last_pose["orientation"]["z"] - - return [side_front_opposite, side_back_opposite] - - def get_last_intermediate_pose(self, side_intermediate_pose, center_pose, last_pose): - header = Header() - # header.stamp = self.get_clock().now().to_msg() - header.frame_id = last_pose["target_pose"]["header"]["frame_id"] - target_pose = PoseStamped() - target_pose.header = header - intermediate_pose = side_intermediate_pose.pose.position - center_pose = center_pose["node"]["pose"] - last_pose = last_pose["target_pose"]['pose'] - target_pose.pose.position.x = last_pose["position"]["x"] + (intermediate_pose.x - center_pose["position"]["x"]) - target_pose.pose.position.y = last_pose["position"]["y"] + (intermediate_pose.y - center_pose["position"]["y"]) - target_pose.pose.position.z = center_pose["position"]["z"] - target_pose.pose.orientation.w = center_pose["orientation"]["w"] - target_pose.pose.orientation.x = center_pose["orientation"]["x"] - target_pose.pose.orientation.y = center_pose["orientation"]["y"] - target_pose.pose.orientation.z = center_pose["orientation"]["z"] - return target_pose - - - def get_intermediate_pose(self, pose1, pose2, header_frame_id, dev_factor=1.0): - header = Header() - # header.stamp = self.get_clock().now().to_msg() - header.frame_id = header_frame_id - target_pose = PoseStamped() - target_pose.header = header - pose1 = pose1["node"]["pose"] - pose2 = pose2["node"]["pose"] - if(dev_factor > 1.0): - target_pose.pose.position.x = (pose1["position"]["x"] + pose2["position"]["x"])/2.0 - target_pose.pose.position.y = (pose1["position"]["y"] + pose2["position"]["y"])/2.0 - else: - target_pose.pose.position.x = pose2["position"]["x"] - target_pose.pose.position.y = pose2["position"]["y"] - - target_pose.pose.position.z = pose1["position"]["z"] - target_pose.pose.orientation.w = pose1["orientation"]["w"] - target_pose.pose.orientation.x = pose1["orientation"]["x"] - target_pose.pose.orientation.y = pose1["orientation"]["y"] - target_pose.pose.orientation.z = pose1["orientation"]["z"] - return target_pose - - def create_pose_stamped_msg(self, frame_id, goal): - header = Header() - # header.stamp = self.get_clock().now().to_msg() - header.frame_id = frame_id - target_pose = PoseStamped() - target_pose.header = header - desired_target_pose = goal - target_pose.pose.position.x = desired_target_pose["position"]["x"] - target_pose.pose.position.y = desired_target_pose["position"]["y"] - target_pose.pose.position.z = desired_target_pose["position"]["z"] - target_pose.pose.orientation.w = desired_target_pose["orientation"]["w"] - target_pose.pose.orientation.x = desired_target_pose["orientation"]["x"] - target_pose.pose.orientation.y = desired_target_pose["orientation"]["y"] - target_pose.pose.orientation.z = desired_target_pose["orientation"]["z"] - return target_pose - - def crete_pose_stamped_msg_from_position(self, frame_id, goal): - header = Header() - # header.stamp = self.get_clock().now().to_msg() - header.frame_id = frame_id - target_pose = PoseStamped() - target_pose.header = header - target_pose.pose = goal.pose - return target_pose - - def get_result(self, ): - return self.goal_response - - def execute_row_operation_one_step(self, next_goal, target_pose_frame_id): - target_goal = NavigateThroughPoses.Goal() - if(self.ACTIONS.ROW_TRAVERSAL in self.bt_trees): - target_goal.behavior_tree = self.bt_trees[self.ACTIONS.ROW_TRAVERSAL] - self.get_logger().info("Row traversal BT path {}".format(target_goal.behavior_tree)) - else: - self.get_logger().info("Row traversal BT path not found") - target_pose = self.crete_pose_stamped_msg_from_position(target_pose_frame_id, next_goal) - target_goal.poses.append(target_pose) - - controller_plugin = self.ACTIONS.bt_tree_with_control_server_config[self.ACTIONS.ROW_TRAVERSAL] - - control_server_config = self.ACTIONS.planner_with_goal_checker_config[controller_plugin] - self.get_logger().info(" Control params {}".format(control_server_config)) - self.update_params_control_server.set_params(control_server_config) - - send_goal_future = self.client.send_goal_async(target_goal, feedback_callback=self.feedback_callback) - goal_accepted = self.send_goal_request(send_goal_future, self.ACTIONS.ROW_OPERATION) - if(goal_accepted == False): - return False - processed_goal = self.processing_goal_request(self.ACTIONS.ROW_OPERATION) - return processed_goal - - def send_goal_request(self, send_goal_future, msg): - while rclpy.ok(): - try: - # rclpy.spin_once(self) - # print("send_goal_future ", send_goal_future) - rclpy.spin_once(self, executor=self.executor_nav_client, timeout_sec=0.5) - # rclpy.spin_until_future_complete(self, send_goal_future, executor=self.executor_nav_client, timeout_sec=2.0) - if send_goal_future.done(): - self.goal_handle = send_goal_future.result() - break - except Exception as e: - self.get_logger().error("Nav2 server got some unexpected errors : {} while executing send_goal_request {}".format(e, msg)) - return False - - if not self.goal_handle.accepted: - self.get_logger().error('The goal rejected: {}'.format(msg)) - return False - self.get_logger().info('The goal accepted for {}'.format(msg)) - return True - - def processing_goal_request(self, target_action): - if self.goal_handle is None: - self.get_logger().error("Something wrong with the goal request, there is no goal to process {}".format(self.action_status)) - return True - self.goal_get_result_future = self.goal_handle.get_result_async() - self.get_logger().info("Waiting for {} action to complete".format(self.action_server_name)) - while rclpy.ok(): - try: - # rclpy.spin_once(self) - # print("goal_get_result_future ", self.goal_get_result_future) - rclpy.spin_once(self, executor=self.executor_nav_client, timeout_sec=1.5) - # rclpy.spin_until_future_complete(self, self.goal_get_result_future, executor=self.executor_nav_client, timeout_sec=2.0) - if self.goal_get_result_future.done(): - status = self.goal_get_result_future.result().status - self.action_status = status - self.get_logger().info("Executing the action response with status {}".format(self.get_status_msg(self.action_status))) - self.current_action = self.action_name - self.goal_response = self.goal_get_result_future.result() - if((self.get_status_msg(self.action_status) == "STATUS_EXECUTING") or (self.get_status_msg(self.action_status) == "STATUS_SUCCEEDED")): - self.get_logger().info("action is completed {}".format(target_action)) - break - if target_action in self.ACTIONS.ABORT_NOT_CONTINUE: - self.get_logger().info("action is not completed {}".format(target_action)) - return False - else: - self.get_logger().info("action is not completed {}. Executing next actions...".format(target_action)) - break - except Exception as e: - # self.goal_handle = Nosne - self.get_logger().error("Nav2 server got some unexpected errors: {} while executing processing_goal_request {}".format(e, target_action)) - return False - return True - - def execute_row_operation(self,): - target_goal = NavigateThroughPoses.Goal() - if(self.ACTIONS.ROW_OPERATION in self.bt_trees): - target_goal.behavior_tree = self.bt_trees[self.ACTIONS.ROW_OPERATION] - self.get_logger().info("Row operation BT path {}".format(target_goal.behavior_tree)) - else: - self.get_logger().error("Row operation BT is not provided {}") - return False - send_goal_future = self.client.send_goal_async(target_goal) - goal_accepted = self.send_goal_request(send_goal_future, self.ACTIONS.ROW_OPERATION) - if(goal_accepted == False): - return False - processed_goal = self.processing_goal_request(self.ACTIONS.ROW_OPERATION) - return processed_goal - - def execute_row_recovery(self, intermediate_pose, target_pose_frame_id): - target_goal = NavigateThroughPoses.Goal() - if(self.ACTIONS.ROW_RECOVERY in self.bt_trees): - target_goal.behavior_tree = self.bt_trees[self.ACTIONS.ROW_RECOVERY] - self.get_logger().info("Row recovery BT path {}".format(target_goal.behavior_tree)) - else: - self.get_logger().error("Row recovery BT is not provided {}") - return False - target_pose = self.crete_pose_stamped_msg_from_position(target_pose_frame_id, intermediate_pose) - target_goal.poses.append(target_pose) - send_goal_future = self.client.send_goal_async(target_goal) - goal_accepted = self.send_goal_request(send_goal_future, self.ACTIONS.ROW_RECOVERY) - if(goal_accepted == False): - return False - processed_goal = self.processing_goal_request(self.ACTIONS.ROW_RECOVERY) - return processed_goal - - def publish_robot_current_status_msg(self, action, status): - msg = String() - msg.data = status - self.robot_current_status_pub.publish(msg) - if self.robot_current_behavior_pub is not None: - from robot_behavior_msg.msg import RobotBehavior - robot_behavir = RobotBehavior() - robot_behavir.message = status - robot_behavir.behavior_code = self.ACTIONS.getCodeForRobotCurrentStatus(status) - self.robot_current_behavior_pub.publish(robot_behavir) - - def execute_row_operation_action(self, action_msg): - nav_goal = action_msg.getNavGoal() - boundary_info = action_msg.getBoundary() - target_frame_id = action_msg.getTargetFrameId() - self.boundary_publisher.publish(boundary_info) - self.is_row_boundary_published = False - - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_NATURAL_STATE - self.publish_robot_current_status_msg(self.ACTIONS.NAVIGATE_THROUGH_POSES, self.robot_current_status) - self.in_row_inter_pose = nav_goal.poses - - if len(self.in_row_inter_pose) > 0: - self.get_logger().info("Executing action : {} ".format(self.ACTIONS.ROW_OPERATION)) - while rclpy.ok(): - try: - # rclpy.spin_once(self) - # print("send_goal_future self.current_robot_pose ", self.current_robot_pose) - rclpy.spin_once(self, executor=self.executor_nav_client, timeout_sec=0.5) - if(self.current_robot_pose is not None): - # self.get_logger().info("Robot current pose: {}".format(self.current_robot_pose)) - break - except Exception as e: - self.allowed_row_operation = False - self.get_logger().error("Could not set the current pose of the robot {}".format(e, self.ACTIONS.ROW_OPERATION)) - return False - - if(len(self.in_row_inter_pose)==1): - self.in_row_inter_pose = [self.current_robot_pose, self.in_row_inter_pose[0]] - - self.get_logger().info(" ==========start executing in row operaitons============") - inrow_opt = RowOperations(self.in_row_inter_pose, self.inrow_step_size, intermediate_dis=self.intermediate_dis) - robot_init_pose = self.current_robot_pose - - if inrow_opt.isPlanCalculated(): - while True: - robot_init_pose = self.current_robot_pose - next_goal, intermediate_pose, get_to_goal = inrow_opt.getNextGoal(robot_init_pose) - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_PREPARATION_STATE - if(self.current_node is not None): - node_id = self.current_node.split("-") - if (len(node_id) == 2): - node_id = node_id[1] - if(node_id.startswith(self.ACTIONS.ROW_COLUMN_START_INDEX)): - if (self.ACTIONS.ROW_START_INDEX not in node_id): - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_HARVESTING_STATE - - self.publish_robot_current_status_msg(self.ACTIONS.ROW_OPERATION, self.robot_current_status) - done_operation = self.execute_row_operation() - self.get_logger().info("done_operation {} ".format(done_operation)) - # target_goal = NavigateThroughPoses.Goal() - self.get_logger().info("Robot current pose: {},{}" - .format(next_goal.pose.position.x, next_goal.pose.position.y)) - if(self.intermediate_dis > 0.0): - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_PREPARATION_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status) - get_proper_alignment = self.execute_row_recovery(intermediate_pose, target_frame_id) - if(get_proper_alignment == False): - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status) - return False - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status) - step_moved = self.execute_row_operation_one_step(next_goal, target_frame_id) - if(step_moved == False): - if(self.intermediate_dis > 0.0): - intermediate_pose, _ = inrow_opt.getNextIntermediateGoal(self.current_robot_pose) - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_RECOVERY_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status) - get_proper_alignment = self.execute_row_recovery(intermediate_pose, target_frame_id) - if(get_proper_alignment == False): - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status) - return False - else: - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status) - step_moved = self.execute_row_operation_one_step(next_goal, target_frame_id) - if(step_moved == False): - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status) - return False - else: - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status) - step_moved = self.execute_row_operation_one_step(next_goal, target_frame_id) - if(step_moved == False): - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status) - return False - if(get_to_goal): - if step_moved: - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_NATURAL_STATE - self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status) - self.get_logger().info("Reach to the final goal {},{}" - .format(next_goal.pose.position.x, next_goal.pose.position.y)) - - else: - self.get_logger().error("Can not reach to the final goal {},{}" - .format(next_goal.pose.position.x, next_goal.pose.position.y)) - break - return True - - - def execute(self): - self.get_logger().warn("===========================================Executing action===========================================") - if not self.client.server_is_ready(): - self.get_logger().info("Waiting for the action server {}...".format(self.action_server_name)) - self.client.wait_for_server(timeout_sec=2) - - if not self.client.server_is_ready(): - self.get_logger().info("action server {} not responding ... can not perform any action".format(self.action_server_name)) - return - - self.get_logger().info("Number of actions : {} ".format(len(self.action_msgs))) - for index, action_msg in enumerate(self.action_msgs): - target_goal, target_action = action_msg.getNavGoal(), action_msg.getAction() - self.get_logger().warn("Executing action : {} ".format(target_action)) - - # Publish segment destination for visualisation purposes - current_destination = (target_goal.poses[-1].pose.position.x, target_goal.poses[-1].pose.position.y) - self.get_logger().warn("Current destination : {} ".format(self.destination_node_str[current_destination])) - self.current_dest.publish(String(data=self.destination_node_str[current_destination])) - - # Handles both lists and dictionaries - control_server_config = None - if isinstance(self.control_server_configs, list): - # New exec_policy mode - if index < len(self.control_server_configs): - control_server_config = self.control_server_configs[index] - elif isinstance(self.control_server_configs, dict): - # Original go-to-node mode - if target_action in self.control_server_configs: - control_server_config = self.control_server_configs[target_action] - - # If a valid config was found, apply it. - if control_server_config: - self.get_logger().info(f"Applying parameters for segment {index}: {control_server_config}") - self.update_params_control_server.set_params(control_server_config) - - if (target_action == self.ACTIONS.ROW_OPERATION): - row_operation_is_completed = self.execute_row_operation_action(action_msg) - if(row_operation_is_completed == False): - return False - else: - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE - self.publish_robot_current_status_msg(self.ACTIONS.NAVIGATE_THROUGH_POSES, self.robot_current_status) - send_goal_future = self.client.send_goal_async(target_goal, feedback_callback=self.feedback_callback) - goal_accepted = self.send_goal_request(send_goal_future, target_action) - if(goal_accepted == False): - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE - self.publish_robot_current_status_msg(self.ACTIONS.NAVIGATE_THROUGH_POSES, self.robot_current_status) - return False - processed_goal = self.processing_goal_request(target_action) - if(processed_goal == False): - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE - self.publish_robot_current_status_msg(self.ACTIONS.NAVIGATE_THROUGH_POSES, self.robot_current_status) - return processed_goal - self.robot_current_status = self.ACTIONS.ROBOT_STATUS_NATURAL_STATE - self.publish_robot_current_status_msg(self.ACTIONS.NAVIGATE_THROUGH_POSES, self.robot_current_status) - return True diff --git a/topological_navigation/topological_navigation/edge_controller.py b/topological_navigation/topological_navigation/edge_controller.py deleted file mode 100644 index 9eefb0f1..00000000 --- a/topological_navigation/topological_navigation/edge_controller.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import math -import tf - -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point - -from visualization_msgs.msg import * -from interactive_markers.interactive_marker_server import * - -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation.topological_map import * - - - -class edge_controllers(object): - - def __init__(self) : - self.in_feedback=False - #self.timer = Timer(1.0, self.timer_callback) - #map_name = rospy.get_param('topological_map_name', 'top_map') - self.map_update = rospy.Publisher('/update_map', std_msgs.msg.Time, queue_size=10) - - self._edge_server = InteractiveMarkerServer("/topological_map_edges") - rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback) - - - def update_map(self, msg) : - print("updating edge controllers...") - - self.topo_map = topological_map(msg.name, msg=msg) - self._edge_server.clear() - self._edge_server.applyChanges() - - for node in self.topo_map.nodes : - for i in node.edges : - ind = self.topo_map._get_node_index(i['node']) - V1=Point() - V2=Point() - V1= (node._get_pose()).position - V2= (self.topo_map.nodes[ind]._get_pose()).position - edge_name=node.name+"_"+self.topo_map.nodes[ind].name - self._edge_marker(edge_name, V1, V2, edge_name) - - - """ - MapCallback - - This function receives the Topological Map - """ - def MapCallback(self, msg) : - self.update_map(msg) - - - - def makeEmptyMarker(self, dummyBox=True ): - int_marker = InteractiveMarker() - int_marker.header.frame_id = "map" - int_marker.scale = 1 - return int_marker - - - - def makeBox(self, msg ): - marker = Marker() - - marker.type = Marker.ARROW - marker.scale.x = msg.scale * 0.45 - marker.scale.y = msg.scale * 0.25 - marker.scale.z = msg.scale * 0.15 - marker.lifetime.secs = 3 - marker.color.r = 0.8 - marker.color.g = 0.1 - marker.color.b = 0.0 - marker.color.a = 1.0 - - return marker - - - - def _edge_marker(self, marker_name, point1, point2, marker_description="edge marker") : - marker = self.makeEmptyMarker() - marker.name = marker_name - #marker.description = marker_description - - control = InteractiveMarkerControl() - - control.interaction_mode = InteractiveMarkerControl.BUTTON - control.always_visible = True - - control.markers.append( self.makeBox( marker ) ) - marker.controls.append(control) - - self._edge_server.insert(marker, self.feedback_cb) - self._edge_server.applyChanges() - - pose = Pose() - - pose.position.x = (point1.x+point2.x)/2 - pose.position.y = (point1.y+point2.y)/2 - pose.position.z = ((point1.z+point2.z)/2)+0.15 - angle = math.atan2((point2.y-point1.y),(point2.x-point1.x)) - qat = tf.transformations.quaternion_from_euler(0, 0, angle) - pose.orientation.w = qat[3] - pose.orientation.x = qat[0] - pose.orientation.y = qat[1] - pose.orientation.z = qat[2] - - if pose is not None: - self._edge_server.setPose( marker.name, pose ) - self._edge_server.applyChanges() - - - - def feedback_cb(self, feedback): - self.topo_map.remove_edge(feedback.marker_name) - self._edge_server.erase(feedback.marker_name) - self._edge_server.applyChanges() - self.map_update.publish(rospy.Time.now()) - - - - def clear(): - self._edge_server.clear() - self._edge_server.applyChanges() - - - - def __del__(self): - del self._edge_server diff --git a/topological_navigation/topological_navigation/edge_reconfigure_manager.py b/topological_navigation/topological_navigation/edge_reconfigure_manager.py deleted file mode 100644 index 1789571d..00000000 --- a/topological_navigation/topological_navigation/edge_reconfigure_manager.py +++ /dev/null @@ -1,127 +0,0 @@ -#!/usr/bin/env python -""" -Created on Wed Feb 10 15:58:34 2021 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -############################################################################################################# -import rospy, dynamic_reconfigure.client -from topological_navigation_msgs.srv import ReconfAtEdges - - -class EdgeReconfigureManager(object): - - - def __init__(self): - - self.active = False - - self.edge = {} - self.initial_config = {} - self.edge_config = {} - self.namespaces = [] - - self.current_edge_group = "none" - self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {}) - - - def register_edge(self, edge): - - self.active = False - self.edge = edge - - namespaces = [] - if "config" in edge: - namespaces = [param["namespace"] for param in edge["config"]] - - self.namespaces = list(set(namespaces)) - if self.namespaces: - self.active = True - rospy.loginfo("Edge Reconfigure Manager: Processing edge {}".format(edge["edge_id"])) - - - def initialise(self): - - self.initial_config = {} - self.edge_config = {} - - for namespace in self.namespaces: - rospy.loginfo("Edge Reconfigure Manager: Getting initial configuration for {}".format(namespace)) - - client = dynamic_reconfigure.client.Client(namespace, timeout=2.0) - try: - config = client.get_configuration() - - except rospy.ServiceException as e: - rospy.logwarn("Edge Reconfigure Manager: Caught service exception: {}".format(e)) - continue - - self.initial_config[namespace] = {} - self.edge_config[namespace] = {} - - for param in self.edge["config"]: - if param["namespace"] == namespace: - self.edge_config[namespace][param["name"]] = param["value"] - - reset = True if "reset" not in param else param["reset"] - if reset: - self.initial_config[namespace][param["name"]] = config[param["name"]] - - - def reconfigure(self): - """ - If using the new map then edge reconfigure is done using settings in the map. - """ - for namespace in self.edge_config: - rospy.loginfo("Edge Reconfigure Manager: Setting {} = {}".format(namespace, self.edge_config[namespace])) - self.update(namespace, self.edge_config[namespace]) - - - def _reset(self): - """ - Used to reset edge params to their default values when the action has completed (only if using the new map) - """ - for namespace in self.initial_config: - if self.initial_config[namespace]: - rospy.loginfo("Edge Reconfigure Manager: Resetting {} = {}".format(namespace, self.initial_config[namespace])) - self.update(namespace, self.initial_config[namespace]) - - - def update(self, namespace, params): - - client = dynamic_reconfigure.client.Client(namespace, timeout=2.0) - try: - client.update_configuration(params) - - except rospy.ServiceException as e: - rospy.logwarn("Edge Reconfigure Manager: Caught service exception: {}".format(e)) - - - def srv_reconfigure(self, edge_id): - """ - If using the old map then edge reconfigure is done via a service. - """ - edge_group = "none" - for group in self.edge_groups: - print("Check Edge: ", edge_id, "in ", group) - if edge_id in self.edge_groups[group]["edges"]: - edge_group = group - break - - print("current group: ", self.current_edge_group) - print("edge group: ", edge_group) - - if edge_group is not self.current_edge_group: - print("RECONFIGURING EDGE: ", edge_id) - print("TO ", edge_group) - try: - rospy.wait_for_service("reconf_at_edges", timeout=3) - reconf_at_edges = rospy.ServiceProxy("reconf_at_edges", ReconfAtEdges) - resp1 = reconf_at_edges(edge_id) - print(resp1.success) - if resp1.success: - self.current_edge_group = edge_group - except rospy.ServiceException as e: - rospy.logerr("Service call failed: %s" % e) - print("-------") -############################################################################################################# \ No newline at end of file diff --git a/topological_navigation/topological_navigation/edge_reconfigure_manager2.py b/topological_navigation/topological_navigation/edge_reconfigure_manager2.py deleted file mode 100644 index 332bcc21..00000000 --- a/topological_navigation/topological_navigation/edge_reconfigure_manager2.py +++ /dev/null @@ -1,126 +0,0 @@ -#!/usr/bin/env python -""" -Created on Tue Nov 5 22:02:24 2023 -@author: Geesara Kulathunga (ggeesara@gmail.com) - -""" -############################################################################################################# -import rclpy -from topological_navigation_msgs.srv import ReconfAtEdges -from topological_navigation.scripts.param_processing import ParameterUpdaterNode -from rclpy import Parameter -from rclpy.callback_groups import ReentrantCallbackGroup - -class EdgeReconfigureManager(rclpy.node.Node): - def __init__(self): - super().__init__("edge_conguration_manager") - self.active = False - self.edge = {} - self.initial_config = {} - self.edge_config = {} - self.namespaces = [] - self.current_edge_group = "none" - self.declare_parameter("/edge_nav_reconfig_groups", Parameter.Type.STRING_ARRAY) - self.edge_groups = self.get_parameter_or("/edge_nav_reconfig_groups", Parameter('str', Parameter.Type.STRING_ARRAY, [])).value - self.callback_group = ReentrantCallbackGroup() - self.reconf_at_edges = self.create_client(ReconfAtEdges, '/reconf_at_edges') - - - def register_edge(self, edge): - self.active = False - self.edge = edge - namespaces = [] - if "config" in edge: - namespaces = [param["namespace"] for param in edge["config"]] - self.namespaces = list(set(namespaces)) - if self.namespaces: - self.active = True - self.get_logger().info("Edge Reconfigure Manager: Processing edge {}".format(edge["edge_id"])) - - - def initialise(self): - - self.initial_config = {} - self.edge_config = {} - for namespace in self.namespaces: - self.get_logger().info("Edge Reconfigure Manager: Getting initial configuration for {}".format(namespace)) - client = ParameterUpdaterNode(namespace) - try: - config = client.get_params() - except Exception as e: - self.get_logger().warning("Edge Reconfigure Manager: Caught service exception: {}".format(e)) - continue - - self.initial_config[namespace] = {} - self.edge_config[namespace] = {} - - for param in self.edge["config"]: - if param["namespace"] == namespace: - self.edge_config[namespace][param["name"]] = param["value"] - - reset = True if "reset" not in param else param["reset"] - if reset: - self.initial_config[namespace][param["name"]] = config[param["name"]] - - - def reconfigure(self): - """ - If using the new map then edge reconfigure is done using settings in the map. - """ - for namespace in self.edge_config: - self.get_logger().info("Edge Reconfigure Manager: Setting {} = {}".format(namespace, self.edge_config[namespace])) - self.update(namespace, self.edge_config[namespace]) - - - def _reset(self): - """ - Used to reset edge params to their default values when the action has completed (only if using the new map) - """ - for namespace in self.initial_config: - if self.initial_config[namespace]: - self.get_logger().info("Edge Reconfigure Manager: Resetting {} = {}".format(namespace, self.initial_config[namespace])) - self.update(namespace, self.initial_config[namespace]) - - - def update(self, namespace, params): - client = ParameterUpdaterNode(namespace) - try: - client.get_params() - client.set_params(params) - except Exception as e: - self.get_logger().warning("Edge Reconfigure Manager: Caught service exception: {}".format(e)) - - - def srv_reconfigure(self, edge_id): - """ - If using the old map then edge reconfigure is done via a service. - """ - edge_group = "none" - for group in self.edge_groups: - self.get_logger().info("Check Edge: ", edge_id, "in ", group) - if edge_id in self.edge_groups[group]["edges"]: - edge_group = group - break - - self.get_logger().info("current group: ", self.current_edge_group) - self.get_logger().info("edge group: ", edge_group) - - if edge_group is not self.current_edge_group: - self.get_logger().info("RECONFIGURING EDGE: ", edge_id) - self.get_logger().info("TO ", edge_group) - self.req_get = ReconfAtEdges.Request() - self.req_get.edge_id = edge_id - self.future = self.reconf_at_edges.call_async(self.req_get) - while rclpy.ok(): - rclpy.spin_once(self) - if self.future.done(): - try: - response = self.future.result() - if response.success: - self.current_edge_group = edge_group - break - except Exception as e: - self.get_logger().error("Error while reconfiguring the edge {}".format(e)) - pass - -############################################################################################################# \ No newline at end of file diff --git a/topological_navigation/topological_navigation/edge_std.py b/topological_navigation/topological_navigation/edge_std.py deleted file mode 100644 index fa947fb3..00000000 --- a/topological_navigation/topological_navigation/edge_std.py +++ /dev/null @@ -1,114 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import math -import tf -import numpy - -import matplotlib as mpl -import matplotlib.cm as cm - -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point - -from visualization_msgs.msg import * - -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation.topological_map import * -from topological_navigation_msgs.msg import NavRoute - - -class edges_std_marker(object): - - def __init__(self, map_name) : - self.map_name = map_name - self.route_nodes = NavRoute() - self.updating=True - self.update_map(map_name) - - - def update_map(self,map_name) : - - self.topo_map = topological_map(self.map_name) - - self.map_edges = MarkerArray() - - total = len(self.route_nodes.source) - - if total > 0: - self.maxval= numpy.nanmax(self.route_nodes.prob)+0.000001 - #self.minval= numpy.min(self.route_nodes.prob)-0.000001 - else : - self.maxval= 0 - self.minval= 0 - counter=0 - total = len(self.route_nodes.source) - - while counter < total : - inds = self.topo_map._get_node_index(self.route_nodes.source[counter]) - indt = self.topo_map._get_node_index(self.route_nodes.target[counter]) - point1=Point() - point2=Point() - point1= (self.topo_map.nodes[inds]._get_pose()).position - point2= (self.topo_map.nodes[indt]._get_pose()).position - val = self.route_nodes.prob[counter] - #val = val/maxval - if not math.isnan(val) : - self.create_edge(point1, point2, val) - counter+=1 - - idn = 0 - for m in self.map_edges.markers: - m.id = idn - idn += 1 - self.updating=False - - - def create_edge(self, point1, point2, val): - norm = mpl.colors.Normalize(vmin=0.0, vmax=self.maxval) - cmap = cm.YlOrRd - m = cm.ScalarMappable(norm=norm, cmap=cmap) - - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.ARROW - pose = Pose() - -# pose.position.x = (point1.x+point2.x)/2 -# pose.position.y = (point1.y+point2.y)/2 -# pose.position.z = (point1.z+point2.z)/2 - pose.position.x = point1.x - pose.position.y = point1.y - pose.position.z = point1.z - angle = math.atan2((point2.y-point1.y),(point2.x-point1.x)) - - qat = tf.transformations.quaternion_from_euler(0, 0, angle) - pose.orientation.w = qat[3] - pose.orientation.x = qat[0] - pose.orientation.y = qat[1] - pose.orientation.z = qat[2] - - r = math.hypot((point2.y-point1.y),(point2.x-point1.x))/3.0 - marker.scale.x = r - marker.scale.y = 0.2 - marker.scale.z = 0.2 - - #val = float(counter)/float(total) - v = m.to_rgba(1.0-val) - marker.color.a = v[3] - marker.color.r = v[0] - marker.color.g = v[1] - marker.color.b = v[2] - marker.pose = pose - self.map_edges.markers.append(marker) - - - def received_route(self, route): - self.route_nodes = route - self.clear() - self.update_map(self.topo_map) - - def clear(self): - self.updating=True - del self.map_edges \ No newline at end of file diff --git a/topological_navigation/topological_navigation/goto.py b/topological_navigation/topological_navigation/goto.py deleted file mode 100644 index f4fba5c6..00000000 --- a/topological_navigation/topological_navigation/goto.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import math -import tf -import actionlib -from threading import Timer - - -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point - -from visualization_msgs.msg import * -from interactive_markers.interactive_marker_server import * - -from topological_navigation_msgs.msg import TopologicalMap, GotoNodeGoal, GotoNodeAction -from topological_navigation.topological_map import * - - - -class go_to_controllers(object): - - def __init__(self) : - self.in_feedback=False - #self.timer = Timer(1.0, self.timer_callback) - #map_name = rospy.get_param('topological_map_name', 'top_map') - self._goto_server = InteractiveMarkerServer("go_to_node") - self.client = actionlib.SimpleActionClient('topological_navigation', GotoNodeAction) - self.client.wait_for_server() - rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) - rospy.loginfo(" ... Go to Initialised") - - - def update_map(self, map_msg) : - print("updating goto controllers...") - #self.topo_map = topological_map(map_name) - self._goto_server.clear() - for i in map_msg.nodes : - self.create_marker(i.name, i.pose, i.name) - - """ - MapCallback - - This function receives the Topological Map - """ - def MapCallback(self, msg) : - self.update_map(msg) - - - - def makeEmptyMarker(self, dummyBox=True ) : - int_marker = InteractiveMarker() - int_marker.header.frame_id = "map" - int_marker.scale = 1 - return int_marker - - - def makeBox(self, msg ): - marker = Marker() - - marker.type = Marker.ARROW - marker.scale.x = msg.scale * 0.5 - marker.scale.y = msg.scale * 0.25 - marker.scale.z = msg.scale * 0.15 - marker.lifetime.secs = 3 - marker.color.r = 0.1 - marker.color.g = 0.8 - marker.color.b = 0.1 - marker.color.a = 1.0 - return marker - - - def create_marker(self, marker_name, pos, marker_description="goto marker") : - # create an interactive marker for our server - - marker = self.makeEmptyMarker() - marker.name = marker_name - marker.description = marker_name - - control = InteractiveMarkerControl() - - control.interaction_mode = InteractiveMarkerControl.BUTTON - control.always_visible = True - - control.markers.append( self.makeBox( marker ) ) - marker.controls.append(control) - - self._goto_server.insert(marker, self.feedback_cb) - self._goto_server.applyChanges() - - if pos is not None: - pos.position.z=pos.position.z+0.15 - self._goto_server.setPose( marker.name, pos ) - self._goto_server.applyChanges() - - - - def feedback_cb(self, feedback): - if not self.in_feedback : - self.in_feedback=True - print('GOTO: '+str(feedback.marker_name)) - self.client.cancel_all_goals() - navgoal = GotoNodeGoal() - navgoal.target = feedback.marker_name - #navgoal.origin = orig - # Sends the goal to the action server. - self.client.send_goal(navgoal) -# self.update_needed=True - self.timer = Timer(1.0, self.timer_callback) - self.timer.start() - - - def timer_callback(self) : - self.in_feedback = False - - def clear(): - self._goto_server.clear() - self._goto_server.applyChanges() - - - def __del__(self): - del self._goto_server diff --git a/topological_navigation/topological_navigation/load_maps_from_yaml.py b/topological_navigation/topological_navigation/load_maps_from_yaml.py deleted file mode 100644 index dcdd3c7c..00000000 --- a/topological_navigation/topological_navigation/load_maps_from_yaml.py +++ /dev/null @@ -1,87 +0,0 @@ -# -*- coding: utf-8 -*- -""" -Created on Wed Nov 4 10:56:07 2015 - -@author: cdondrup -""" - -import rospy -import yaml -import mongodb_store.util as dc_util -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation.topological_map import topological_map -import os - - -class YamlMapLoader(object): - def __init__(self): - self.msg_store = MessageStoreProxy(collection='topological_maps') - - def get_maps(self): - """ - Queries the database and returns details of the available topological maps. - :return: A dictionary where each key is the name of a topological map and each - item is a dictionary of details. Details are: - "number_nodes" ; integer - "edge_actions" : list of action server names used for traversal - "last_modified" : datetime.datetime object for the last time a node was inserted - """ - maps = dict() - - nodes = self.msg_store.query(TopologicalNode._type) - - for node in nodes: - pointset = node[1]["pointset"] - if not maps.has_key(pointset): - maps[pointset] = {"number_nodes": 0, "edge_actions": set(), "last_modified": ""} - maps[pointset]["number_nodes"] += 1 - if (maps[pointset]["last_modified"] == "" or - node[1]["inserted_at"] > maps[pointset]["last_modified"]): - maps[pointset]["last_modified"] = node[1]["inserted_at"] - for edge in node[0].edges: - maps[pointset]["edge_actions"].add(edge.action) - - return maps - - def _load_yaml(self, filename): - rospy.loginfo("loading %s"%filename) - with open(filename, 'r') as f: - return yaml.load(f) - - def read_maps(self, p): - data = [] - if os.path.isdir(p): - for f in os.listdir(p): - if f.endswith(".yaml"): - data.append(self._load_yaml(p+'/'+f)) - else: - data.append(self._load_yaml(p)) - return data - - def insert_maps(self, data, new_pointset=None, force=False): - current_maps = self.get_maps() - for idx, tmap in enumerate(data): - pointset = None - if new_pointset != None: # If there are more than one map, it takes the custom pointset and appends an index - pointset = new_pointset+str(idx+1) if len(data) > 1 else new_pointset - first_node = True - for i in tmap: - try: - meta = i['meta'] - meta['pointset'] = pointset if pointset != None else meta['pointset'] - if meta['pointset'] in current_maps and first_node: - first_node = False - if not force: - rospy.logwarn("Map '%s' already in datacentre, skipping! Use -f to force override or change pointset name with --pointset" % meta['pointset']) - break - else: - topo_map = topological_map(meta['pointset']) - topo_map.delete_map() - elif first_node: - first_node = False - rospy.loginfo("Inserting map: %s" % meta['pointset']) - msgv = dc_util.dictionary_to_message(i['node'], TopologicalNode) - self.msg_store.insert(msgv,meta) - except TypeError: - pass # Not a topo map \ No newline at end of file diff --git a/topological_navigation/topological_navigation/manager.py b/topological_navigation/topological_navigation/manager.py deleted file mode 100644 index ea9f25d6..00000000 --- a/topological_navigation/topological_navigation/manager.py +++ /dev/null @@ -1,1011 +0,0 @@ -#!/usr/bin/env python -import math -import rospy -import sys -import pymongo -import json -import yaml -import re -import uuid -import std_msgs.msg -import os - -from topological_navigation_msgs.msg import * -from topological_navigation_msgs.srv import * -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation.manager2 import map_manager_2 -from rospy_message_converter import message_converter -from topological_navigation.tmap_utils import get_node_names_from_edge_id - - -def node_dist(node1,node2): - dist = math.sqrt((node1.pose.position.x - node2.pose.position.x)**2 + (node1.pose.position.y - node2.pose.position.y)**2 ) - return dist - - -class map_manager(object): - - - def __init__(self): - - #This service returns any given map - self.get_map_srv=rospy.Service('/topological_map_publisher/get_topological_map', topological_navigation_msgs.srv.GetTopologicalMap, self.get_topological_map_cb) - #This service switches topological map - self.switch_map_srv=rospy.Service('/topological_map_manager/switch_topological_map', topological_navigation_msgs.srv.GetTopologicalMap, self.switch_topological_map_cb) - #This service adds a node - self.add_node_srv=rospy.Service('/topological_map_manager/add_topological_node', topological_navigation_msgs.srv.AddNode, self.add_topological_node_cb) - #This service deletes a node - self.remove_node_srv=rospy.Service('/topological_map_manager/remove_topological_node', topological_navigation_msgs.srv.RmvNode, self.remove_node_cb) - #This service adds content to a node - self.add_content_to_node_srv=rospy.Service('/topological_map_manager/add_content_to_node', topological_navigation_msgs.srv.AddContent, self.add_content_cb) - self.update_node_name_srv = rospy.Service("/topological_map_manager/update_node_name", topological_navigation_msgs.srv.UpdateNodeName, self.update_node_name_cb) - self.update_node_waypoint_srv = rospy.Service("/topological_map_manager/update_node_pose", topological_navigation_msgs.srv.AddNode, self.update_node_waypoint_cb) - self.update_node_tolerance_srv = rospy.Service("/topological_map_manager/update_node_tolerance", topological_navigation_msgs.srv.UpdateNodeTolerance, self.update_node_tolerance_cb) - #This service adds a tag to the meta information of a list of nodes - self.get_tag_srv=rospy.Service('/topological_map_manager/get_tags', topological_navigation_msgs.srv.GetTags, self.get_tags_cb) - #This service adds gets all tags from the meta information of a node - self.get_node_tag_srv=rospy.Service('/topological_map_manager/get_node_tags', topological_navigation_msgs.srv.GetNodeTags, self.get_node_tags_cb) - #This service adds gets all tags from the meta information of a node - self.modify_tag_srv=rospy.Service('/topological_map_manager/modify_node_tags', topological_navigation_msgs.srv.ModifyTag, self.modify_tag_cb) - #This service adds a tag to the meta information of a list of nodes - self.add_tag_srv=rospy.Service('/topological_map_manager/add_tag_to_node', topological_navigation_msgs.srv.AddTag, self.add_tag_cb) - #This service removes a tag from the meta information of a list of nodes - self.rm_tag_srv=rospy.Service('/topological_map_manager/rm_tag_from_node', topological_navigation_msgs.srv.AddTag, self.rm_tag_cb) - #This service returns a list of nodes that have a given tag - self.get_tagged_srv=rospy.Service('/topological_map_manager/get_tagged_nodes', topological_navigation_msgs.srv.GetTaggedNodes, self.get_tagged_cb) - #This service returns a list of edges_ids between two nodes - self.get_node_edges_srv=rospy.Service('/topological_map_manager/get_edges_between_nodes', topological_navigation_msgs.srv.GetEdgesBetweenNodes, self.get_edges_between_cb) - #adds edge between two nodes - self.add_edges_srv=rospy.Service('/topological_map_manager/add_edges_between_nodes', topological_navigation_msgs.srv.AddEdge, self.add_edge_cb) - self.update_edge_srv=rospy.Service('/topological_map_manager/update_edge', topological_navigation_msgs.srv.UpdateEdgeLegacy, self.update_edge_cb) - self.remove_edge_srv=rospy.Service('/topological_map_manager/remove_edge', topological_navigation_msgs.srv.AddEdge, self.remove_edge_cb) - - self.manager2 = map_manager_2() - - - def init_map(self, name, load=True, load_from_file=False): - - self.name = name - self.load_from_file = load_from_file - self.map_ok = True - self.yaw_goal_tolerance = 0.1 - self.xy_goal_tolerance = 0.3 - - if load: - if not load_from_file: - self.nodes = self.loadMap(name) - else: - self.nodes, self.tmap = self.load_map_from_file(name) - rospy.set_param('topological_map_path', os.path.split(name)[0]) - rospy.set_param('topological_map_filename', os.path.split(name)[1]) - - self.names = self.create_list_of_nodes() - self.tmap_to_tmap2() # convert map to new format - else: - self.nodes = topological_navigation_msgs.msg.TopologicalMap() - self.nodes.name = name - self.nodes.pointset = name - self.names=[] - - rospy.set_param('topological_map_name', self.nodes.pointset) - self.map_pub = rospy.Publisher('/topological_map', topological_navigation_msgs.msg.TopologicalMap, latch=True, queue_size=1) - self.last_updated = rospy.Time.now() - self.map_pub.publish(self.nodes) - - rospy.Subscriber('/update_map', std_msgs.msg.Time, self.updateCallback) - - - def updateCallback(self, msg) : -# if msg.data > self.last_updated : - self.nodes = self.loadMap(self.name) - self.tmap_to_tmap2() - self.last_updated = rospy.Time.now() - self.map_pub.publish(self.nodes) - self.names = self.create_list_of_nodes() - rospy.set_param('topological_map_name', self.nodes.pointset) - - - def get_tags_cb(self, req): - """ - get tags callback - This function is the callback for the get tags service - It returns a list of available tags in the map - """ - tt = self.get_tags_from_file() if self.load_from_file else self.get_tags_from_mongo() - return tt - - - def get_tags_from_mongo(self): - - host = rospy.get_param("mongodb_host") - port = rospy.get_param("mongodb_port") - client = pymongo.MongoClient(host, port) - - db=client.message_store - collection=db["topological_maps"] - available = collection.find({"pointset": self.nodes.name}).distinct("_meta.tag") - tt=[] - #for i in available: - tt.append(available) - return tt - - - def get_tags_from_file(self): - tt = [tag for node in self.tmap if "tag" in node["meta"] for tag in node["meta"]["tag"]] - return [set(tt)] - - - def get_node_tags_cb(self, req): - """ - get node tags callback - This function is the callback for the get node tags service - It returns a list of a node's tags - """ - succeded, tags = self.get_node_tags_from_file(req) if self.load_from_file else self.get_node_tags_from_mongo(req) - return succeded, tags - - - def get_node_tags_from_mongo(self, req): - #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node)) - succeded = True - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name" : req.node_name, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - - #print query, query_meta - available = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) - #print len(available) - if len(available) == 1: - # The meta information for a node is in the second part of the tuple - # returned by the message store query - if 'tag' in available[0][1]: - tags = available[0][1]['tag'] - else: - tags = [] - else: - succeded = False - tags = [] - - return succeded, tags - - - def get_node_tags_from_file(self, req): - - num_available = 0 - for node in self.tmap: - if node["meta"]["node"] == req.node_name and node["node"]["name"] == req.node_name: - if "tag" in node["meta"]: - tags = node["meta"]["tag"] - else: - tags = [] - - num_available+=1 - - if num_available == 1: - succeded = True - else: - succeded = False - tags = [] - - return succeded, tags - - - def get_tagged_nodes(self, tag): - """ - get tagged nodes callback - This function is the callback for the get tagged nodes service - Returns a list of node names that have a specific tag - """ - mm = self.get_tagged_nodes_from_file(tag) if self.load_from_file else self.get_tagged_nodes_from_mongo(tag) - return mm - - - def get_tagged_nodes_from_mongo(self, tag): - mm=[] - a=[] - - #db.topological_maps.find({ "_meta.tag":"AAA" }) - - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"_meta.tag": tag, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - - #print query, query_meta - available = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) - #print len(available) - for i in available: - nname= i[1]['node'] - a.append(nname) - - mm.append(a) - - return mm - - - def get_tagged_nodes_from_file(self, tag): - mm = [] - a=[] - - for node in self.tmap: - if "tag" in node["meta"]: - if tag in node["meta"]["tag"]: - a.append(node["node"]["name"]) - - mm.append(a) - - return mm - - - def get_tagged_cb(self, msg): - return self.get_tagged_nodes(msg.tag) - - - def add_content_cb(self, req): - #print req - data = json.loads(req.content) - #print data - - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name" : req.node, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - - #print query, query_meta - available = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) - #print len(available) - if len(available) != 1: - succeded = False - meta_out = None - print('there are no nodes or more than 1 with that name') - else: - succeded = True - for i in available: - msgid= i[1]['_id'] - if 'contains' in i[1]: - if type(data) is list : - for j in data: - if 'category' in j and 'name' in j : - i[1]['contains'].append(j) - elif type(data) is dict : - if 'category' in data and 'name' in data : - i[1]['contains'].append(data) - else: - a=[] - if type(data) is list : - for j in data: - if 'category' in j and 'name' in j : - a.append(j) - elif type(data) is dict : - if 'category' in data and 'name' in data : - a.append(data) - i[1]['contains']=a - meta_out = str(i[1]) - print("Updating %s--%s" %(i[0].pointset, i[0].name)) - msg_store.update_id(msgid, i[0], i[1], upsert = False) - - return succeded, meta_out - - - def add_tag_cb(self, msg): - """ - add tag callback - This function adds the callback for the add tags service - It adds tag to a node in the map - """ - succeded, meta_out = self.add_tag_to_file(msg) if self.load_from_file else self.add_tag_to_mongo(msg) - #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node)) - return succeded, meta_out - - - def add_tag_to_mongo(self, msg): - succeded = True - meta_out = None - for j in msg.node: - - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name" : j, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - - #print query, query_meta - available = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) - #print len(available) - for i in available: - msgid= i[1]['_id'] - if 'tag' in i[1]: - if not msg.tag in i[1]['tag']: - i[1]['tag'].append(msg.tag) - else: - a=[] - a.append(msg.tag) - i[1]['tag']=a - meta_out = str(i[1]) - - msg_store.update_id(msgid, i[0], i[1], upsert = False) - #print trstr - if len(available) == 0: - succeded = False - - return succeded, meta_out - - - def add_tag_to_file(self, msg): - succeded = False - meta_out = None - for j in msg.node: - for node in self.tmap: - if j == node["meta"]["node"] and j == node["node"]["name"]: - succeded = True - if "tag" in node["meta"]: - if msg.tag not in node["meta"]["tag"]: - node["meta"]["tag"].append(msg.tag) - else: - a = [] - a.append(msg.tag) - node["meta"][ "tag"] = a - meta_out = str(node["meta"]) - return succeded, meta_out - - - def rm_tag_cb(self, msg): - #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node)) - succeded = True - for j in msg.node: - - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name" : j, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - - #print query, query_meta - available = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) - #print len(available) - succeded = False - meta_out = None - for i in available: - msgid= i[1]['_id'] - if 'tag' in i[1]: - if msg.tag in i[1]['tag']: - print('removing tag') - i[1]['tag'].remove(msg.tag) - print('new list of tags') - print(i[1]['tag']) - msg_store.update_id(msgid, i[0], i[1], upsert = False) - succeded = True - meta_out = str(i[1]) - - return succeded, meta_out - - - def modify_tag_cb(self, msg): - succeded = True - meta_out = None - for node in msg.node: - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name" : node, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - - #print query, query_meta - available = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) - #print len(available) - for node_plus_meta in available: - msgid= node_plus_meta[1]['_id'] - if 'tag' in node_plus_meta[1]: - if not msg.tag in node_plus_meta[1]['tag']: - continue - else: - tag_ind = node_plus_meta[1]['tag'].index(msg.tag) - node_plus_meta[1]['tag'][tag_ind] = msg.new_tag - meta_out = str(node_plus_meta[1]) - - msg_store.update_id(msgid, node_plus_meta[0], node_plus_meta[1], upsert = True) - if len(available) == 0: - succeded = False - - return succeded, meta_out - - - def get_topological_map_cb(self, req): - nodes = self.loadMap(req.pointset) - print("Returning Map %s"%req.pointset) - nodes.nodes.sort(key=lambda node: node.name) - return nodes - - - def switch_topological_map_cb(self, req): - self.nodes=[] - if not self.load_from_file: - self.nodes = self.loadMap(req.pointset) - self.name = req.pointset - print("Returning Map {}".format(req.pointset)) - else: - rospy.set_param('topological_map_filename', req.pointset) - path = rospy.get_param('topological_map_path') - f = path + "/" + req.pointset - self.nodes, self.tmap = self.load_map_from_file(f) - self.name = f - print("Returning Map {}".format(f)) - #nodes.nodes.sort(key=lambda node: node.name) - self.names = self.create_list_of_nodes() - self.map_pub.publish(self.nodes) - self.tmap_to_tmap2() # convert map to new format - - rospy.set_param('topological_map_name', self.nodes.pointset) - return self.nodes - - - def get_new_name(self): - namesnum=[] - for i in self.names : - if i.startswith('WayPoint') : - nam = i.strip('WayPoint') - namesnum.append(int(nam)) - namesnum.sort() - if namesnum: - nodname = 'WayPoint%d'%(int(namesnum[-1])+1) - else : - nodname = 'WayPoint1' - return nodname - - - def add_edge_cb(self, req): - return self.add_edge(req.origin, req.destination, req.action, req.edge_id) - - - def add_edge(self, or_waypoint, de_waypoint, action, edge_id) : - - rospy.loginfo('Adding Edge from '+or_waypoint+' to '+de_waypoint+' using '+action) - node_name = or_waypoint - - #nodeindx = self._get_node_index(edged[0]) - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name" : node_name, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - - #print query, query_meta - available = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) - #print len(available) - if len(available) == 1 : - eids = [] - for i in available[0][0].edges : - eids.append(i.edge_id) - - if not edge_id or edge_id in eids: - test=0 - eid = '%s_%s' %(or_waypoint, de_waypoint) - while eid in eids: - eid = '%s_%s_%03d' %(or_waypoint, de_waypoint, test) - test += 1 - else: - eid=edge_id - - edge = topological_navigation_msgs.msg.Edge() - edge.node = de_waypoint - edge.action = action - edge.top_vel = 0.55 - edge.edge_id = eid - edge.map_2d = available[0][0].map - - available[0][0].edges.append(edge) - - #print available[0][0] - msg_store.update(available[0][0], query_meta, query, upsert=True) - return True - else : - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - return False - - - def generate_circle_vertices(self, radius=0.75, number=8): - separation_angle = 2 * math.pi / number - start_angle = separation_angle / 2 - current_angle = start_angle - points = [] - for i in range(0, number): - points.append((math.cos(current_angle) * radius, math.sin(current_angle) * radius)) - current_angle += separation_angle - - return points - - - def add_topological_node_cb(self, req): - return self.add_topological_node(req.name, req.pose, req.add_close_nodes) - - - def add_topological_node(self, node_name, node_pose, add_close_nodes, dist=8.0): - #Get New Node Name - if node_name: - name = node_name - else: - name = self.get_new_name() - - rospy.loginfo('Creating Node: '+name) - - if name in self.names: - rospy.logerr("Node already exists, try another name") - return False - - #Create Message store - msg_store = MessageStoreProxy(collection='topological_maps') - - meta = {} - meta["map"] = self.nodes.map - meta["pointset"] = self.nodes.name - meta["node"] = name - - node = topological_navigation_msgs.msg.TopologicalNode() - node.name = name - node.map = self.nodes.map - node.pointset = self.name - node.pose = node_pose - node.yaw_goal_tolerance = self.yaw_goal_tolerance - node.xy_goal_tolerance = self.xy_goal_tolerance - node.localise_by_topic = '' - vertices=self.generate_circle_vertices() - for j in vertices : - v = topological_navigation_msgs.msg.Vertex() - v.x = float(j[0]) - v.y = float(j[1]) - node.verts.append(v) - - if add_close_nodes: - close_nodes = [] - for i in self.nodes.nodes: - ndist = node_dist(node, i) - if ndist < dist : - if i.name != 'ChargingPoint': - close_nodes.append(i.name) - - - for i in close_nodes: - e = topological_navigation_msgs.msg.Edge() - e.node = i - e.action = 'move_base' - eid = '%s_%s' %(node.name, i) - e.edge_id = eid - e.top_vel =0.55 - e.map_2d = node.map - node.edges.append(e) - - for i in close_nodes: - self.add_edge(i, node.name, 'move_base', '') - - msg_store.insert(node,meta) - return True - - - def update_node_name_cb(self, req): - return self.update_node_name(req.node_name, req.new_name) - - - def update_node_name(self, node_name, new_name): - if new_name in self.names: - return False, "node with name {0} already exists".format(new_name) - - msg_store = MessageStoreProxy(collection='topological_maps') - # The query retrieves the node name with the given name from the given pointset. - query = {"name": node_name, "pointset": self.name} - # The meta-information is some additional information about the specific - # map that we are interested in (?) - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.nodes.map - # This returns a tuple containing the object, if it exists, and some - # information about how it's stored in the database. - available = msg_store.query(TopologicalNode._type, query, query_meta) - - if len(available) == 1: - available[0][0].name = new_name - # Also need to update all edges which involve the renamed node - allnodes_query = {"pointset": self.name} - allnodes_query_meta = {} - allnodes_query_meta["pointset"] = self.name - allnodes_query_meta["map"] = self.nodes.map - # this produces a list of tuples, each with [0] as the node, [1] as database info - allnodes_available = msg_store.query(TopologicalNode._type, {}, allnodes_query_meta) - - # Check the edges of each node for a reference to the node to be - # renamed, and change the edge id if there is one. Enumerate the - # values so that we can edit the objects in place to send them back - # to the database. - for node_ind, node_tpl in enumerate(allnodes_available): - for edge_ind, edge in enumerate(node_tpl[0].edges): - # change names of the edges in other nodes, and update their values in the database - if node_tpl[0].name != node_name and node_name in edge.edge_id: - allnodes_available[node_ind][0].edges[edge_ind].edge_id = edge.edge_id.replace(node_name, new_name) - # must also update the name of the node this edge goes to - allnodes_available[node_ind][0].edges[edge_ind].node = new_name - curnode_query = {"name": node_tpl[0].name, "pointset": self.name} - msg_store.update(allnodes_available[node_ind][0], allnodes_query_meta, curnode_query, upsert=True) - - # update all edge ids for this node - for edge_ind, edge in enumerate(available[0][0].edges): - available[0][0].edges[edge_ind].edge_id = edge.edge_id.replace(node_name, new_name) - - msg_store.update(available[0][0], query_meta, query, upsert=True) - return True, "" - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - return False, "multiple nodes with the name existed, or node not found" - - - def update_node_waypoint_cb(self, req): - return self.update_node_waypoint(req.name, req.pose) - - - def update_node_waypoint(self, name, pose): - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name": name, "pointset": self.name} - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.nodes.map - available = msg_store.query(TopologicalNode._type, query, query_meta) - if len(available) == 1: - positionZ=available[0][0].pose.position.z - available[0][0].pose = pose - available[0][0].pose.position.z = positionZ - msg_store.update(available[0][0], query_meta, query, upsert=True) - return True - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - return False - - - def update_node_tolerance_cb(self, req): - return self.update_node_tolerance(req.node_name, req.xy_tolerance, req.yaw_tolerance) - - - def update_node_tolerance(self, name, new_xy, new_yaw): - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name": name, "pointset": self.name} - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.nodes.map - available = msg_store.query(TopologicalNode._type, query, query_meta) - if len(available) == 1: - available[0][0].xy_goal_tolerance = new_xy - available[0][0].yaw_goal_tolerance = new_yaw - - msg_store.update(available[0][0], query_meta, query, upsert=True) - return True, "" - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - return False, "" - - - def remove_node_cb(self, req): - res = self.remove_node(req.name) - return res - - - def remove_node(self, node_name) : - rospy.loginfo('Removing Node: '+node_name) - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name" : node_name, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - - available = msg_store.query(TopologicalNode._type, query, query_meta) - - node_found = False - if len(available) == 1 : - node_found = True - rm_id = str(available[0][1]['_id']) - print(rm_id) - else : - rospy.logerr("Node not found "+str(len(available))+" waypoints found after query") - #rospy.logerr("Available data: "+str(available)) - - if node_found : - query_meta = {} - query_meta["pointset"] = self.nodes.name - edges_to_rm = [] - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - for i in message_list: - for j in i[0].edges : - if j.node == node_name : - edge_rm = j.edge_id - edges_to_rm.append(edge_rm) - - for k in edges_to_rm : - print('remove: '+k) - self.remove_edge(k) - - msg_store.delete(rm_id) - return True - else: - return False - - - def remove_edge_cb(self, req): - return self.remove_edge(req.edge_id) - - - def remove_edge(self, edge_name) : - #print 'removing edge: '+edge_name - rospy.loginfo('Removing Edge: '+edge_name) - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"edges.edge_id" : edge_name, "pointset": self.nodes.name} - query_meta = {} - query_meta["pointset"] = self.nodes.name - query_meta["map"] = self.nodes.map - available = msg_store.query(TopologicalNode._type, query, query_meta) - - if len(available) >= 1 : - for i in available : - print(i[0]) - for j in i[0].edges: - if j.edge_id == edge_name : - i[0].edges.remove(j) - msg_store.update(i[0], query_meta, query, upsert=True) - return True - else : - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - return False - - - def update_edge_cb(self, req): - return self.update_edge(req.edge_id, req.action, req.top_vel) - - - def update_edge(self, edge_id, action, top_vel): - msg_store = MessageStoreProxy(collection='topological_maps') - # The query retrieves the node name with the given name from the given pointset. - node_name, _ = get_node_names_from_edge_id(self.nodes, edge_id) - query = {"name": node_name, "pointset": self.name} - # The meta-information is some additional information about the specific - # map that we are interested in (?) - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.nodes.map - # This returns a tuple containing the object, if it exists, and some - # information about how it's stored in the database. - available = msg_store.query(TopologicalNode._type, query, query_meta) - if len(available) == 1: - for edge in available[0][0].edges: - if edge.edge_id == edge_id: - edge.action = action or edge.action - edge.top_vel = top_vel or edge.top_vel - - msg_store.update(available[0][0], query_meta, query, upsert=True) - return True, "" - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - return False, "no edge found or multiple edges found" - - - def get_edges_between(self, nodea, nodeb): - ab=[] - ba=[] - for i in self.nodes.nodes: - if nodea == i.name: - for j in i.edges: - if j.node == nodeb: - ab.append(j.edge_id) - if nodeb == i.name: - for j in i.edges: - if j.node == nodea: - ba.append(j.edge_id) - return ab, ba - - def get_edges_between_cb(self, req): - return self.get_edges_between(req.nodea, req.nodeb) - - - def loadMap(self, point_set) : - msg_store = MessageStoreProxy(collection='topological_maps') - points = topological_navigation_msgs.msg.TopologicalMap() - points.name = point_set - points.pointset = point_set - - query_meta = {} - query_meta["pointset"] = point_set - - ntries=1 - map_found=False - - #Tries to load the map for a minute if not it quits - while not map_found : - available = len(msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, {}, query_meta)) - if available <= 0 : - rospy.logerr("Desired pointset '"+point_set+"' not in datacentre, try :"+str(ntries)) - if ntries <=10 : - ntries+=1 - rospy.sleep(rospy.Duration.from_sec(6)) - else : - raise Exception("Can't find waypoints.") - return points - #We just want to raise the exception not quit - #sys.exit(2) - else: - map_found=True - - query_meta = {} - query_meta["pointset"] = point_set - - message_list = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, {}, query_meta) - - points.name = point_set - points.pointset = point_set - for i in message_list: - b = i[0] - points.nodes.append(b) - - points.map = points.nodes[0].map - self.map_check(points) - - return points - - - def load_map_from_file(self, filename): - points = topological_navigation_msgs.msg.TopologicalMap() - - with open(filename, 'r') as f: - try: - tmap = yaml.safe_load(f) - except Exception as exc: - print(exc) - return points - - - point_set = tmap[0]["node"]["pointset"] - points.name = point_set - points.pointset = point_set - points.map, tmap[0]["node"] = self.set_val(tmap[0]["node"], "map", "map") # optional - - for node in tmap: - msg = topological_navigation_msgs.msg.TopologicalNode() - msg.name = node["node"]["name"] - msg.map, node["node"] = self.set_val(node["node"], "map", "map") # optional - msg.pointset = node["node"]["pointset"] - - msg.pose.position.x = node["node"]["pose"]["position"]["x"] - msg.pose.position.y = node["node"]["pose"]["position"]["y"] - msg.pose.position.z = node["node"]["pose"]["position"]["z"] - - msg.pose.orientation.x = node["node"]["pose"]["orientation"]["x"] - msg.pose.orientation.y = node["node"]["pose"]["orientation"]["y"] - msg.pose.orientation.z = node["node"]["pose"]["orientation"]["z"] - msg.pose.orientation.w = node["node"]["pose"]["orientation"]["w"] - - msg.yaw_goal_tolerance = node["node"]["yaw_goal_tolerance"] - msg.xy_goal_tolerance = node["node"]["xy_goal_tolerance"] - - msgs_verts = [] - for v in node["node"]["verts"]: - msg_v = topological_navigation_msgs.msg.Vertex() - msg_v.x = v["x"] - msg_v.y = v["y"] - msgs_verts.append(msg_v) - msg.verts = msgs_verts - - msgs_edges = [] - for e in node["node"]["edges"]: - msg_e = topological_navigation_msgs.msg.Edge() - msg_e.edge_id = e["edge_id"] - msg_e.node = e["node"] - msg_e.action = e["action"] - msg_e.top_vel, e = self.set_val(e, "top_vel", 0.55) # optional - msg_e.map_2d, e = self.set_val(e, "map_2d", "map") # optional - msg_e.inflation_radius, e = self.set_val(e, "inflation_radius", 0.0) # optional - msg_e.recovery_behaviours_config, e = self.set_val(e, "recovery_behaviours_config", "") # optional - msgs_edges.append(msg_e) - msg.edges = msgs_edges - - msg.localise_by_topic, node["node"] = self.set_val(node["node"], "localise_by_topic", "") # optional - points.nodes.append(msg) - - self.map_check(points) - return points, tmap - - - def set_val(self, d, key, def_val): - - if key in d: - val = d[key] - else: - val = def_val - d[key] = def_val - - return val, d - - - def map_check(self, nodes): - - self.map_ok = True - - # check that all nodes have the same pointset - pointsets = [node.pointset for node in nodes.nodes] - if len(set(pointsets)) > 1: - rospy.logwarn("multiple poinsets found: {}".format(set(pointsets))) - self.map_ok = False - - # check for duplicate node names - names = [node.name for node in nodes.nodes] - names.sort() - for name in set(names): - n = names.count(name) - if n > 1: - rospy.logwarn("{} instances of node with name '{}' found".format(n, name)) - self.map_ok = False - - sep = "_" + str(uuid.uuid4()) + "_" - edge_ids = [node.name + sep + e.node for node in nodes.nodes for e in node.edges] - edge_ids.sort() - - # check for duplicate edges - for e in set(edge_ids): - edge_nodes = re.match("(.*)" + sep + "(.*)", e).groups() - origin = edge_nodes[0] - destination = edge_nodes[1] - - n = edge_ids.count(e) - if n > 1: - rospy.logwarn("{} instances of edge with origin '{}' and destination '{}' found".format(n, origin, destination)) - self.map_ok = False - - # check that an edge's destination node exists - for e in set(edge_ids): - edge_nodes = re.match("(.*)" + sep + "(.*)", e).groups() - origin = edge_nodes[0] - destination = edge_nodes[1] - - if destination not in names: - rospy.logwarn("edge with origin '{}' has a destination '{}' that does not exist".format(origin, destination)) - self.map_ok = False - - - def create_list_of_nodes(self): - names=[] - for i in self.nodes.nodes : - names.append(i.name) - names.sort() - return names - - - def tmap_to_tmap2(self): - - filename = "" - if self.load_from_file: - filename = os.path.splitext(self.name)[0] + ".tmap2" - - self.manager2.init_map(name=self.nodes.name, metric_map=self.nodes.map, pointset=self.nodes.pointset, filename=filename, load=False) - - for node in self.nodes.nodes: - - pose = message_converter.convert_ros_message_to_dictionary(node.pose) - verts = [message_converter.convert_ros_message_to_dictionary(vert) for vert in node.verts] - - properties = {} - properties["xy_goal_tolerance"] = node.xy_goal_tolerance - properties["yaw_goal_tolerance"] = node.yaw_goal_tolerance - - req = topological_navigation_msgs.srv.GetNodeTags() - req.node_name = node.name - tags = self.get_node_tags_cb(req)[1] - - self.manager2.add_node(node.name, pose, node.localise_by_topic, verts, properties, tags) - - for edge in node.edges: - self.manager2.add_edge_to_node(node.name, edge.node, edge.action, edge.edge_id, [], edge.recovery_behaviours_config) - - self.manager2.update() -################################################################################################################### \ No newline at end of file diff --git a/topological_navigation/topological_navigation/manager2.py b/topological_navigation/topological_navigation/manager2.py deleted file mode 100644 index d0b11a83..00000000 --- a/topological_navigation/topological_navigation/manager2.py +++ /dev/null @@ -1,1538 +0,0 @@ - #!/usr/bin/env python3 -""" -Created on Tue Sep 29 16:06:36 2020 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -######################################################################################################### -import yaml, datetime, json -import re, uuid, copy, os -import multiprocessing, math -import importlib -import traceback - -import rclpy -from rclpy.parameter import Parameter -import rosidl_runtime_py -import tf2_ros -from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy -from ament_index_python.packages import get_package_share_directory -from topological_navigation_msgs.msg import * -import topological_navigation_msgs.msg -import std_msgs.msg -from geometry_msgs.msg import Vector3, Quaternion, TransformStamped, Pose - -from std_srvs.srv import Trigger, Empty -import topological_navigation_msgs.srv as tn_srv - -import topological_navigation -from topological_navigation.tmap_utils import get_node_names_from_edge_id_2 - - -def pose_dist(pose1, pose2): - return math.sqrt((pose1["position"]["x"] - pose2["position"]["x"])**2 + (pose1["position"]["y"] - pose2["position"]["y"])**2) - -class NoAliasDumper(yaml.SafeDumper): - def ignore_aliases(self, data): - return True -######################################################################################################### - - -# this ensures that all the poses and translates -# are float-type and not int-type as there is an -# assertion in ros2 messages (vector3, pose etc.) -# for float-type [x,y,z,w] keys. -class CustomSafeLoader(yaml.SafeLoader): - def construct_mapping(self, node, deep=False): - mapping = super().construct_mapping(node, deep=deep) - - # this can be extended to test the validity of the tmap2 - # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w', 'yaw_goal_tolerance', 'xy_goal_tolerance']: - if key in mapping and isinstance(mapping[key], int): - mapping[key] = float(mapping[key]) - - return mapping - - -######################################################################################################### -class map_manager_2(rclpy.node.Node): - - def __init__(self, advertise_srvs=True): - super().__init__('topological_map_manager_2') - - self.cache_maps = self.get_parameter_or("~cache_topological_maps", Parameter('bool', Parameter.Type.BOOL, False)).value - self.auto_write = self.get_parameter_or("~auto_write_topological_maps", Parameter('bool', Parameter.Type.BOOL, False)).value - - package_path = get_package_share_directory('topological_navigation') - nav_config = str(os.path.join(package_path, 'config', 'navigation_goal.yaml')) - - self.nav_config = str(self.get_parameter_or("nav_config", Parameter('str', Parameter.Type.STRING, nav_config)).value) - self.get_logger().info("cache_topological_maps: {}".format(self.cache_maps)) - self.get_logger().info("auto_write_topological_maps: {}".format(self.auto_write)) - self.get_logger().info("nav config file: {}".format(self.nav_config)) - - self.cache_dir = os.path.join(os.path.expanduser("~"), ".ros", "topological_maps") - if not os.path.exists(self.cache_dir): - os.mkdir(self.cache_dir) - - self.goal_mappings = {} - - with open(self.nav_config, "r") as f: - self.move_base_goal = yaml.safe_load(f)["topological_navigation/navigation_goal"] - - if advertise_srvs: - self.advertise() - - - def advertise(self): - - # Services that retrieve information from the map - self.get_map_srv = self.create_service(Trigger, '/topological_map_manager2/get_topological_map', self.get_topological_map_cb) - - self.get_tagged_srv = self.create_service(tn_srv.GetTaggedNodes, '/topological_map_manager2/get_tagged_nodes', self.get_tagged_cb) - self.get_tag_srv = self.create_service(tn_srv.GetTags, '/topological_map_manager2/get_tags', self.get_tags_cb) - self.get_node_tag_srv = self.create_service(tn_srv.GetNodeTags, '/topological_map_manager2/get_node_tags', self.get_node_tags_cb) - self.get_node_edges_srv = self.create_service(tn_srv.GetEdgesBetweenNodes, '/topological_map_manager2/get_edges_between_nodes', self.get_edges_between_cb) - - # Services that modify the map - self.write_map_srv = self.create_service(tn_srv.WriteTopologicalMap, '/topological_map_manager2/write_topological_map', self.write_topological_map_cb) - self.switch_map_srv = self.create_service(tn_srv.WriteTopologicalMap, '/topological_map_manager2/switch_topological_map', self.switch_topological_map_cb) - - self.add_node_srv = self.create_service(tn_srv.AddNode, '/topological_map_manager2/add_topological_node', self.add_topological_node_cb) - self.remove_node_srv = self.create_service(tn_srv.RmvNode, '/topological_map_manager2/remove_topological_node', self.remove_node_cb) - - self.add_edges_srv = self.create_service(tn_srv.AddEdge, '/topological_map_manager2/add_edges_between_nodes', self.add_edge_cb) - self.remove_edge_srv = self.create_service(tn_srv.AddEdge, '/topological_map_manager2/remove_edge', self.remove_edge_cb) - - self.add_content_to_node_srv = self.create_service(tn_srv.AddContent, '/topological_map_manager2/add_content_to_node', self.add_content_cb) - - self.update_node_name_srv = self.create_service(tn_srv.UpdateNodeName, "/topological_map_manager2/update_node_name", self.update_node_name_cb) - self.update_node_waypoint_srv = self.create_service(tn_srv.AddNode, "/topological_map_manager2/update_node_pose", self.update_node_waypoint_cb) - self.update_node_tolerance_srv = self.create_service(tn_srv.UpdateNodeTolerance, "/topological_map_manager2/update_node_tolerance", self.update_node_tolerance_cb) - - self.modify_tag_srv = self.create_service(tn_srv.ModifyTag, '/topological_map_manager2/modify_node_tags', self.modify_tag_cb) - self.add_tag_srv = self.create_service(tn_srv.AddTag, '/topological_map_manager2/add_tag_to_node', self.add_tag_cb) - self.rm_tag_srv = self.create_service(tn_srv.AddTag, '/topological_map_manager2/rm_tag_from_node', self.rm_tag_cb) - - self.add_param_to_edge_config_srv = self.create_service(tn_srv.UpdateEdgeConfig, '/topological_map_manager2/add_param_to_edge_config', self.add_param_to_edge_config_cb) - self.rm_param_from_edge_config_srv = self.create_service(tn_srv.UpdateEdgeConfig, '/topological_map_manager2/rm_param_from_edge_config', self.rm_param_from_edge_config_cb) - self.rm_param_from_topological_map_srv = self.create_service(tn_srv.UpdateEdgeConfig, '/topological_map_manager2/rm_param_from_topological_map', self.rm_param_from_topological_map_cb) - - self.update_node_restrictions_srv = self.create_service(tn_srv.UpdateRestrictions, '/topological_map_manager2/update_node_restrictions', self.update_node_restrictions_cb) - self.update_edge_restrictions_srv = self.create_service(tn_srv.UpdateRestrictions, '/topological_map_manager2/update_edge_restrictions', self.update_edge_restrictions_cb) - - self.update_edge_srv = self.create_service(tn_srv.UpdateEdge, '/topological_map_manager2/update_edge', self.update_edge_cb) - self.update_action_srv = self.create_service(tn_srv.UpdateAction, '/topological_map_manager2/update_action', self.update_action_cb) - - self.add_datum_srv = self.create_service(tn_srv.AddDatum, '/topological_map_manager2/add_datum', self.add_datum_cb) - - self.update_fail_policy_srv = self.create_service(tn_srv.UpdateFailPolicy, '/topological_map_manager2/update_fail_policy', self.update_fail_policy_cb) - self.set_influence_zone_srv = self.create_service(tn_srv.SetInfluenceZone, '/topological_map_manager2/set_node_influence_zone', self.set_influence_zone_cb) - - self.clear_nodes_srv = self.create_service(Empty, '/topological_map_manager2/clear_topological_nodes', self.clear_nodes_cb) - - # Services for modifying the map quickly - self.add_nodes_srv = self.create_service(tn_srv.AddNodeArray, '/topological_map_manager2/add_topological_node_multi', self.add_topological_nodes_cb) - self.add_edges_srv = self.create_service(tn_srv.AddEdgeArray, '/topological_map_manager2/add_edges_between_nodes_multi', self.add_edges_cb) - self.add_params_to_edges_srv = self.create_service(tn_srv.UpdateEdgeConfigArray, '/topological_map_manager2/add_param_to_edge_config_multi', self.add_params_to_edges_cb) - self.set_influence_zones_srv = self.create_service(tn_srv.SetInfluenceZoneArray, '/topological_map_manager2/set_node_influence_zone_multi', self.set_influence_zones_cb) - - - def init_map(self, name="new_map", metric_map="map_2d", pointset="new_map", transformation="default", filename="", load=True): - - self.name = name - self.metric_map = metric_map - self.pointset = pointset - - if transformation == "default": - self.transformation = {} - self.transformation["rotation"] = {} - self.transformation["rotation"]["w"] = 1.0 - self.transformation["rotation"]["x"] = 0.0 - self.transformation["rotation"]["y"] = 0.0 - self.transformation["rotation"]["z"] = 0.0 - self.transformation["translation"] = {} - self.transformation["translation"]["x"] = 0.0 - self.transformation["translation"]["y"] = 0.0 - self.transformation["translation"]["z"] = 0.0 - self.transformation["child"] = "topo_map" - self.transformation["parent"] = "map" - else: - self.transformation = transformation - - self.filename = filename - if not self.filename: - self.filename = os.path.join(self.cache_dir, self.name + ".tmap2") - - self.loaded = False - self.load = load - if self.load: - self.load_map(self.filename) - else: - self.tmap2 = {} - self.tmap2["name"] = self.name - self.tmap2["metric_map"] = self.metric_map - self.tmap2["pointset"] = self.pointset - self.tmap2["transformation"] = self.transformation - self.tmap2["meta"] = {} - self.tmap2["meta"]["last_updated"] = self.get_time() - self.tmap2["nodes"] = [] - self.declare_parameter('topological_map2_name', self.pointset) - self.declare_parameter('topological_map2_filename', os.path.split(self.filename)[1]) - self.declare_parameter('topological_map2_path', os.path.split(self.filename)[0]) - - - qos = QoSProfile(depth=10, - reliability=ReliabilityPolicy.RELIABLE, - history=HistoryPolicy.KEEP_LAST, - durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.map_pub = self.create_publisher(std_msgs.msg.String, '/topological_map_2', qos) - self.map_pub.publish(std_msgs.msg.String(data=json.dumps(self.tmap2))) - self.names = self.create_list_of_nodes() - - self.broadcaster = tf2_ros.transform_broadcaster.TransformBroadcaster(self) - self.broadcast_transform() - - self.convert_to_legacy = self.get_parameter_or("~convert_to_legacy", Parameter('bool', Parameter.Type.BOOL, True)).get_parameter_value() - - if self.tmap2 and self.convert_to_legacy: - self.points_pub = self.create_publisher(topological_navigation_msgs.msg.TopologicalMap, '/topological_map', qos) - self.tmap2_to_tmap() - self.points_pub.publish(self.points) - - # self.create_timer(10.0, self.topnav_map_pub_callback) - - - # def topnav_map_pub_callback(self, ): - # if self.tmap2: - # self.map_pub.publish(std_msgs.msg.String(data=json.dumps(self.tmap2))) - # else: - # self.get_logger().warning('there is no topological map...', skip_first=True) - - def get_time(self): - return datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S') - - - def load_map(self, filename): - - def loader(filename, transporter): - try: - with open(filename, "r") as f: - # transporter["tmap2"] = yaml.safe_load(f) - transporter["tmap2"] = yaml.load(f, Loader = CustomSafeLoader) - except Exception as e: - self.get_logger().error(e) - transporter["tmap2"] = {} - - - self.loaded = False - self.get_logger().info("Loading Topological Map {} ...".format(filename)) - - transporter = multiprocessing.Manager().dict() - p = multiprocessing.Process(target=loader, args=(filename, transporter)) - p.start() - p.join() - - self.tmap2 = transporter["tmap2"] - if not self.tmap2: - return - - e1 = "Loaded map is {} and should be {}." - e2 = " You may be attemting to load a legacy map using topological_navigation/map_manager2.py." \ - " In that case please use topological_navigation/map_manager.py instead." - - map_type = type(self.tmap2) - if map_type is list: - self.get_logger().error((e1+e2).format(map_type, dict)) - self.tmap2 = {} - return - elif map_type is not dict: - self.get_logger().error(e1.format(map_type, dict)) - self.tmap2 = {} - return - - self.loaded = True - - self.name = self.tmap2["name"] - self.metric_map = self.tmap2["metric_map"] - self.pointset = self.tmap2["pointset"] - self.transformation = self.tmap2["transformation"] - - self.names = self.create_list_of_nodes() - - self.declare_parameter('topological_map2_name', self.pointset) - self.declare_parameter('topological_map2_filename', os.path.split(self.filename)[1]) - self.declare_parameter('topological_map2_path', os.path.split(self.filename)[0]) - - self.get_logger().info("Done") - - self.map_check() - - if self.cache_maps: - self.get_logger().info("Caching the map...") - self.write_topological_map(os.path.join(self.cache_dir, os.path.basename(self.filename))) - - - def write_topological_map(self, filename, no_alias=False): - - self.get_logger().info("Writing map to {} ...".format(filename)) - - nodes = copy.deepcopy(self.tmap2["nodes"]) - nodes.sort(key=lambda node: node["node"]["name"]) - self.tmap2["nodes"] = nodes - - if no_alias: - self.get_logger().info("Disabling anchors and aliases in topological map yaml ...") - yml = yaml.dump(self.tmap2, default_flow_style=False, Dumper=NoAliasDumper) - else: - yml = yaml.safe_dump(self.tmap2, default_flow_style=False) - - fh = open(filename, "w") - fh.write(str(yml)) - fh.close - - self.get_logger().info("Done") - - - def update(self, update_time=True): - - if update_time: - self.tmap2["meta"]["last_updated"] = self.get_time() - - self.map_pub.publish(std_msgs.msg.String(json.dumps(self.tmap2))) - self.names = self.create_list_of_nodes() - self.map_check() - - if self.tmap2 and self.convert_to_legacy: - self.tmap2_to_tmap() - self.points_pub.publish(self.points) - - - def broadcast_transform(self): - - trans, rot = Vector3(), Quaternion() - rosidl_runtime_py.set_message_fields(trans, self.transformation["translation"]) - rosidl_runtime_py.set_message_fields(rot, self.transformation["rotation"]) - - msg = TransformStamped() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = self.transformation["parent"] - msg.child_frame_id = self.transformation["child"] - msg.transform.translation = trans - msg.transform.rotation = rot - - self.broadcaster.sendTransform(msg) - - - def create_list_of_nodes(self): - - names = [] - if "nodes" in self.tmap2: - names = [node["node"]["name"] for node in self.tmap2["nodes"]] - names.sort() - return names - - - def get_topological_map_cb(self, req): - """ - Returns the topological map - """ - ans = Trigger.response() - ans.success = True - ans.message = json.dumps(self.tmap2) - - return ans - - - def switch_topological_map_cb(self, req): - """ - Changes the topological map - """ - self.declare_parameter('topological_map2_filename', req.filename) - path = self.get_parameter("topological_map2_path").get_parameter_value() - self.filename = path + "/" + req.filename - - self.load_map(self.filename) - self.update(False) - self.broadcast_transform() - - return True, json.dumps(self.tmap2) - - - def get_tagged_cb(self, req, res): - """ - Returns a list of nodes that have a given tag - """ - res.nodes=[] - for node in self.tmap2["nodes"]: - if "tag" in node["meta"]: - if req.tag in node["meta"]["tag"]: - res.nodes.append(node["node"]["name"]) - return res - - - def get_tags_cb(self, req): - """ - Returns a list of available tags in the map - """ - tags = [tag for node in self.tmap2["nodes"] if "tag" in node["meta"] for tag in node["meta"]["tag"]] - return [set(tags)] - - - def get_node_tags_cb(self, req): - """ - Returns a list of a node's tags - """ - num_available = 0 - for node in self.tmap2["nodes"]: - if node["node"]["name"] == req.node_name: - if "tag" in node["meta"]: - tags = node["meta"]["tag"] - else: - tags = [] - - num_available+=1 - - succeded = True - if num_available != 1: - succeded = False - tags = [] - - return succeded, tags - - - def get_edges_between_cb(self, req): - """ - Returns a list of the ids of edges from nodea to nodeb and vice-versa - """ - return self.get_edges_between(req.nodea, req.nodeb) - - - def get_edges_between(self, nodea, nodeb): - - ab=[]; ba=[] - for node in self.tmap2["nodes"]: - if nodea == node["node"]["name"]: - for edge in node["node"]["edges"]: - if edge["node"] == nodeb: - ab.append(edge["edge_id"]) - if nodeb == node["node"]["name"]: - for edge in node["node"]["edges"]: - if edge["node"] == nodea: - ba.append(edge["edge_id"]) - - return ab, ba - - - def write_topological_map_cb(self, req): - """ - Saves the topological map to a yaml file - """ - filename = req.filename - if not filename: - path = self.get_parameter("topological_map2_path").get_parameter_value() - fname = self.get_parameter_or("topological_map2_filename").get_parameter_value() - filename = path + "/" + fname - - try: - message = "Writing map to {}".format(filename) - self.write_topological_map(filename, req.no_alias) - success = True - except Exception as message: - success = False - - return success, str(message) - - - def add_topological_node_cb(self, req): - """ - Adds a node to the topological map - """ - return self.add_topological_node(req.name, req.pose, req.add_close_nodes) - - - def add_topological_node(self, node_name, node_pose, add_close_nodes, dist=8.0, update=True, write_map=True): - - if node_name: - name = node_name - else: - name = self.get_new_name() - - self.get_logger().info("Creating Node {}".format(name)) - - if name in self.names: - self.get_logger().error("Node {} already exists, try another name".format(name)) - return False - - pose = rosidl_runtime_py.message_to_ordereddict(node_pose) - - close_nodes = [] - if add_close_nodes: - for node in self.tmap2["nodes"]: - ndist = pose_dist(pose, node["node"]["pose"]) - if ndist < dist : - if node["node"]["name"] != "ChargingPoint": - close_nodes.append(node["node"]["name"]) - - self.add_node(name, pose) - - for close_node in close_nodes: - self.add_edge(name, close_node, "move_base", "", update=False, write_map=False) - self.add_edge(close_node, name, "move_base", "", update=False, write_map=False) - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - - return True - - - def get_new_name(self): - - namesnum=[] - for i in self.names : - if i.startswith('WayPoint') : - nam = i.strip('WayPoint') - namesnum.append(int(nam)) - namesnum.sort() - if namesnum: - nodname = 'WayPoint%d'%(int(namesnum[-1])+1) - else : - nodname = 'WayPoint1' - - return nodname - - - def add_node(self, name, pose, localise_by_topic="", verts="default", properties="default", tags=[], - restrictions_planning="True", restrictions_runtime="True"): - - if "orientation" not in pose: - pose["orientation"] = {} - pose["orientation"]["w"] = 1.0 - pose["orientation"]["x"] = 0.0 - pose["orientation"]["y"] = 0.0 - pose["orientation"]["z"] = 0.0 - - node = {} - node["meta"] = {} - node["meta"]["map"] = self.metric_map - node["meta"]["node"] = name - node["meta"]["pointset"] = self.pointset - if tags: - node["meta"]["tag"] = tags - - node["node"] = {} - node["node"]["edges"] = [] - node["node"]["localise_by_topic"] = localise_by_topic - node["node"]["name"] = name - node["node"]["pose"] = pose - - if verts == "default": - node["node"]["verts"] = self.generate_circle_vertices() - else: - node["node"]["verts"] = verts - - if properties == "default": - node["node"]["properties"] = {} - node["node"]["properties"]["xy_goal_tolerance"] = 0.3 - node["node"]["properties"]["yaw_goal_tolerance"] = 0.1 - else: - node["node"]["properties"] = properties - - node["node"]["restrictions_planning"] = restrictions_planning - node["node"]["restrictions_runtime"] = restrictions_runtime - - node["node"]["parent_frame"] = self.transformation["parent"] - - self.tmap2["nodes"].append(node) - - - def generate_circle_vertices(self, radius=0.75, number=8): - - separation_angle = 2 * math.pi / number - start_angle = separation_angle / 2 - current_angle = start_angle - points = [] - for i in range(0, number): - points.append({"x": math.cos(current_angle) * radius, "y": math.sin(current_angle) * radius}) - current_angle += separation_angle - - return points - - - def add_edge_cb(self, req): - """ - Adds an edge to a topological node - """ - return self.add_edge(req.origin, req.destination, req.action, req.action_type, req.edge_id) - - - def add_edge(self, origin, destination, action, action_type, edge_id, update=True, write_map=True): - - self.get_logger().info("Adding Edge from {} to {} using {}".format(origin, destination, action)) - - num_available, index = self.get_instances_of_node(origin) - - if num_available == 1 : - eids = [] - for edge in self.tmap2["nodes"][index]["node"]["edges"]: - eids.append(edge["edge_id"]) - - if edge_id == edge["edge_id"]: - self.get_logger().error("Error adding edge to node {}. Edge already exists.".format(origin)) - return False - - if not edge_id or edge_id in eids: - test=0 - eid = '%s_%s' %(origin, destination) - while eid in eids: - eid = '%s_%s_%03d' %(origin, destination, test) - test += 1 - else: - eid=edge_id - - self.add_edge_to_node(origin, destination, action, eid, action_type=action_type) - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - else: - self.get_logger().error("Error adding edge to node {}. {} instances of node with name {} found".format(origin, num_available, origin)) - return False - - - def add_edge_to_node(self, origin, destination, action="", edge_id="default", config=[], - recovery_behaviours_config="", action_type="", goal=None, fail_policy="fail", - restrictions_planning="True", restrictions_runtime="True", fluid_navigation=True): - - edge = {} - edge["action"] = action - - - if edge_id == "default": - edge["edge_id"] = origin + "_" + destination - else: - edge["edge_id"] = edge_id - - edge["node"] = destination - edge["config"] = config - edge["recovery_behaviours_config"] = recovery_behaviours_config - - if not action_type: - action_type = "move_base_msgs/MoveBaseGoal" - - the_action_type, the_goal = self.set_goal(action, action_type, goal) - - edge["action_type"] = the_action_type - edge["goal"] = the_goal - - edge["fail_policy"] = fail_policy - edge["restrictions_planning"] = restrictions_planning - edge["restrictions_runtime"] = restrictions_runtime - edge["fluid_navigation"] = fluid_navigation - - for node in self.tmap2["nodes"]: - if node["node"]["name"] == origin: - node["node"]["edges"].append(edge) - - - def set_goal(self, action, action_type, _goal=None): - - if action in self.goal_mappings and action_type == self.goal_mappings[action]["action_type"]: - goal = self.goal_mappings[action]["goal"] - else: - if _goal is not None: - goal = _goal - else: - try: - package = action_type.split("/")[0] - goal_def = action_type.split("/")[1] - - _file = self.get_parameter(f"~{action_type}", Parameter('str', Parameter.Type.STRING, "")).get_parameter_value() - if not _file: - package_object = importlib.import_module(package) - _file = os.path.join(package_object.__path__[0], '..', 'config', f"{goal_def}.yaml") - with open(_file, "r") as f: - goal = yaml.safe_load(f) - except: - action_type = self.move_base_goal["action_type"] - goal = self.move_base_goal["goal"] - - self.goal_mappings[action] = {"action_type": action_type, "goal": goal} - - return action_type, goal - - - def set_action_type(self, action): - - package = action + "_msgs" - items = [item[0].upper() + item[1:] for item in action.split("_")] - goal_type = "".join(items) + "Goal" - action_type = package + "/" + goal_type - - return action_type - - - def remove_node_cb(self, req): - """ - Removes a node from the topological map - """ - return self.remove_node(req.name) - - - def remove_node(self, node_name, update=True, write_map=True): - - self.get_logger().info("Removing Node {}".format(node_name)) - - num_available, rm_id = self.get_instances_of_node(node_name) - - if num_available == 1: - del self.tmap2["nodes"][rm_id] - - for node in self.tmap2["nodes"]: - for edge in node["node"]["edges"]: - if edge["node"] == node_name: - self.remove_edge(edge["edge_id"], False) - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - else: - self.get_logger().error("Error removing node {}. {} instances of node with name {} found".format(node_name, num_available, node_name)) - return False - - - def remove_edge_cb(self, req): - """ - Removes an edge from a topological node - """ - return self.remove_edge(req.edge_id) - - - def remove_edge(self, edge_name, update=True, write_map=True): - - self.get_logger().info("Removing Edge {}".format(edge_name)) - - num_available = 0 - for node in self.tmap2["nodes"]: - for edge in node["node"]["edges"]: - if edge["edge_id"] == edge_name: - num_available+=1 - - if num_available >= 1: - for node in self.tmap2["nodes"]: - edges = copy.deepcopy(node["node"]["edges"]) - edges_new = list(filter(lambda edge: edge["edge_id"] != edge_name, edges)) - node["node"]["edges"] = edges_new - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - else: - self.get_logger().error("No edges with id {} found".format(edge_name)) - return False - - - def add_content_cb(self, req): - """ - Adds content to a node's meta information - """ - data = json.loads(req.content) - - num_available, index = self.get_instances_of_node(req.node) - - if num_available != 1: - succeded = False - meta_out = None - self.get_logger().error("There are no nodes or more than one with name {}".format(req.node)) - else: - succeded = True - node_meta = self.tmap2["nodes"][index]["meta"] - if "contains" in node_meta: - if type(data) is list: - for j in data: - if "category" in j and "name" in j: - node_meta['contains'].append(j) - elif type(data) is dict: - if "category" in data and "name" in data: - node_meta["contains"].append(data) - else: - a=[] - if type(data) is list: - for j in data: - if "category" in j and "name" in j: - a.append(j) - elif type(data) is dict: - if "category" in data and "name" in data: - a.append(data) - node_meta["contains"] = a - meta_out = str(node_meta) - - self.get_logger().info("Updating %s--%s" %(self.tmap2["name"], req.node)) - self.update() - if self.auto_write: - self.write_topological_map(self.filename) - - return succeded, meta_out - - - def update_node_name_cb(self, req): - """ - Changes a node's name and updates edges which involve the renamed node - """ - return self.update_node_name(req.node_name, req.new_name) - - - def update_node_name(self, node_name, new_name, update=True, write_map=True): - if new_name in self.names: - return False, "node with name {0} already exists".format(new_name) - - num_available, index = self.get_instances_of_node(node_name) - - if num_available == 1: - # update all the edges which involve the renamed node - for node in self.tmap2["nodes"]: - for edge in node["node"]["edges"]: - if node["node"]["name"] == node_name: - edge["edge_id"] = new_name + "_" + edge["node"] - if edge["node"] == node_name: - edge["node"] = new_name - edge["edge_id"] = node["node"]["name"] + "_" + new_name - - the_node = copy.deepcopy(self.tmap2["nodes"][index]) - the_node["meta"]["node"] = new_name - the_node["node"]["name"] = new_name - self.tmap2["nodes"][index] = the_node - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True, "" - else: - return False, "Multiple nodes with name {} found, or it does not exist".format(node_name) - - - def update_node_waypoint_cb(self, req): - """ - Updates a node's pose - """ - return self.update_node_waypoint(req.name, req.pose) - - - def update_node_waypoint(self, name, pose_msg, update=True, write_map=True): - - num_available, index = self.get_instances_of_node(name) - - if num_available == 1: - pose = rosidl_runtime_py.message_to_ordereddict(pose_msg) - - self.tmap2["nodes"][index]["node"]["pose"] = pose - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - else: - self.get_logger().error("Error updating the pose of node {}. {} instances of node with name {} found".format(name, num_available, name)) - return False - - - def update_node_tolerance_cb(self, req): - """ - Update node tolerances - """ - return self.update_node_tolerance(req.node_name, req.xy_tolerance, req.yaw_tolerance) - - - def update_node_tolerance(self, name, new_xy, new_yaw, update=True, write_map=True): - - num_available, index = self.get_instances_of_node(name) - - if num_available == 1: - the_node = copy.deepcopy(self.tmap2["nodes"][index]) - - if "properties" in the_node["node"]: - the_node["node"]["properties"]["xy_goal_tolerance"] = new_xy - the_node["node"]["properties"]["yaw_goal_tolerance"] = new_yaw - else: - properties = {} - properties["xy_goal_tolerance"] = new_xy - properties["yaw_goal_tolerance"] = new_yaw - the_node["node"]["properties"] = properties - - self.tmap2["nodes"][index] = the_node - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True, "" - else: - self.get_logger().error("Error updating the tolerance of node {}. {} instances of node with name {} found".format(name, num_available, name)) - return False, "" - - - def modify_tag_cb(self, msg): - """ - Changes the tag belonging to a node or a list of nodes - """ - succeded = True - meta_out = None - for node_name in msg.node: - available = [i for i, node in enumerate(self.tmap2["nodes"]) if node["node"]["name"] == node_name] - - for i in available: - meta = self.tmap2["nodes"][i]["meta"] - if "tag" in meta: - if not msg.tag in meta["tag"]: - continue - else: - tag_ind = meta["tag"].index(msg.tag) - meta["tag"][tag_ind] = msg.new_tag - - meta_out = str(meta) - - if len(available) == 0: - succeded = False - - if succeded: - self.update() - if self.auto_write: - self.write_topological_map(self.filename) - - return succeded, meta_out - - - def add_tag_cb(self, msg): - """ - Adds a tag to nodes in the map - """ - succeded = False - meta_out = None - for j in msg.node: - for node in self.tmap2["nodes"]: - if j == node["node"]["name"]: - succeded = True - if "tag" in node["meta"]: - if msg.tag not in node["meta"]["tag"]: - node["meta"]["tag"].append(msg.tag) - else: - a = [] - a.append(msg.tag) - node["meta"][ "tag"] = a - meta_out = str(node["meta"]) - - if succeded: - self.update() - if self.auto_write: - self.write_topological_map(self.filename) - - return succeded, meta_out - - - def rm_tag_cb(self, msg): - """ - Remove a tag from nodes in the map - """ - succeded = True - for node_name in msg.node: - available = [i for i, node in enumerate(self.tmap2["nodes"]) if node["node"]["name"] == node_name] - - succeded = False - meta_out = None - for i in available: - meta = self.tmap2["nodes"][i]["meta"] - if "tag" in meta: - if msg.tag in meta["tag"]: - print('removing tag') - meta["tag"].remove(msg.tag) - print('new list of tags') - print(meta["tag"]) - succeded = True - meta_out = str(meta) - - if succeded: - self.update() - if self.auto_write: - self.write_topological_map(self.filename) - - return succeded, meta_out - - - def add_param_to_edge_config_cb(self, req): - """ - Update edge reconfigure parameters. - """ - return self.add_param_to_edge_config(req.edge_id, req.namespace, req.name, req.value, req.value_is_string, req.not_reset) - - - def add_param_to_edge_config(self, edge_id, namespace, name, value, value_is_string, not_reset, update=True, write_map=True): - - if not value: - return False, "no value provided" - - if not value_is_string: - value = eval(value) - - node_name, _ = get_node_names_from_edge_id_2(self.tmap2, edge_id) - num_available, index = self.get_instances_of_node(node_name) - - param = {"namespace":namespace, "name":name, "value":value, "reset":not not_reset} - - if num_available == 1: - the_node = copy.deepcopy(self.tmap2["nodes"][index]) - msg = "" - for edge in the_node["node"]["edges"]: - if edge["edge_id"] == edge_id: - - config = copy.deepcopy(edge["config"]) - config = [i for i in config if not (i["namespace"] == param["namespace"] and i["name"] == param["name"])] - - self.get_logger().info("Adding param {} to edge {}".format(param, edge["edge_id"])) - config.append(param) - edge["config"] = config - - msg = "edge action is {} and edge config is {}".format(edge["action"], edge["config"]) - - self.tmap2["nodes"][index] = the_node - if update: - self.update() - if write_map and self.auto_write: - self.write_topological_map(self.filename) - - return True, msg - else: - self.get_logger().error("Cannot update edge {}. {} instances of node with name {} found".format(edge_id, num_available, node_name)) - return False, "No edge found or multiple edges found" - - - def rm_param_from_edge_config_cb(self, req): - """ - Remove a param from an edge's config. - """ - return self.rm_param_from_edge_config(req.edge_id, req.namespace, req.name) - - - def rm_param_from_edge_config(self, edge_id, namespace, name, update=True, write_map=True): - - node_name, _ = get_node_names_from_edge_id_2(self.tmap2, edge_id) - num_available, index = self.get_instances_of_node(node_name) - - if num_available == 1: - the_node = copy.deepcopy(self.tmap2["nodes"][index]) - msg = "" - for edge in the_node["node"]["edges"]: - if edge["edge_id"] == edge_id: - - config = copy.deepcopy(edge["config"]) - config = [i for i in config if not (i["namespace"] == namespace and i["name"] == name)] - - edge["config"] = config - msg = "edge action is {} and edge config is {}".format(edge["action"], edge["config"]) - - self.tmap2["nodes"][index] = the_node - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - - return True, msg - else: - self.get_logger().error("Cannot update edge {}. {} instances of node with name {} found".format(edge_id, num_available, node_name)) - return False, "No edge found or multiple edges found" - - - def rm_param_from_topological_map_cb(self, req): - """ - Remove all instances of a param from the topological map. - """ - return self.rm_param_from_topological_map(req.namespace, req.name) - - - def rm_param_from_topological_map(self, namespace, name, update=True, write_map=True): - - success = False - for node in self.tmap2["nodes"]: - for edge in node["node"]["edges"]: - config0 = copy.deepcopy(edge["config"]) - config0 = [i for i in config0 if (i["namespace"] == namespace and i["name"] == name)] - if config0: - success = True - - config = copy.deepcopy(edge["config"]) - config = [i for i in config if not (i["namespace"] == namespace and i["name"] == name)] - edge["config"] = config - - if success: - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True, "" - else: - return False, "parameter not found in topological map" - - - def update_node_restrictions_cb(self, req): - """ - Update a node's restrictions - """ - return self.update_node_restrictions(req.name, req.restrictions_planning, req.restrictions_runtime, req.update_edges) - - - def update_node_restrictions(self, node_name, restrictions_planning, restrictions_runtime, update_edges, update=True, write_map=True): - - num_available, index = self.get_instances_of_node(node_name) - - if num_available == 1: - if restrictions_planning: - self.tmap2["nodes"][index]["node"]["restrictions_planning"] = restrictions_planning - if restrictions_runtime: - self.tmap2["nodes"][index]["node"]["restrictions_runtime"] = restrictions_runtime - - edge_ids = [] - for node in self.tmap2["nodes"]: - for edge in node["node"]["edges"]: - if node["node"]["name"] == node_name or edge["node"] == node_name: - edge_ids.append(edge["edge_id"]) - - if restrictions_planning and update_edges: - for edge_id in set(edge_ids): - self.update_edge_restrictions(edge_id, restrictions_planning, "", False) - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True, "" - else: - self.get_logger().error("Error updating the restrictions of node {}. {} instances of node with name {} found".format(node_name, num_available, node_name)) - return False, "" - - - def update_edge_restrictions_cb(self, req): - """ - Update an edge's restrictions - """ - return self.update_edge_restrictions(req.name, req.restrictions_planning, req.restrictions_runtime) - - - def update_edge_restrictions(self, edge_id, restrictions_planning, restrictions_runtime, update=True, write_map=True): - - node_name, _ = get_node_names_from_edge_id_2(self.tmap2, edge_id) - num_available, index = self.get_instances_of_node(node_name) - - if num_available == 1: - the_node = copy.deepcopy(self.tmap2["nodes"][index]) - for edge in the_node["node"]["edges"]: - if edge["edge_id"] == edge_id: - if restrictions_planning: - edge["restrictions_planning"] = restrictions_planning - if restrictions_runtime: - edge["restrictions_runtime"] = restrictions_runtime - - self.tmap2["nodes"][index] = the_node - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True, "" - else: - self.get_logger().error("Error updating the restrictions of edge {}. {} instances of node with name {} found".format(edge_id, num_available, node_name)) - return False, "" - - - def update_edge_cb(self, req): - """ - Updates an edge's args (action, action type, goal etc) - """ - return self.update_edge(req.edge_id, req.action_name, req.action_type, req.goal, req.fail_policy, req.not_fluid) - - - def update_edge(self, edge_id, action_name, action_type, goal, fail_policy, not_fluid, update=True, write_map=True): - - node_name, _ = get_node_names_from_edge_id_2(self.tmap2, edge_id) - num_available, index = self.get_instances_of_node(node_name) - - if num_available == 1: - the_node = copy.deepcopy(self.tmap2["nodes"][index]) - for edge in the_node["node"]["edges"]: - if edge["edge_id"] == edge_id: - if action_name: - edge["action"] = action_name - if action_type: - edge["action_type"] = action_type - if goal: - edge["goal"] = json.loads(goal) - elif action_type and not goal: - _action_type, _goal = self.set_goal(action_name, action_type) - edge["action_type"] = _action_type - edge["goal"] = _goal - if fail_policy: - edge["fail_policy"] = fail_policy - if not_fluid: - edge["fluid_navigation"] = False - else: - edge["fluid_navigation"] = True - - self.tmap2["nodes"][index] = the_node - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - else: - self.get_logger().error("Cannot update edge {}. {} instances of node with name {} found".format(edge_id, num_available, node_name)) - return False - - - def update_action_cb(self, req): - """ - Updates the action type and goal for all action_name edges - """ - return self.update_action(req.action_name, req.action_type, req.goal) - - - def update_action(self, action_name, action_type, goal, update=True, write_map=True): - - success = False - for node in self.tmap2["nodes"]: - for edge in node["node"]["edges"]: - if edge["action"] == action_name: - if action_type: - edge["action_type"] = action_type - if goal: - edge["goal"] = json.loads(goal) - elif action_type and not goal: - _action_type, _goal = self.set_goal(action_name, action_type) - edge["action_type"] = _action_type - edge["goal"] = _goal - success = True - - if success: - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - - return success - - - def add_datum_cb(self, req): - """ - Adds GNSS latitude/longitude to the topological map's top-level meta info - """ - return self.add_datum(req.latitude, req.longitude) - - - def add_datum(self, latitude, longitude, update=True, write_map=True): - - try: - self.tmap2["meta"]["datum_latitude"] = latitude - self.tmap2["meta"]["datum_longitude"] = longitude - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - - except Exception as e: - self.get_logger().error(e) - return False - - - def update_fail_policy_cb(self, req): - """ - Update he fail policy of all edges in the map - """ - return self.update_fail_policy(req.fail_policy) - - - def update_fail_policy(self, fail_policy, update=True, write_map=True): - - if not fail_policy: - return False - - try: - for node in self.tmap2["nodes"]: - for edge in node["node"]["edges"]: - edge["fail_policy"] = fail_policy - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - - except Exception as e: - self.get_logger().error(e) - return False - - - def set_influence_zone_cb(self, req): - """ - Set the influence zone (vertices) of a node - """ - return self.set_influence_zone(req.name, req.vertices_x, req.vertices_y) - - - def set_influence_zone(self, node_name, vertices_x, vertices_y, update=True, write_map=True): - - num_available, index = self.get_instances_of_node(node_name) - - if num_available == 1: - - if len(vertices_x) < 3 or len(vertices_y) < 3 or len(vertices_x) != len(vertices_y): - self.get_logger().error("Invalid node vertices") - return False - else: - verts = [{"x":x, "y":y} for x, y in zip(vertices_x, vertices_y)] - - self.tmap2["nodes"][index]["node"]["verts"] = verts - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - else: - self.get_logger().error("Error updating the influence zone of node {}. {} instances of node with name {} found".format(node_name, num_available, node_name)) - return False - - - def clear_nodes_cb(self, req): - """ - Remove all nodes from the topological map - """ - self.clear_nodes() - - ans = Empty.response() - return ans - - - def clear_nodes(self, update=True, write_map=True): - - self.tmap2["nodes"] = [] - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - - - def add_topological_nodes_cb(self, req): - """ - Add a list of nodes to the topological map - """ - return self.add_topological_nodes(req.data) - - - def add_topological_nodes(self, data, update=True, write_map=True): - - for item in data: - success = self.add_topological_node(item.name, item.pose, add_close_nodes=False, update=False, write_map=False) - if not success: - return False - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - - - def add_edges_cb(self, req): - """ - Add a list of edges to the topological map - """ - return self.add_edges(req.data) - - - def add_edges(self, data, update=True, write_map=True): - - for item in data: - success = self.add_edge(item.origin, item.destination, item.action, item.action_type, item.edge_id, update=False, write_map=False) - if not success: - return False - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - - - def add_params_to_edges_cb(self, req): - """ - Add parameters to a list of edges - """ - return self.add_params_to_edges(req.data) - - - def add_params_to_edges(self, data, update=True, write_map=True): - - for item in data: - success,_ = self.add_param_to_edge_config(item.edge_id, item.namespace, item.name, item.value, item.value_is_string, item.not_reset, update=False, write_map=False) - if not success: - return False - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - - - def set_influence_zones_cb(self, req): - """ - Set the influence zone of a list of edges - """ - return self.set_influence_zones(req.data) - - - def set_influence_zones(self, data, update=True, write_map=True): - - for item in data: - success = self.set_influence_zone(item.name, item.vertices_x, item.vertices_y, update=False, write_map=False) - if not success: - return False - - if update: - self.update() - if self.auto_write and write_map: - self.write_topological_map(self.filename) - return True - - - def get_instances_of_node(self, node_name): - - num_available = 0 - index = None - for i, node in enumerate(self.tmap2["nodes"]): - if node["node"]["name"] == node_name: - num_available+=1 - index = i - - return num_available, index - - - def map_check(self): - - self.map_ok = True - - # check that all nodes have the same pointset - pointsets = [node["meta"]["pointset"] for node in self.tmap2["nodes"]] - if len(set(pointsets)) > 1: - self.get_logger().warn("Multiple poinsets found in meta info: {}".format(set(pointsets))) - self.map_ok = False - - # check for duplicate node names - names = self.create_list_of_nodes() - for name in set(names): - n = names.count(name) - if n > 1: - self.get_logger().warn("{} instances of node with name '{}' found".format(n, name)) - self.map_ok = False - - sep = "_" + str(uuid.uuid4()) + "_" - edge_ids = [node["node"]["name"] + sep + edge["node"] for node in self.tmap2["nodes"] for edge in node["node"]["edges"]] - edge_ids.sort() - - # check for duplicate edges - for edge in set(edge_ids): - edge_nodes = re.match("(.*)" + sep + "(.*)", edge).groups() - origin = edge_nodes[0] - destination = edge_nodes[1] - - n = edge_ids.count(edge) - if n > 1: - self.get_logger().warn("{} instances of edge with origin '{}' and destination '{}' found".format(n, origin, destination)) - self.map_ok = False - - # check that an edge's destination node exists - for edge in set(edge_ids): - edge_nodes = re.match("(.*)" + sep + "(.*)", edge).groups() - origin = edge_nodes[0] - destination = edge_nodes[1] - - if destination not in names: - self.get_logger().warn("Edge with origin '{}' has a destination '{}' that does not exist".format(origin, destination)) - self.map_ok = False - - # check if a node has an edge to itself - for node in self.tmap2["nodes"]: - for edge in node["node"]["edges"]: - if node["node"]["name"] == edge["node"]: - self.get_logger().warn("Edge with id '{}' has a destination '{}' equal to its origin".format(edge["edge_id"], edge["node"])) - self.map_ok = False - - - def tmap2_to_tmap(self): - self.points = map_manager_2.convert_tmap2_to_tmap(self.tmap2, self.pointset, self.metric_map) - - - @classmethod - def convert_tmap2_to_tmap(cls, tmap2, pointset, metric_map): - points = topological_navigation_msgs.msg.TopologicalMap() - - try: - point_set = pointset - points.name = point_set - points.pointset = point_set - points.map = metric_map - - for node in tmap2["nodes"]: - msg = topological_navigation_msgs.msg.TopologicalNode() - msg.name = node["node"]["name"] - msg.map = metric_map - msg.pointset = point_set - - msg.pose = Pose() - rosidl_runtime_py.set_message_fields(msg.pose, node["node"]["pose"]) - - msg.yaw_goal_tolerance = node["node"]["properties"]["yaw_goal_tolerance"] - msg.xy_goal_tolerance = node["node"]["properties"]["xy_goal_tolerance"] - - msgs_verts = [] - for v in node["node"]["verts"]: - msg_v = topological_navigation_msgs.msg.Vertex() - msg_v.x = v["x"] - msg_v.y = v["y"] - msgs_verts.append(msg_v) - msg.verts = msgs_verts - - msgs_edges = [] - for e in node["node"]["edges"]: - msg_e = topological_navigation_msgs.msg.Edge() - msg_e.edge_id = e["edge_id"] - msg_e.node = e["node"] - msg_e.action = e["action"] - msg_e.top_vel = 0.55 - msg_e.map_2d = metric_map - msg_e.inflation_radius = 0.0 - msg_e.recovery_behaviours_config = e["recovery_behaviours_config"] - msgs_edges.append(msg_e) - msg.edges = msgs_edges - - msg.localise_by_topic = node["node"]["localise_by_topic"] - points.nodes.append(msg) - - except Exception as e: - print(traceback.format_exc()) - print("Cannot convert map to the legacy format. The conversion requires all fields of the legacy map to be set.") - points = topological_navigation_msgs.msg.TopologicalMap() - - return points -######################################################################################################### diff --git a/topological_navigation/topological_navigation/map_marker.py b/topological_navigation/topological_navigation/map_marker.py deleted file mode 100644 index ab2d0b3c..00000000 --- a/topological_navigation/topological_navigation/map_marker.py +++ /dev/null @@ -1,210 +0,0 @@ -#!/usr/bin/env python - -import rospy - -from visualization_msgs.msg import * - -from topological_navigation_msgs.msg import TopologicalMap -from geometry_msgs.msg import Point - -import topological_navigation.tmap_utils as tmap_utils - - -class TopologicalVis(object): - _pallete=[[1,1,1],[0,0,0],[1,0,0],[0,1,0],[0,0,1],[1,1,0],[1,0,1],[0,1,1]] - def __init__(self) : - self.actions=[] - self.map_markers = MarkerArray() - self.topmap_pub = rospy.Publisher('topological_map_visualisation', MarkerArray, queue_size = 1, latch=True) - self._killall=False - self.lnodes = None - #Waiting for Topological Map - self.map_received=False - rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) - rospy.loginfo("Waiting for Topological map ...") - while not self.map_received and not self._killall : - rospy.sleep(rospy.Duration.from_sec(0.05)) - rospy.loginfo(" ...done") - - - self.publish_markers() - #rospy.loginfo("All Done ...") - - - def _update_everything(self) : - print("updating marker arrays ...") - del self.map_markers - self.map_markers = MarkerArray() - self.map_markers.markers=[] - - - idn = 0 - for i in self.lnodes.nodes : - marker = self.get_node_marker(i,idn) - self.map_markers.markers.append(marker) - idn += 1 - - marker = self.get_name_marker(i,idn) - self.map_markers.markers.append(marker) - idn += 1 - - for j in i.edges : - marker = self.get_edge_marker(i, j, idn) - if marker: - self.map_markers.markers.append(marker) - idn += 1 - - legend =0 - for k in self.actions: - marker = self.get_legend_marker(k, legend, idn) - self.map_markers.markers.append(marker) - idn += 1 - legend+=1 - - marker = self.get_zone_marker(i, idn) - self.map_markers.markers.append(marker) - idn += 1 - - self.publish_markers() - - def get_legend_marker(self, action, row, idn): - col=self.get_colour(self.actions.index(action)) - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = marker.TEXT_VIEW_FACING - marker.text=action - marker.pose.position.x= 1.0+(0.12*row) - marker.pose.position.y= 0.0 - marker.pose.position.z= 0.2 - marker.pose.orientation.w= 1.0 - marker.scale.z = 0.1 - marker.color.a = 0.5 - marker.color.r = col[0] - marker.color.g = col[1] - marker.color.b = col[2] - marker.ns='/legend' - return marker - - - def get_name_marker(self, node, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = marker.TEXT_VIEW_FACING - marker.text= node.name - marker.pose = node.pose - marker.scale.z = 0.12 - marker.color.a = 0.9 - marker.color.r = 0.3 - marker.color.g = 0.3 - marker.color.b = 0.3 - marker.ns='/names' - return marker - - - - def get_edge_marker(self, node, edge, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = marker.LINE_LIST - V1=Point() - V2=Point() - V1= node.pose.position - to_node=tmap_utils.get_node(self.lnodes, edge.node) - col=self.get_colour(self.actions.index(edge.action)) - #print col - if to_node: - V2= to_node.pose.position - marker.pose.orientation.w= 1.0 - marker.scale.x = 0.1 - marker.color.a = 0.5 - marker.color.r = col[0] - marker.color.g = col[1] - marker.color.b = col[2] - marker.points.append(V1) - marker.points.append(V2) - marker.ns='/edges' - return marker - else: - rospy.logwarn("No node %s found" %edge.node) - return None - - - def get_node_marker(self, node, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = Marker.SPHERE - marker.scale.x = 0.2 - marker.scale.y = 0.2 - marker.scale.z = 0.2 - marker.color.a = 0.4 - marker.color.r = 0.2 - marker.color.g = 0.2 - marker.color.b = 0.7 - marker.pose = node.pose - marker.pose.position.z = marker.pose.position.z+0.1 - marker.ns='/nodes' - return marker - - - def get_zone_marker(self, node, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = marker.LINE_STRIP - marker.pose.orientation.w= 1.0 - marker.scale.x = 0.1 - marker.color.a = 0.8 - marker.color.r = 0.7 - marker.color.g = 0.1 - marker.color.b = 0.2 - - for j in node.verts : - Vert = Point() - Vert.z = 0.05 - Vert.x = node.pose.position.x + j.x - Vert.y = node.pose.position.y + j.y - marker.points.append(Vert) - - Vert = Point() - Vert.z = 0.05 - Vert.x = node.pose.position.x + node.verts[0].x - Vert.y = node.pose.position.y + node.verts[0].y - marker.points.append(Vert) - marker.ns='/zones' - return marker - - def publish_markers(self): - self.topmap_pub.publish(MarkerArray()) - self.topmap_pub.publish(self.map_markers) - - - """ - MapCallback - - This function receives the Topological Map - """ - def MapCallback(self, msg) : - if self.lnodes: - del self.lnodes - self.lnodes = msg - - for i in self.lnodes.nodes: - for j in i.edges: - if not j.action in self.actions: - self.actions.append(j.action) - - print(self.actions) - - self.map_received = True - self._update_everything() - - def get_colour(self, number): - return self._pallete[number] - - - def on_node_shutdown(self): - self._killall=True diff --git a/topological_navigation/topological_navigation/marker_arrays.py b/topological_navigation/topological_navigation/marker_arrays.py deleted file mode 100644 index 04c04ee1..00000000 --- a/topological_navigation/topological_navigation/marker_arrays.py +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy - - -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point - -from visualization_msgs.msg import * -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation.topological_map import * - - -class waypoints_markers(object): - - def __init__(self, nodes) : - self.map_nodes = MarkerArray() - for i in nodes.nodes : - marker = Marker() - marker.header.frame_id = "map" - #marker.header.stamp = rospy.now() - #marker.type = marker.ARROW - marker.type = Marker.SPHERE - marker.scale.x = 0.2 - marker.scale.y = 0.2 - marker.scale.z = 0.2 - marker.color.a = 1.0 - marker.color.r = 0.2 - marker.color.g = 0.2 - marker.color.b = 0.7 - #marker.lifetime=2 - marker.pose = i._get_pose() - marker.pose.position.z = marker.pose.position.z+0.1 - self.map_nodes.markers.append(marker) - - - idn = 0 - for m in self.map_nodes.markers: - m.id = idn - idn += 1 - - - -class edges_marker(object): - - def __init__(self, nodes) : - self.map_edges = MarkerArray() - for node in nodes.nodes : - for i in node.edges : - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.LINE_LIST - ind = nodes._get_node_index(i['node']) - V1=Point() - V2=Point() - V1= (node._get_pose()).position - V2= (nodes.nodes[ind]._get_pose()).position - marker.scale.x = 0.1 - marker.color.a = 0.6 - marker.color.r = 0.1 - marker.color.g = 0.1 - marker.color.b = 0.1 - marker.points.append(V1) - marker.points.append(V2) - self.map_edges.markers.append(marker) - - idn = 0 - for m in self.map_edges.markers: - m.id = idn - idn += 1 - - -class vertices_marker(object): - - def __init__(self, nodes) : - self.node_zone = MarkerArray() - for node in nodes.nodes : - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.LINE_STRIP - marker.scale.x = 0.1 - marker.color.a = 0.8 - marker.color.r = 0.7 - marker.color.g = 0.1 - marker.color.b = 0.2 - node._get_coords() - count=0 - for i in node.vertices : - #print i[0], i[1] - Vert = Point() - Vert.z = 0.05 - Vert.x = node.px + i[0] - Vert.y = node.py + i[1] - marker.points.append(Vert) - #vertname = "%s-%d" %(node.name, count) - Pos = Pose() - Pos.position = Vert - # OJO: esto hay que hacerlo de alguna forma - #self._vertex_marker(vertname, Pos, vertname) - count+=1 - marker.points.append(marker.points[0]) - self.node_zone.markers.append(marker) - - idn = 0 - for m in self.node_zone.markers: - m.id = idn - idn += 1 - - diff --git a/topological_navigation/topological_navigation/navigation_graph.py b/topological_navigation/topological_navigation/navigation_graph.py new file mode 100644 index 00000000..54a9934f --- /dev/null +++ b/topological_navigation/topological_navigation/navigation_graph.py @@ -0,0 +1,627 @@ +""" +Navigation graph utilities using NetworkX. + +Provides graph-based route planning, state machine, action segment merging, +and boundary polygon computation for topological navigation. + +Key Features: + - Navigation state machine with validated transitions + - Route planning via NetworkX A* shortest path + - Action segment merging for consecutive same-type edges + - Boundary polygon computation for RowTraversal edges + - Route distance calculation + +Dependencies: + - networkx (>=2.5): Graph data structures and algorithms + - numpy (>=1.19): Numerical operations +""" + +import math +import networkx as nx +import numpy as np +from enum import Enum +from dataclasses import dataclass, field +from threading import Lock +from typing import List, Dict, Optional, Tuple, Any + + +# ============================================================================== +# State Machine +# ============================================================================== + +class NavState(Enum): + """Navigation state machine states. + + Lifecycle: + IDLE -> WAITING_FOR_MAP -> WAITING_FOR_LOCALISATION -> READY + READY -> PLANNING -> EXECUTING_* -> SUCCEEDED / FAILED / CANCELLED + SUCCEEDED / FAILED / CANCELLED -> READY + """ + + IDLE = "IDLE" + WAITING_FOR_MAP = "WAITING_FOR_MAP" + WAITING_FOR_LOCALISATION = "WAITING_FOR_LOCALISATION" + READY = "READY" + PLANNING = "PLANNING" + EXECUTING_NAVIGATE_TO_POSE = "EXECUTING_NAVIGATE_TO_POSE" + EXECUTING_GOAL_ALIGN = "EXECUTING_GOAL_ALIGN" + EXECUTING_ROW_TRAVERSAL = "EXECUTING_ROW_TRAVERSAL" + SUCCEEDED = "SUCCEEDED" + FAILED = "FAILED" + CANCELLED = "CANCELLED" + + +_EXECUTING_STATES = { + NavState.EXECUTING_NAVIGATE_TO_POSE, + NavState.EXECUTING_GOAL_ALIGN, + NavState.EXECUTING_ROW_TRAVERSAL, +} + +_TERMINAL_STATES = { + NavState.SUCCEEDED, + NavState.FAILED, + NavState.CANCELLED, +} + +_EXEC_AND_TERMINAL = _EXECUTING_STATES | _TERMINAL_STATES + +VALID_TRANSITIONS: Dict[NavState, set] = { + NavState.IDLE: {NavState.WAITING_FOR_MAP}, + NavState.WAITING_FOR_MAP: {NavState.WAITING_FOR_LOCALISATION}, + NavState.WAITING_FOR_LOCALISATION: {NavState.READY}, + NavState.READY: {NavState.PLANNING, NavState.CANCELLED}, + NavState.PLANNING: ( + _EXECUTING_STATES | {NavState.SUCCEEDED, NavState.FAILED, + NavState.CANCELLED} + ), + NavState.EXECUTING_NAVIGATE_TO_POSE: ( + _EXEC_AND_TERMINAL + ), + NavState.EXECUTING_GOAL_ALIGN: ( + _EXEC_AND_TERMINAL + ), + NavState.EXECUTING_ROW_TRAVERSAL: ( + _EXEC_AND_TERMINAL + ), + NavState.SUCCEEDED: {NavState.READY}, + NavState.FAILED: {NavState.READY}, + NavState.CANCELLED: {NavState.READY}, +} + + +ACTION_TO_STATE: Dict[str, NavState] = { + "NavigateToPose": NavState.EXECUTING_NAVIGATE_TO_POSE, + "navigate_to_pose": NavState.EXECUTING_NAVIGATE_TO_POSE, + "navigate_to_pose_goal_align": NavState.EXECUTING_GOAL_ALIGN, + "goal_align": NavState.EXECUTING_GOAL_ALIGN, + "GoalAlign": NavState.EXECUTING_GOAL_ALIGN, + "row_traversal": NavState.EXECUTING_ROW_TRAVERSAL, + "RowTraversal": NavState.EXECUTING_ROW_TRAVERSAL, +} + + +# ============================================================================== +# Action Name Normalisation (dual naming convention) +# ============================================================================== + +CANONICAL_ACTIONS: Dict[str, str] = { + # CamelCase forms + 'NavigateToPose': 'NavigateToPose', + 'RowTraversal': 'row_traversal', + 'GoalAlign': 'goal_align', + # snake_case forms + 'navigate_to_pose': 'NavigateToPose', + 'row_traversal': 'row_traversal', + 'goal_align': 'goal_align', +} + + +def normalize_action_name(name: str) -> str: + """Normalize edge action name to its canonical form. + + Both CamelCase and snake_case conventions are accepted:: + + 'NavigateToPose' / 'navigate_to_pose' -> 'NavigateToPose' + 'RowTraversal' / 'row_traversal' -> 'row_traversal' + 'GoalAlign' / 'goal_align' -> 'goal_align' + + Unknown action names are returned unchanged. + + Args: + name: Edge action name in either convention. + + Returns: + Canonical action name string. + """ + return CANONICAL_ACTIONS.get(name, name) + + +class NavStateMachine: + """Navigation state machine with validated transitions. + + Thread-safe via internal lock. All state reads/writes are atomic. + Invalid transitions are rejected with a warning log. + + Example: + >>> sm = NavStateMachine() + >>> sm.transition(NavState.WAITING_FOR_MAP) + True + >>> sm.state + + >>> sm.transition(NavState.READY) # invalid + False + """ + + def __init__(self, logger=None): + self._state = NavState.IDLE + self._logger = logger + self._lock = Lock() + + @property + def state(self) -> NavState: + """Current state (thread-safe read).""" + with self._lock: + return self._state + + def transition(self, new_state: NavState) -> bool: + """Attempt state transition. + + Args: + new_state: Target state. + + Returns: + True if transition succeeded, False if invalid. + """ + with self._lock: + old = self._state + valid = VALID_TRANSITIONS.get(old, set()) + + if new_state not in valid: + if self._logger: + self._logger.warning( + f"Invalid state transition: {old.value} -> " + f"{new_state.value}. Valid: " + f"{[s.value for s in sorted(valid, key=lambda s: s.value)]}" + ) + return False + + self._state = new_state + if self._logger: + self._logger.info( + f"[STATE] {old.value} -> {new_state.value}" + ) + return True + + def is_executing(self) -> bool: + """True if currently executing any action.""" + return self.state in _EXECUTING_STATES + + def is_terminal(self) -> bool: + """True if in a terminal state (SUCCEEDED/FAILED/CANCELLED).""" + return self.state in _TERMINAL_STATES + + def reset(self) -> bool: + """Reset to READY from a terminal state.""" + return self.transition(NavState.READY) + + +# ============================================================================== +# Action Segments +# ============================================================================== + +@dataclass +class ActionSegment: + """A batch of consecutive edges sharing the same action type. + + When a route contains consecutive edges with the same action, + they are merged into a single ActionSegment for efficient + batch execution and unified boundary publishing. + + Attributes: + action_type: Edge action name ('NavigateToPose', 'goal_align', + 'row_traversal'). + edge_ids: List of edge IDs in this segment. + source_nodes: List of source node names. + target_nodes: List of target node names. + edge_data: List of full edge attribute dicts from the graph. + """ + + action_type: str + edge_ids: List[str] = field(default_factory=list) + source_nodes: List[str] = field(default_factory=list) + target_nodes: List[str] = field(default_factory=list) + edge_data: List[Dict[str, Any]] = field(default_factory=list) + + @property + def is_empty(self) -> bool: + """True if segment contains no edges.""" + return len(self.edge_ids) == 0 + + @property + def first_source(self) -> Optional[str]: + """Name of the first source node, or None.""" + return self.source_nodes[0] if self.source_nodes else None + + @property + def last_target(self) -> Optional[str]: + """Name of the last target node, or None.""" + return self.target_nodes[-1] if self.target_nodes else None + + @property + def num_edges(self) -> int: + """Number of edges in this segment.""" + return len(self.edge_ids) + + +# ============================================================================== +# Route Planning +# ============================================================================== + +def _heuristic(u: str, v: str, graph: nx.DiGraph) -> float: + """Euclidean distance heuristic for A*.""" + ux, uy = graph.nodes[u]['x'], graph.nodes[u]['y'] + vx, vy = graph.nodes[v]['x'], graph.nodes[v]['y'] + return math.hypot(vx - ux, vy - uy) + + +def plan_route( + graph: nx.DiGraph, + origin: str, + target: str, + avoid_edges: Optional[List[str]] = None, + logger=None, + algorithm: str = 'astar', + weight: str = 'weight', +) -> Optional[List[str]]: + """Plan shortest route using NetworkX algorithms. + + Supports multiple path-finding algorithms selectable at runtime + via the ``algorithm`` parameter: + + - ``'astar'`` (default): A* with Euclidean distance heuristic. + Optimal and efficient for spatial graphs. + - ``'dijkstra'``: Dijkstra's algorithm. No heuristic -- explores + more nodes but handles non-spatial weights better. + + The ``weight`` parameter selects which edge attribute is used as + the path cost. By default this is ``'weight'`` (set to 1.0 + during graph construction but overridable via edge properties). + + Args: + graph: NetworkX DiGraph from build_graph_from_tmap(). + origin: Name of the origin node. + target: Name of the target node. + avoid_edges: Optional list of edge_ids to exclude. + logger: Optional ROS 2 logger. + algorithm: ``'astar'`` or ``'dijkstra'``. + weight: Edge attribute used as cost (default ``'weight'``). + + Returns: + Ordered list of node names [origin, ..., target], + or None if no route exists. + """ + if origin == target: + return [origin] + + if origin not in graph: + if logger: + logger.error(f"Origin node '{origin}' not in graph") + return None + + if target not in graph: + if logger: + logger.error(f"Target node '{target}' not in graph") + return None + + # Optionally filter out avoided edges + if avoid_edges: + avoid_set = set(avoid_edges) + view = nx.subgraph_view( + graph, + filter_edge=lambda u, v: ( + graph[u][v].get('edge_id', '') not in avoid_set + ), + ) + else: + view = graph + + try: + if algorithm == 'dijkstra': + path = nx.dijkstra_path( + view, origin, target, weight=weight, + ) + else: + # Default: A* with Euclidean heuristic + path = nx.astar_path( + view, origin, target, + heuristic=lambda u, v: _heuristic(u, v, graph), + weight=weight, + ) + return path + except nx.NetworkXNoPath: + if logger: + logger.warning(f"No route from '{origin}' to '{target}'") + return None + except nx.NodeNotFound as exc: + if logger: + logger.error(f"Node not found during planning: {exc}") + return None + + +def get_route_edges( + graph: nx.DiGraph, + route_nodes: List[str], +) -> List[Dict[str, Any]]: + """Extract edge data for consecutive nodes along a route. + + Args: + graph: NetworkX DiGraph with edge attributes. + route_nodes: Ordered node names [origin, ..., target]. + + Returns: + List of edge data dicts. Each contains at minimum: + ``edge_id``, ``source``, ``target``, ``action``, + ``action_type``, ``properties``, ``weight``. + """ + edges: List[Dict[str, Any]] = [] + for i in range(len(route_nodes) - 1): + src = route_nodes[i] + tgt = route_nodes[i + 1] + + if graph.has_edge(src, tgt): + attrs = dict(graph[src][tgt]) + attrs['source'] = src + attrs['target'] = tgt + edges.append(attrs) + + return edges + + +def merge_action_segments( + route_edges: List[Dict[str, Any]], + map_actions: Optional[Dict[str, Any]] = None, +) -> List[ActionSegment]: + """Merge consecutive same-action-type edges into segments. + + Given edges: [Nav, Nav, Row, Row, GoalAlign] + Produces: [Seg(Nav,2), Seg(Row,2), Seg(GoalAlign,1)] + + When ``map_actions`` is provided the ``composable`` flag from + each action configuration controls merging: + + - ``composable: true`` -- consecutive same-action edges are + merged into a single multi-waypoint segment. + - ``composable: false`` -- each edge becomes its own segment + even when the action name matches the previous edge. + + If ``map_actions`` is *None* (legacy maps without an ``actions`` + section), all edges are merged by action name as before. + + Args: + route_edges: Edge data dicts from :func:`get_route_edges`. + map_actions: Optional ``actions`` dict from the topological + map YAML. Keys are action names, values are config + dicts with at least a ``composable`` boolean. + + Returns: + List of ActionSegment instances with merged edges. + """ + if not route_edges: + return [] + + segments: List[ActionSegment] = [] + current: Optional[ActionSegment] = None + + for edge in route_edges: + action = edge.get('action', 'navigate_to_pose') + # Legacy maps: normalise CamelCase -> canonical form + if map_actions is None: + action = normalize_action_name(action) + + # Determine if this action type is composable + composable = True + if map_actions and action in map_actions: + composable = map_actions[action].get('composable', True) + + # Start a new segment when the action changes *or* when + # the action is explicitly non-composable. + if ( + current is None + or current.action_type != action + or not composable + ): + if current is not None: + segments.append(current) + current = ActionSegment(action_type=action) + + current.edge_ids.append(edge.get('edge_id', '')) + current.source_nodes.append(edge['source']) + current.target_nodes.append(edge['target']) + current.edge_data.append(edge) + + if current is not None: + segments.append(current) + + return segments + + +# ============================================================================== +# Boundary Polygon +# ============================================================================== + +def compute_boundary_polygon( + graph: nx.DiGraph, + segment: ActionSegment, + default_left: float = 0.5, + default_right: float = 0.5, +) -> List[Tuple[float, float]]: + """Compute boundary polygon for an action segment. + + Creates a corridor polygon around the path by offsetting each + waypoint perpendicularly. The polygon goes left-side forward + then right-side backward, forming a closed boundary. + + Boundary distances are read from the first edge's properties: + ``boundary_left`` and ``boundary_right``. Falls back to defaults. + + This is action-type agnostic -- any segment whose edges carry + ``boundary_left`` / ``boundary_right`` properties will produce + a corridor polygon. + + Args: + graph: NetworkX DiGraph with node position attributes. + segment: ActionSegment whose edges may carry boundary props. + default_left: Default left offset in meters. + default_right: Default right offset in meters. + + Returns: + List of ``(x, y)`` tuples forming a closed polygon. + Empty list if segment has fewer than 2 waypoints. + """ + if segment.is_empty: + return [] + + # Collect unique waypoints along the segment + waypoints: List[Tuple[float, float]] = [] + seen: List[str] = [] + + for edge in segment.edge_data: + src = edge['source'] + if src not in seen: + waypoints.append(( + float(graph.nodes[src]['x']), + float(graph.nodes[src]['y']), + )) + seen.append(src) + + tgt = edge['target'] + if tgt not in seen: + waypoints.append(( + float(graph.nodes[tgt]['x']), + float(graph.nodes[tgt]['y']), + )) + seen.append(tgt) + + if len(waypoints) < 2: + return [] + + # Read boundary distances from the first edge properties + props = segment.edge_data[0].get('properties', {}) + left_dist = float(props.get('boundary_left', default_left)) + right_dist = float(props.get('boundary_right', default_right)) + + # Compute perpendicular offsets at each waypoint + left_pts: List[Tuple[float, float]] = [] + right_pts: List[Tuple[float, float]] = [] + + for i in range(len(waypoints)): + # Direction vector: use forward difference, backward for last + if i < len(waypoints) - 1: + dx = waypoints[i + 1][0] - waypoints[i][0] + dy = waypoints[i + 1][1] - waypoints[i][1] + else: + dx = waypoints[i][0] - waypoints[i - 1][0] + dy = waypoints[i][1] - waypoints[i - 1][1] + + length = math.hypot(dx, dy) + if length < 1e-9: + continue + + # Perpendicular unit vector (left = rotate 90deg CCW) + perp_x = -dy / length + perp_y = dx / length + + x, y = waypoints[i] + left_pts.append((x + perp_x * left_dist, y + perp_y * left_dist)) + right_pts.append((x - perp_x * right_dist, y - perp_y * right_dist)) + + # Closed polygon: left side forward + right side backward + return left_pts + list(reversed(right_pts)) + + +# ============================================================================== +# NavRoute conversion +# ============================================================================== + +def plan_route_as_navroute( + graph: nx.DiGraph, + origin: str, + target: str, + avoid_edges: Optional[List[str]] = None, + logger=None, + algorithm: str = 'astar', + weight: str = 'weight', +): + """Plan a route and return a ``NavRoute`` message. + + Thin wrapper around :func:`plan_route` that converts the + resulting node list into the ``NavRoute`` format expected by + legacy services (``source[]`` and ``edge_id[]``). + + The caller must import ``NavRoute`` from + ``topological_navigation_msgs.msg``. + + Args: + graph: NetworkX DiGraph from :func:`build_graph_from_tmap`. + origin: Name of the origin node. + target: Name of the target node. + avoid_edges: Optional list of edge_ids to exclude. + logger: Optional ROS 2 logger. + algorithm: ``'astar'`` or ``'dijkstra'``. + weight: Edge attribute used as cost (default ``'weight'``). + + Returns: + ``NavRoute`` message with ``source`` and ``edge_id`` fields + populated when a route is found, or an empty ``NavRoute`` on + failure. + """ + from topological_navigation_msgs.msg import NavRoute + + route_msg = NavRoute() + route_nodes = plan_route( + graph, origin, target, + avoid_edges=avoid_edges, + logger=logger, + algorithm=algorithm, + weight=weight, + ) + if not route_nodes or len(route_nodes) < 2: + return route_msg + + route_edges = get_route_edges(graph, route_nodes) + for edge in route_edges: + route_msg.source.append(edge['source']) + route_msg.edge_id.append(edge.get('edge_id', '')) + + return route_msg + + +# ============================================================================== +# Distance +# ============================================================================== + +def get_route_distance( + graph: nx.DiGraph, + route_nodes: List[str], +) -> float: + """Compute total Euclidean distance along a route. + + Args: + graph: NetworkX DiGraph with node position attributes. + route_nodes: Ordered list of node names. + + Returns: + Total Euclidean distance in map units. + """ + if not route_nodes or len(route_nodes) < 2: + return 0.0 + + total = 0.0 + for i in range(len(route_nodes) - 1): + n1, n2 = route_nodes[i], route_nodes[i + 1] + if n1 in graph and n2 in graph: + dx = graph.nodes[n2]['x'] - graph.nodes[n1]['x'] + dy = graph.nodes[n2]['y'] - graph.nodes[n1]['y'] + total += math.hypot(dx, dy) + + return total diff --git a/topological_navigation/topological_navigation/navigation_stats.py b/topological_navigation/topological_navigation/navigation_stats.py deleted file mode 100644 index bcd64c03..00000000 --- a/topological_navigation/topological_navigation/navigation_stats.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python - -from datetime import datetime - -class nav_stats(object): - - def __init__(self, origin, target, topol_map, edge_id): - self.status= "active" - self.origin=origin - self.target=target - self.topological_map = topol_map - self.edge_id = edge_id - #self.date_at_node = datetime.now() - self.set_start() - - def set_start(self): - self.date_started = datetime.now() - self.date_at_node = self.date_started - - def set_ended(self, node): - self.final_node=node - self.date_finished = datetime.now() - self.get_operation_time() - self.get_time_to_wp() - - def set_at_node(self): - self.date_at_node = datetime.now() - - def get_operation_time(self): - operation_delta = self.date_finished - self.date_started - self.operation_time = operation_delta.total_seconds() - return self.operation_time - - def get_time_to_wp(self): - if self.date_at_node != self.date_started : - operation_delta = self.date_finished - self.date_at_node - self.time_to_wp = operation_delta.total_seconds() - else : - self.time_to_wp = 0 - return self.time_to_wp - - def get_start_time_str(self): - dt_text=self.date_started.strftime('%A, %B %d %Y, at %H:%M:%S hours') - return dt_text - - def get_finish_time_str(self): - dt_text=self.date_finished.strftime('%A, %B %d %Y, at %H:%M:%S hours') - return dt_text \ No newline at end of file diff --git a/topological_navigation/topological_navigation/networkx_utils.py b/topological_navigation/topological_navigation/networkx_utils.py new file mode 100644 index 00000000..5c579559 --- /dev/null +++ b/topological_navigation/topological_navigation/networkx_utils.py @@ -0,0 +1,2088 @@ +""" +NetworkX utilities for topological navigation. + +This module provides utilities for converting topological map data structures +into NetworkX graphs and performing graph-based operations for localization. + +Key Features: +- Convert YAML topological maps to NetworkX DiGraph +- Build KD-tree spatial index for efficient nearest neighbor search +- Point-in-polygon checks for influence zone localization +- Edge distance calculations using vectorized operations +- NetworkX algorithm wrappers for shortest paths and connectivity + +Performance: +- KD-tree construction: O(n log n) where n is number of nodes +- KD-tree query: O(log n) average case for nearest neighbor +- Point-in-polygon: O(m) where m is number of polygon vertices +- Edge distance: O(e) where e is number of edges (vectorized) + +Dependencies: +- networkx (>=2.5): Graph data structures and algorithms +- scipy (>=1.5): KD-tree spatial indexing +- numpy (>=1.19): Numerical operations +""" + +import json +import numpy as np +import networkx as nx +from scipy.spatial import KDTree +from typing import Dict, Any, Optional, Tuple, List + + +def build_graph_from_tmap(tmap_data: Dict[str, Any], logger=None) -> Optional[nx.DiGraph]: + """ + Convert topological map YAML data to NetworkX DiGraph. + + This function takes a topological map dictionary (typically loaded from YAML) + and converts it into a NetworkX directed graph representation. Node positions, + influence zones, properties, and edge metadata are stored as graph attributes. + + Args: + tmap_data: Dictionary containing topological map data with structure: + logger: Optional ROS 2 logger for error messages + { + 'nodes': [ + { + 'node': { + 'name': str, + 'pose': { + 'position': {'x': float, 'y': float, 'z': float}, + 'orientation': {'x': float, 'y': float, 'z': float, 'w': float} + }, + 'verts': [{'x': float, 'y': float}, ...], + 'parent_frame': str, + 'properties': dict (optional), + 'localise_by_topic': str (optional), + 'edges': [ + { + 'edge_id': str, + 'node': str, + 'action': str, + 'action_type': str (optional), + 'properties': dict (optional) + }, + ... + ] + }, + 'meta': dict (optional) + }, + ... + ] + } + + Returns: + NetworkX DiGraph with node and edge attributes, or None if map data is invalid. + + Node attributes: + - name: str - Node name (also the node ID) + - x, y, z: float - Position coordinates + - orientation: dict - Quaternion {x, y, z, w} + - verts: list - Influence zone vertices [{'x': float, 'y': float}, ...] + - parent_frame: str - Coordinate frame (structural) + - nav_frame: str - Navigation frame override for PoseStamped + goals. Empty string means "use map-level default + (topo_frame_id)". + - properties: dict - Optional user-defined properties + - localise_by_topic: str - JSON config string for topic-based localization + - meta: dict - Metadata including tags + + Edge attributes: + - edge_id: str - Unique edge identifier + - action: str - Action name (e.g., "NavigateToPose") + - action_type: str - ROS 2 action type + - properties: dict - Optional user-defined properties + - weight: float - Edge weight for shortest path (default: 1.0) + + Example: + >>> import yaml + >>> with open('map.yaml') as f: + ... tmap_data = yaml.safe_load(f) + >>> graph = build_graph_from_tmap(tmap_data) + >>> if graph: + ... print(f"Graph has {graph.number_of_nodes()} nodes") + ... node_attrs = graph.nodes['WP1'] + ... print(f"WP1 position: ({node_attrs['x']}, {node_attrs['y']})") + + Raises: + No exceptions are raised. Invalid data returns None. + """ + # Validate required fields + if not isinstance(tmap_data, dict): + if logger: + logger.error("Invalid map data: Expected dictionary, got {}".format(type(tmap_data).__name__)) + return None + + if 'nodes' not in tmap_data: + if logger: + logger.error("Invalid map data: Missing required 'nodes' field") + return None + + if not isinstance(tmap_data['nodes'], list): + if logger: + logger.error("Invalid map data: 'nodes' field must be a list, got {}".format(type(tmap_data['nodes']).__name__)) + return None + + # Create directed graph + G = nx.DiGraph() + + # Resolve default nav_frame from transformation section + tx = tmap_data.get('transformation', {}) + default_nav_frame = tx.get('topo_frame_id', tx.get('parent', 'map')) + + def _resolve_tx_ref(value): + """Resolve ``${transformation.}`` to a concrete value.""" + if ( + isinstance(value, str) + and value.startswith('${transformation.') + and value.endswith('}') + ): + key = value[len('${transformation.'):-1] + return tx.get(key, value) + return value + + try: + # Process each node in the map + for idx, node_data in enumerate(tmap_data['nodes']): + # Validate node structure + if not isinstance(node_data, dict): + if logger: + logger.warning(f"Skipping node at index {idx}: Expected dictionary, got {type(node_data).__name__}") + continue + + if 'node' not in node_data: + if logger: + logger.warning(f"Skipping node at index {idx}: Missing 'node' field") + continue + + node = node_data['node'] + + # Validate required node fields + if 'name' not in node: + if logger: + logger.warning(f"Skipping node at index {idx}: Missing required 'name' field") + continue + + node_name = node['name'] + + if 'pose' not in node: + if logger: + logger.warning(f"Skipping node '{node_name}': Missing required 'pose' field") + continue + + pose = node['pose'] + if 'position' not in pose: + if logger: + logger.warning(f"Skipping node '{node_name}': Missing 'position' in pose") + continue + + if 'orientation' not in pose: + if logger: + logger.warning(f"Skipping node '{node_name}': Missing 'orientation' in pose") + continue + + position = pose['position'] + orientation = pose['orientation'] + + # Validate position coordinates + if not all(key in position for key in ['x', 'y', 'z']): + if logger: + missing = [k for k in ['x', 'y', 'z'] if k not in position] + logger.warning(f"Skipping node '{node_name}': Missing position coordinates: {missing}") + continue + + # Validate orientation quaternion + if not all(key in orientation for key in ['x', 'y', 'z', 'w']): + if logger: + missing = [k for k in ['x', 'y', 'z', 'w'] if k not in orientation] + logger.warning(f"Skipping node '{node_name}': Missing orientation components: {missing}") + continue + + # Extract node attributes with defaults for optional fields + try: + node_attrs = { + 'name': node_name, + 'x': float(position['x']), + 'y': float(position['y']), + 'z': float(position['z']), + 'orientation': { + 'x': float(orientation['x']), + 'y': float(orientation['y']), + 'z': float(orientation['z']), + 'w': float(orientation['w']) + }, + 'verts': node.get('verts', []), + 'parent_frame': node.get('parent_frame', 'map'), + 'nav_frame': _resolve_tx_ref( + node.get('nav_frame', '') + ) or default_nav_frame, + 'properties': node.get('properties', {}), + 'localise_by_topic': node.get('localise_by_topic', ''), + 'meta': node_data.get('meta', {}) + } + except (ValueError, TypeError) as e: + if logger: + logger.warning(f"Skipping node '{node_name}': Error converting coordinates to float: {e}") + continue + + # Add node to graph + G.add_node(node_name, **node_attrs) + + # Process edges for this node + edges = node.get('edges', []) + if not isinstance(edges, list): + if logger: + logger.warning(f"Node '{node_name}': 'edges' field must be a list, got {type(edges).__name__}") + continue + + for edge_idx, edge in enumerate(edges): + # Validate edge structure + if not isinstance(edge, dict): + if logger: + logger.warning(f"Node '{node_name}': Skipping edge at index {edge_idx}: Expected dictionary") + continue + + if 'edge_id' not in edge: + if logger: + logger.warning(f"Node '{node_name}': Skipping edge at index {edge_idx}: Missing 'edge_id'") + continue + + if 'node' not in edge: + if logger: + logger.warning(f"Node '{node_name}': Skipping edge '{edge.get('edge_id', 'unknown')}': Missing target 'node'") + continue + + if 'action' not in edge: + if logger: + logger.warning(f"Node '{node_name}': Skipping edge '{edge['edge_id']}': Missing 'action'") + continue + + target_node = edge['node'] + + # Extract edge attributes + edge_props = edge.get('properties', {}) + if not isinstance(edge_props, dict): + if logger: + logger.warning(f"Node '{node_name}': Edge '{edge['edge_id']}': 'properties' must be a dictionary, using empty dict") + edge_props = {} + + try: + edge_attrs = { + 'edge_id': edge['edge_id'], + 'action': edge['action'], + 'action_type': edge.get('action_type', ''), + 'properties': edge_props, + 'weight': float(edge_props.get('weight', 1.0)) # Default weight for shortest path + } + except (ValueError, TypeError) as e: + if logger: + logger.warning(f"Node '{node_name}': Edge '{edge['edge_id']}': Error converting weight to float: {e}, using default 1.0") + edge_attrs = { + 'edge_id': edge['edge_id'], + 'action': edge['action'], + 'action_type': edge.get('action_type', ''), + 'properties': edge_props, + 'weight': 1.0 + } + + # Add edge to graph + G.add_edge(node_name, target_node, **edge_attrs) + + # Check if graph has any nodes + if G.number_of_nodes() == 0: + if logger: + logger.error("Invalid map data: No valid nodes found in map") + return None + + return G + + except Exception as e: + # Catch any unexpected errors + if logger: + logger.error(f"Unexpected error building graph from map data: {e}") + return None + + +def build_kdtree_from_graph(graph: Optional[nx.DiGraph], logger=None) -> Tuple[Optional[KDTree], List[str]]: + """ + Build KD-tree spatial index from node positions in NetworkX graph. + + This function extracts 2D coordinates (x, y) from all nodes in the graph + and constructs a scipy.spatial.KDTree for efficient O(log n) nearest + neighbor spatial queries. The KD-tree enables fast localization by quickly + finding the closest nodes to the robot's current position. + + The function returns both the KD-tree and a list of node names in the same + order as the points in the tree, allowing efficient mapping from KD-tree + query results back to node names. + + Args: + graph: NetworkX DiGraph with node position attributes ('x', 'y'). + Can be None or empty, in which case (None, []) is returned. + logger: Optional ROS 2 logger for warning/error messages. + + Returns: + Tuple of (kdtree, node_names) where: + - kdtree: scipy.spatial.KDTree for spatial queries, or None if graph is empty/invalid + - node_names: List of node names in same order as kdtree points (empty list if no tree) + + Performance: + - Construction: O(n log n) where n is number of nodes + - Memory: O(n) for storing 2D points + - Query: O(log n) average case for nearest neighbor search + + Example: + >>> import networkx as nx + >>> G = nx.DiGraph() + >>> G.add_node('WP1', x=0.0, y=0.0, z=0.0) + >>> G.add_node('WP2', x=5.0, y=0.0, z=0.0) + >>> G.add_node('WP3', x=5.0, y=5.0, z=0.0) + >>> kdtree, node_names = build_kdtree_from_graph(G) + >>> # Query nearest neighbor to point (2.5, 0.0) + >>> distances, indices = kdtree.query([[2.5, 0.0]], k=1) + >>> closest_node = node_names[indices[0][0]] + >>> print(f"Closest node: {closest_node}") # 'WP1' + + Edge Cases: + - None graph: Returns (None, []) + - Empty graph: Returns (None, []) + - Single node: Returns valid KDTree with one point + - Missing 'x' or 'y' attributes: Node is skipped with warning + + Raises: + No exceptions are raised. Invalid input returns (None, []). + + Requirements: + Validates: Requirements 2.1, 2.2, 2.5, 15.2, 15.3 + - 2.1: Build KD-tree index from node positions + - 2.2: Store 2D coordinates (x, y) for each node + - 2.5: Rebuild KD-tree when map is updated + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Handle None or empty graph + if graph is None: + if logger: + logger.debug("Cannot build KD-tree: graph is None") + return None, [] + + if graph.number_of_nodes() == 0: + if logger: + logger.debug("Cannot build KD-tree: graph has no nodes") + return None, [] + + # Extract 2D coordinates and node names + points = [] + node_names = [] + + for node_name in graph.nodes(): + node_attrs = graph.nodes[node_name] + + # Validate required attributes + if 'x' not in node_attrs or 'y' not in node_attrs: + if logger: + logger.warning( + f"Skipping node '{node_name}' in KD-tree construction: " + f"Missing required 'x' or 'y' coordinate" + ) + continue + + try: + # Extract 2D coordinates (x, y only, z is not used for spatial indexing) + x = float(node_attrs['x']) + y = float(node_attrs['y']) + + points.append([x, y]) + node_names.append(node_name) + + except (ValueError, TypeError) as e: + if logger: + logger.warning( + f"Skipping node '{node_name}' in KD-tree construction: " + f"Invalid coordinate values: {e}" + ) + continue + + # Check if we have any valid points + if len(points) == 0: + if logger: + logger.error( + "Cannot build KD-tree: No nodes with valid 'x' and 'y' coordinates found" + ) + return None, [] + + # Build KD-tree from 2D points + try: + kdtree = KDTree(np.array(points)) + + if logger: + logger.debug( + f"Built KD-tree with {len(node_names)} nodes for efficient spatial queries" + ) + + return kdtree, node_names + + except Exception as e: + if logger: + logger.error(f"Failed to build KD-tree: {e}") + return None, [] + + +def query_nearest_nodes(kdtree: Optional[KDTree], node_names: List[str], pose, k: int = 3) -> List[Dict[str, Any]]: + """ + Query k-nearest nodes to a pose using KD-tree spatial index. + + This function performs efficient O(log n) nearest neighbor search using + a pre-built KD-tree to find the k closest nodes to a given robot pose. + Results are returned as a sorted list of dictionaries containing node + names and their Euclidean distances from the query pose. + + The function handles both single nearest neighbor queries (k=1) and + k-nearest neighbors queries (k>1), automatically adjusting for edge + cases where k exceeds the number of available nodes. + + Args: + kdtree: scipy.spatial.KDTree built from node positions. + Can be None, in which case an empty list is returned. + node_names: List of node names corresponding to kdtree points. + Must be in the same order as points used to build kdtree. + Can be empty, in which case an empty list is returned. + pose: geometry_msgs.msg.Pose object with position attributes. + Only position.x and position.y are used for 2D spatial query. + k: Number of nearest neighbors to return (default: 3). + If k > number of nodes, returns all nodes. + Must be >= 1. + + Returns: + List of dictionaries sorted by distance (ascending): + [ + {'node': str, 'dist': float}, # Closest node + {'node': str, 'dist': float}, # 2nd closest + ... + ] + Returns empty list if kdtree is None, node_names is empty, or k < 1. + + Performance: + - Time: O(log n) for k=1, O(k log n) for k>1 where n is number of nodes + - Space: O(k) for storing results + - Query point: 2D coordinates (x, y) extracted from pose + + Example: + >>> from scipy.spatial import KDTree + >>> import numpy as np + >>> from geometry_msgs.msg import Pose + >>> + >>> # Build KD-tree + >>> points = np.array([[0.0, 0.0], [5.0, 0.0], [5.0, 5.0]]) + >>> node_names = ['WP1', 'WP2', 'WP3'] + >>> kdtree = KDTree(points) + >>> + >>> # Create query pose + >>> pose = Pose() + >>> pose.position.x = 2.5 + >>> pose.position.y = 0.0 + >>> + >>> # Query 3 nearest nodes + >>> results = query_nearest_nodes(kdtree, node_names, pose, k=3) + >>> print(results) + [{'node': 'WP1', 'dist': 2.5}, {'node': 'WP2', 'dist': 2.5}, {'node': 'WP3', 'dist': 5.59}] + >>> + >>> # Query single nearest node + >>> results = query_nearest_nodes(kdtree, node_names, pose, k=1) + >>> print(results[0]['node']) # 'WP1' or 'WP2' (both equidistant) + + Edge Cases: + - kdtree is None: Returns [] + - node_names is empty: Returns [] + - k < 1: Returns [] + - k > len(node_names): Returns all nodes sorted by distance + - k = 1: Returns single-element list (handles scipy's different return format) + - Multiple nodes at same distance: Order is determined by KD-tree implementation + + Raises: + No exceptions are raised. Invalid input returns empty list. + + Requirements: + Validates: Requirements 2.3, 2.4, 4.1, 4.2, 4.3, 15.2, 15.3 + - 2.3: Use KD-tree nearest neighbor search for closest node queries + - 2.4: KD-tree query returns results in O(log n) average time + - 4.1: Calculate distance to closest node using KD-tree + - 4.2: Calculate distances to multiple nodes using k-nearest neighbors + - 4.3: Sort nodes by distance in ascending order + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate inputs + if kdtree is None: + return [] + + if not node_names or len(node_names) == 0: + return [] + + if k < 1: + return [] + + # Limit k to available nodes + k_actual = min(k, len(node_names)) + + # Extract 2D query point from pose + query_point = np.array([[pose.position.x, pose.position.y]]) + + try: + # Query k-nearest neighbors from KD-tree + distances, indices = kdtree.query(query_point, k=k_actual) + + # Ensure distances and indices are 1-D arrays. + # scipy returns shape (1,) for k=1 and shape (1, k) for k>1 + # when query_point has shape (1, 2). + distances = np.atleast_1d(distances).flatten() + indices = np.atleast_1d(indices).flatten() + + # Build result list with node names and distances + results = [] + for dist, idx in zip(distances, indices): + results.append({ + 'node': node_names[int(idx)], + 'dist': float(dist) + }) + + # Results are already sorted by distance (KD-tree returns sorted results) + return results + + except Exception as e: + # Handle any unexpected errors from KD-tree query + # This should rarely happen with valid inputs + return [] + + + +def compute_shortest_path(graph: Optional[nx.DiGraph], source: str, target: str, + weight: str = 'weight', logger=None) -> List[str]: + """ + Compute shortest path between two nodes using Dijkstra's algorithm. + + This function uses NetworkX's dijkstra_path implementation to find the + optimal path between source and target nodes in a weighted directed graph. + The algorithm guarantees the shortest path for graphs with non-negative + edge weights. + + The function is a wrapper around networkx.dijkstra_path that provides + consistent error handling and logging for the topological navigation system. + + Args: + graph: NetworkX DiGraph representing the topological map. + Can be None, in which case an empty list is returned. + source: Source node name (starting point of path). + Must exist in the graph. + target: Target node name (destination of path). + Must exist in the graph. + weight: Edge attribute to use as weight for path computation (default: 'weight'). + If an edge doesn't have this attribute, weight of 1.0 is assumed. + logger: Optional ROS 2 logger for warning/error messages. + + Returns: + List of node names representing the shortest path from source to target, + including both source and target nodes. Returns empty list if: + - graph is None or empty + - source or target node doesn't exist + - no path exists between source and target + + Performance: + - Time: O((V + E) log V) using binary heap, where V is vertices, E is edges + - Space: O(V) for storing distances and predecessors + - Algorithm: Dijkstra's shortest path algorithm + + Example: + >>> import networkx as nx + >>> G = nx.DiGraph() + >>> G.add_edge('A', 'B', weight=1.0) + >>> G.add_edge('B', 'C', weight=2.0) + >>> G.add_edge('A', 'C', weight=5.0) + >>> path = compute_shortest_path(G, 'A', 'C') + >>> print(path) # ['A', 'B', 'C'] (total weight: 3.0) + >>> + >>> # No path exists + >>> G.add_node('D') # Isolated node + >>> path = compute_shortest_path(G, 'A', 'D') + >>> print(path) # [] + + Edge Cases: + - graph is None: Returns [] + - Empty graph: Returns [] + - source == target: Returns [source] + - source not in graph: Returns [] + - target not in graph: Returns [] + - No path exists: Returns [] (logs warning if logger provided) + - Missing weight attribute: Uses default weight of 1.0 + + Raises: + No exceptions are raised. All errors are handled gracefully and return empty list. + + Requirements: + Validates: Requirements 3.1, 3.2, 3.3, 15.2, 15.3 + - 3.1: Use networkx.shortest_path or networkx.dijkstra_path for shortest paths + - 3.2: Check graph connectivity using NetworkX algorithms + - 3.3: Compute path lengths using networkx.shortest_path_length + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate graph + if graph is None: + if logger: + logger.debug("Cannot compute shortest path: graph is None") + return [] + + if graph.number_of_nodes() == 0: + if logger: + logger.debug("Cannot compute shortest path: graph is empty") + return [] + + # Validate source and target nodes exist + if source not in graph.nodes: + if logger: + logger.warning(f"Cannot compute shortest path: source node '{source}' not in graph") + return [] + + if target not in graph.nodes: + if logger: + logger.warning(f"Cannot compute shortest path: target node '{target}' not in graph") + return [] + + # Handle trivial case: source == target + if source == target: + return [source] + + try: + # Use Dijkstra's algorithm for weighted shortest path + path = nx.dijkstra_path(graph, source, target, weight=weight) + + if logger: + logger.debug( + f"Found shortest path from '{source}' to '{target}': " + f"{' -> '.join(path)} ({len(path)} nodes)" + ) + + return path + + except nx.NetworkXNoPath: + # No path exists between source and target + if logger: + logger.warning( + f"No path exists from '{source}' to '{target}' in the graph" + ) + return [] + + except nx.NodeNotFound as e: + # This shouldn't happen since we check above, but handle it anyway + if logger: + logger.error(f"Node not found during path computation: {e}") + return [] + + except Exception as e: + # Catch any unexpected errors + if logger: + logger.error(f"Unexpected error computing shortest path: {e}") + return [] + + +def compute_path_length(graph: Optional[nx.DiGraph], source: str, target: str, + weight: str = 'weight', logger=None) -> float: + """ + Compute shortest path length between two nodes using Dijkstra's algorithm. + + This function uses NetworkX's dijkstra_path_length implementation to + calculate the total weight (length) of the shortest path between source + and target nodes without computing the actual path. This is more efficient + than compute_shortest_path() when only the distance is needed. + + Args: + graph: NetworkX DiGraph representing the topological map. + Can be None, in which case infinity is returned. + source: Source node name (starting point). + Must exist in the graph. + target: Target node name (destination). + Must exist in the graph. + weight: Edge attribute to use as weight for path computation (default: 'weight'). + If an edge doesn't have this attribute, weight of 1.0 is assumed. + logger: Optional ROS 2 logger for warning/error messages. + + Returns: + float: Total weight (length) of the shortest path from source to target. + Returns float('inf') if: + - graph is None or empty + - source or target node doesn't exist + - no path exists between source and target + + Performance: + - Time: O((V + E) log V) using binary heap, where V is vertices, E is edges + - Space: O(V) for storing distances + - Algorithm: Dijkstra's shortest path algorithm (length only, no path reconstruction) + - More efficient than compute_shortest_path() when path is not needed + + Example: + >>> import networkx as nx + >>> G = nx.DiGraph() + >>> G.add_edge('A', 'B', weight=1.5) + >>> G.add_edge('B', 'C', weight=2.5) + >>> G.add_edge('A', 'C', weight=5.0) + >>> length = compute_path_length(G, 'A', 'C') + >>> print(length) # 4.0 (via B: 1.5 + 2.5) + >>> + >>> # No path exists + >>> G.add_node('D') # Isolated node + >>> length = compute_path_length(G, 'A', 'D') + >>> print(length) # inf + + Edge Cases: + - graph is None: Returns inf + - Empty graph: Returns inf + - source == target: Returns 0.0 + - source not in graph: Returns inf + - target not in graph: Returns inf + - No path exists: Returns inf (logs warning if logger provided) + - Missing weight attribute: Uses default weight of 1.0 + + Raises: + No exceptions are raised. All errors are handled gracefully and return infinity. + + Requirements: + Validates: Requirements 3.1, 3.3, 15.2, 15.3 + - 3.1: Use NetworkX algorithms for graph operations + - 3.3: Compute path lengths using networkx.shortest_path_length + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate graph + if graph is None: + if logger: + logger.debug("Cannot compute path length: graph is None") + return float('inf') + + if graph.number_of_nodes() == 0: + if logger: + logger.debug("Cannot compute path length: graph is empty") + return float('inf') + + # Validate source and target nodes exist + if source not in graph.nodes: + if logger: + logger.warning(f"Cannot compute path length: source node '{source}' not in graph") + return float('inf') + + if target not in graph.nodes: + if logger: + logger.warning(f"Cannot compute path length: target node '{target}' not in graph") + return float('inf') + + # Handle trivial case: source == target + if source == target: + return 0.0 + + try: + # Use Dijkstra's algorithm to compute path length + length = nx.dijkstra_path_length(graph, source, target, weight=weight) + + if logger: + logger.debug( + f"Shortest path length from '{source}' to '{target}': {length:.2f}" + ) + + return float(length) + + except nx.NetworkXNoPath: + # No path exists between source and target + if logger: + logger.warning( + f"No path exists from '{source}' to '{target}' in the graph" + ) + return float('inf') + + except nx.NodeNotFound as e: + # This shouldn't happen since we check above, but handle it anyway + if logger: + logger.error(f"Node not found during path length computation: {e}") + return float('inf') + + except Exception as e: + # Catch any unexpected errors + if logger: + logger.error(f"Unexpected error computing path length: {e}") + return float('inf') + + +def check_connectivity(graph: Optional[nx.DiGraph], source: str, target: str, logger=None) -> bool: + """ + Check if a path exists between two nodes in a directed graph. + + This function uses NetworkX's has_path implementation to determine whether + there is any directed path from source to target. This is more efficient + than computing the actual path when only connectivity information is needed. + + Note: For directed graphs, connectivity is not symmetric. A path from A to B + does not imply a path from B to A. + + Args: + graph: NetworkX DiGraph representing the topological map. + Can be None, in which case False is returned. + source: Source node name (starting point). + Must exist in the graph. + target: Target node name (destination). + Must exist in the graph. + logger: Optional ROS 2 logger for warning/error messages. + + Returns: + bool: True if a directed path exists from source to target, False otherwise. + Returns False if: + - graph is None or empty + - source or target node doesn't exist + - no path exists between source and target + + Performance: + - Time: O(V + E) using breadth-first search, where V is vertices, E is edges + - Space: O(V) for visited set + - Algorithm: BFS-based reachability check + - More efficient than computing full shortest path + + Example: + >>> import networkx as nx + >>> G = nx.DiGraph() + >>> G.add_edge('A', 'B') + >>> G.add_edge('B', 'C') + >>> + >>> # Path exists A -> B -> C + >>> print(check_connectivity(G, 'A', 'C')) # True + >>> + >>> # No reverse path (directed graph) + >>> print(check_connectivity(G, 'C', 'A')) # False + >>> + >>> # Isolated node + >>> G.add_node('D') + >>> print(check_connectivity(G, 'A', 'D')) # False + >>> + >>> # Same node + >>> print(check_connectivity(G, 'A', 'A')) # True + + Edge Cases: + - graph is None: Returns False + - Empty graph: Returns False + - source == target: Returns True (node is reachable from itself) + - source not in graph: Returns False + - target not in graph: Returns False + - Disconnected components: Returns False if nodes in different components + + Raises: + No exceptions are raised. All errors are handled gracefully and return False. + + Requirements: + Validates: Requirements 3.2, 3.5, 15.2, 15.3 + - 3.2: Check graph connectivity using networkx.is_connected or networkx.has_path + - 3.5: Use networkx.neighbors() for finding adjacent nodes + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate graph + if graph is None: + if logger: + logger.debug("Cannot check connectivity: graph is None") + return False + + if graph.number_of_nodes() == 0: + if logger: + logger.debug("Cannot check connectivity: graph is empty") + return False + + # Validate source and target nodes exist + if source not in graph.nodes: + if logger: + logger.warning(f"Cannot check connectivity: source node '{source}' not in graph") + return False + + if target not in graph.nodes: + if logger: + logger.warning(f"Cannot check connectivity: target node '{target}' not in graph") + return False + + # Handle trivial case: source == target + if source == target: + return True + + try: + # Use NetworkX has_path for efficient connectivity check + has_path = nx.has_path(graph, source, target) + + if logger: + logger.debug( + f"Connectivity check from '{source}' to '{target}': " + f"{'connected' if has_path else 'not connected'}" + ) + + return has_path + + except nx.NodeNotFound as e: + # This shouldn't happen since we check above, but handle it anyway + if logger: + logger.error(f"Node not found during connectivity check: {e}") + return False + + except Exception as e: + # Catch any unexpected errors + if logger: + logger.error(f"Unexpected error checking connectivity: {e}") + return False + + +def get_neighbors(graph: Optional[nx.DiGraph], node: str, logger=None) -> List[str]: + """ + Get all neighboring nodes connected by outgoing edges from a given node. + + This function uses NetworkX's neighbors() method to retrieve all nodes + that are directly reachable from the specified node via outgoing edges. + For directed graphs, this returns only successors (nodes pointed to by + outgoing edges), not predecessors. + + Args: + graph: NetworkX DiGraph representing the topological map. + Can be None, in which case an empty list is returned. + node: Node name to get neighbors for. + Must exist in the graph. + logger: Optional ROS 2 logger for warning/error messages. + + Returns: + List[str]: List of neighbor node names (successors) reachable via + outgoing edges from the specified node. Returns empty list if: + - graph is None or empty + - node doesn't exist in graph + - node has no outgoing edges + + Performance: + - Time: O(k) where k is the number of neighbors (out-degree of node) + - Space: O(k) for storing neighbor list + - Efficient for sparse graphs (typical in topological maps) + + Example: + >>> import networkx as nx + >>> G = nx.DiGraph() + >>> G.add_edge('A', 'B') + >>> G.add_edge('A', 'C') + >>> G.add_edge('B', 'C') + >>> + >>> # Get neighbors of A + >>> neighbors = get_neighbors(G, 'A') + >>> print(sorted(neighbors)) # ['B', 'C'] + >>> + >>> # Get neighbors of C (no outgoing edges) + >>> neighbors = get_neighbors(G, 'C') + >>> print(neighbors) # [] + >>> + >>> # Directed graph: B's neighbors don't include A + >>> neighbors = get_neighbors(G, 'B') + >>> print(neighbors) # ['C'] + + Edge Cases: + - graph is None: Returns [] + - Empty graph: Returns [] + - node not in graph: Returns [] (logs warning if logger provided) + - node has no outgoing edges: Returns [] + - Self-loops: If node has edge to itself, includes itself in neighbors + + Use Cases: + - Finding directly reachable nodes for navigation planning + - Exploring local graph structure around current position + - Validating edge connectivity during map updates + - Building adjacency lists for custom graph algorithms + + Raises: + No exceptions are raised. All errors are handled gracefully and return empty list. + + Requirements: + Validates: Requirements 3.5, 15.2, 15.3 + - 3.5: Use networkx.neighbors() for finding adjacent nodes + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate graph + if graph is None: + if logger: + logger.debug("Cannot get neighbors: graph is None") + return [] + + if graph.number_of_nodes() == 0: + if logger: + logger.debug("Cannot get neighbors: graph is empty") + return [] + + # Validate node exists + if node not in graph.nodes: + if logger: + logger.warning(f"Cannot get neighbors: node '{node}' not in graph") + return [] + + try: + # Use NetworkX neighbors() to get all successor nodes + # For DiGraph, this returns nodes reachable via outgoing edges + neighbors = list(graph.neighbors(node)) + + if logger: + logger.debug( + f"Node '{node}' has {len(neighbors)} neighbor(s): " + f"{', '.join(neighbors) if neighbors else 'none'}" + ) + + return neighbors + + except nx.NetworkXError as e: + # Handle any NetworkX-specific errors + if logger: + logger.error(f"NetworkX error getting neighbors for node '{node}': {e}") + return [] + + except Exception as e: + # Catch any unexpected errors + if logger: + logger.error(f"Unexpected error getting neighbors for node '{node}': {e}") + return [] + + +def point_in_poly_nx(graph: Optional[nx.DiGraph], node_name: str, pose) -> bool: + """ + Check if a pose is inside a node's influence zone polygon using ray-casting algorithm. + + This function implements the ray-casting algorithm for point-in-polygon testing. + It checks if a robot pose is within a node's influence zone by casting a horizontal + ray from the point and counting intersections with polygon edges. An odd number of + intersections means the point is inside the polygon. + + The pose coordinates are transformed to node-relative coordinates before testing, + allowing influence zones to be defined relative to the node's position. + + Args: + graph: NetworkX DiGraph with node attributes including 'x', 'y', and 'verts'. + Can be None, in which case False is returned. + node_name: Name of the node to check. + Must exist in the graph. + pose: geometry_msgs.msg.Pose object with position attributes. + Only position.x and position.y are used for 2D polygon check. + + Returns: + bool: True if pose is inside the node's influence zone polygon, False otherwise. + Returns False if: + - graph is None or empty + - node doesn't exist in graph + - node missing 'x' or 'y' attributes + - node has empty 'verts' list (no influence zone defined) + + Algorithm: + Ray-casting algorithm: + 1. Transform pose to node-relative coordinates + 2. Cast horizontal ray from point to infinity + 3. Count intersections with polygon edges + 4. Odd count = inside, even count = outside + + Time Complexity: O(n) where n is number of polygon vertices + Space Complexity: O(1) + + Example: + >>> import networkx as nx + >>> from geometry_msgs.msg import Pose + >>> + >>> # Create graph with square influence zone + >>> G = nx.DiGraph() + >>> G.add_node('WP1', x=0.0, y=0.0, z=0.0, verts=[ + ... {'x': -1.0, 'y': -1.0}, + ... {'x': 1.0, 'y': -1.0}, + ... {'x': 1.0, 'y': 1.0}, + ... {'x': -1.0, 'y': 1.0} + ... ]) + >>> + >>> # Test point inside square + >>> pose = Pose() + >>> pose.position.x = 0.5 + >>> pose.position.y = 0.5 + >>> print(point_in_poly_nx(G, 'WP1', pose)) # True + >>> + >>> # Test point outside square + >>> pose.position.x = 2.0 + >>> pose.position.y = 2.0 + >>> print(point_in_poly_nx(G, 'WP1', pose)) # False + + Edge Cases: + - graph is None: Returns False + - Empty graph: Returns False + - node not in graph: Returns False + - Empty verts list: Returns False (no polygon to be inside) + - Point on polygon boundary: May return True or False (implementation dependent) + - Degenerate polygon (< 3 vertices): Returns False + - Self-intersecting polygon: Behavior follows ray-casting algorithm + + Requirements: + Validates: Requirements 7.1, 7.2, 7.3, 7.4, 7.5, 15.2, 15.3 + - 7.1: Use ray-casting algorithm for point-in-polygon detection + - 7.2: Transform pose coordinates relative to node position + - 7.3: Check all n edges for ray intersections + - 7.4: Handle edge cases including points on boundaries + - 7.5: Return true when inside, false otherwise + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate graph + if graph is None: + return False + + if graph.number_of_nodes() == 0: + return False + + # Check if node exists + if node_name not in graph.nodes: + return False + + node_attrs = graph.nodes[node_name] + + # Check required attributes + if 'x' not in node_attrs or 'y' not in node_attrs: + return False + + # Get influence zone vertices + verts = node_attrs.get('verts', []) + + # Empty polygon - point cannot be inside + if not verts or len(verts) == 0: + return False + + # Transform pose to node-relative coordinates + x = pose.position.x - node_attrs['x'] + y = pose.position.y - node_attrs['y'] + + # Ray-casting algorithm + n = len(verts) + inside = False + + p1x = verts[0]['x'] + p1y = verts[0]['y'] + + for i in range(n + 1): + p2x = verts[i % n]['x'] + p2y = verts[i % n]['y'] + + # Check if horizontal ray from point intersects this edge + if y > min(p1y, p2y): + if y <= max(p1y, p2y): + if x <= max(p1x, p2x): + if p1y != p2y: + xints = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x + if p1x == p2x or x <= xints: + inside = not inside + + p1x, p1y = p2x, p2y + + return inside + + +# ============================================================================== +# Point-to-line-segment distance (vectorized) +# ============================================================================== + +def _pnt2line( + pnt: np.ndarray, + start: np.ndarray, + end: np.ndarray, +) -> np.ndarray: + """Vectorized perpendicular distance from points to line segments. + + All three arguments are ``(N, 3)`` arrays. Returns an ``(N,)`` + array of distances from each point to the corresponding segment. + + The projection parameter *t* is clamped to ``[0, 1]`` so the + result is the distance to the *segment*, not the infinite line. + + Args: + pnt: Array of query points, shape ``(N, 3)``. + start: Segment start points, shape ``(N, 3)``. + end: Segment end points, shape ``(N, 3)``. + + Returns: + 1-D array of Euclidean distances, shape ``(N,)``. + """ + line_vec = end - start + pnt_vec = pnt - start + + line_len = np.linalg.norm(line_vec, axis=1, keepdims=True) + line_unitvec = line_vec / line_len + pnt_vec_scaled = pnt_vec / line_len + + # Projection parameter clamped to [0, 1] + t = np.sum(line_unitvec * pnt_vec_scaled, axis=1) + t = np.clip(t, 0.0, 1.0) + + nearest = line_vec * t[:, np.newaxis] + diff = nearest - pnt_vec + return np.linalg.norm(diff, axis=1) + + +def get_edge_distances_nx(graph: Optional[nx.DiGraph], pose, logger=None) -> Tuple[List[str], np.ndarray]: + """ + Calculate perpendicular distances from pose to all edges using vectorized operations. + + This function computes the perpendicular distance from a robot pose to each edge + in the topological graph. It uses vectorized numpy operations for efficiency and + the existing pnt2line function for distance calculations. Results are sorted by + distance in ascending order. + + Self-loop edges (where source == destination) are skipped with error logging, + as they don't represent valid navigable paths. + + Args: + graph: NetworkX DiGraph with node position attributes ('x', 'y'). + Can be None, in which case empty results are returned. + pose: geometry_msgs.msg.Pose object with position attributes. + Only position.x and position.y are used (z is set to 0). + logger: Optional ROS 2 logger for error/warning messages. + + Returns: + Tuple of (edge_ids, distances) where: + - edge_ids: List of edge IDs sorted by distance (ascending) + - distances: numpy array of distances corresponding to edge_ids + Returns ([], np.array([])) if: + - graph is None or has no edges + - all edges are invalid (self-loops or missing data) + - distance calculation fails + + Performance: + - Time: O(m) where m is number of edges (vectorized operations) + - Space: O(m) for storing edge vectors and distances + - Uses numpy vectorization for batch distance calculations + + Example: + >>> import networkx as nx + >>> import numpy as np + >>> from geometry_msgs.msg import Pose + >>> + >>> # Create graph with edges + >>> G = nx.DiGraph() + >>> G.add_node('A', x=0.0, y=0.0, z=0.0) + >>> G.add_node('B', x=5.0, y=0.0, z=0.0) + >>> G.add_node('C', x=5.0, y=5.0, z=0.0) + >>> G.add_edge('A', 'B', edge_id='AB') + >>> G.add_edge('B', 'C', edge_id='BC') + >>> + >>> # Calculate distances from pose + >>> pose = Pose() + >>> pose.position.x = 2.5 + >>> pose.position.y = 1.0 + >>> edge_ids, distances = get_edge_distances_nx(G, pose) + >>> print(edge_ids) # ['AB', 'BC'] (sorted by distance) + >>> print(distances) # [1.0, ...] (perpendicular distances) + + Edge Cases: + - graph is None: Returns ([], np.array([])) + - Empty graph: Returns ([], np.array([])) + - No edges: Returns ([], np.array([])) + - Self-loop edges: Skipped with error log + - Missing position data: Edge skipped + - Equal distances: Sorted by edge_id alphabetically (stable sort) + + Error Handling: + - Self-loop edges (u == v): Logged as error, skipped + - Missing 'x' or 'y' attributes: Edge skipped silently + - pnt2line exception: Returns empty results with warning + + Requirements: + Validates: Requirements 4.6, 4.7, 11.1, 11.2, 11.4, 15.2, 15.3 + - 4.6: Use vectorized numpy operations for edge distances + - 4.7: Compute perpendicular distance from pose to edge line segments + - 11.1: Calculate perpendicular distance from robot pose to each edge + - 11.2: Use vectorized numpy operations for efficiency + - 11.4: Log error and skip self-loop edges + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + + # Validate graph + if graph is None: + return [], np.array([]) + + if graph.number_of_edges() == 0: + return [], np.array([]) + + edge_ids = [] + vectors_start = [] + vectors_end = [] + + # Build edge vectors from graph + for u, v, edge_data in graph.edges(data=True): + # Skip self-loop edges + if u == v: + if logger: + logger.error( + f"Cannot get distance to edge {edge_data.get('edge_id', 'unknown')}: " + f"Destination is equal to origin" + ) + continue + + u_attrs = graph.nodes[u] + v_attrs = graph.nodes[v] + + # Skip edges with missing position data + if 'x' not in u_attrs or 'y' not in u_attrs: + continue + if 'x' not in v_attrs or 'y' not in v_attrs: + continue + + edge_ids.append(edge_data.get('edge_id', f"{u}_{v}")) + vectors_start.append([u_attrs['x'], u_attrs['y'], 0]) + vectors_end.append([v_attrs['x'], v_attrs['y'], 0]) + + if len(edge_ids) == 0: + return [], np.array([]) + + # Convert to numpy arrays for vectorized operations + vectors_start = np.array(vectors_start) + vectors_end = np.array(vectors_end) + + # Create array of pose points (one for each edge) + pose_points = np.array(len(edge_ids) * [[pose.position.x, pose.position.y, 0]]) + + # Calculate perpendicular distances using vectorized operation + try: + distances = _pnt2line(pose_points, vectors_start, vectors_end) + except Exception as e: + if logger: + logger.warning(f"Cannot calculate distance to edges: {e}") + return [], np.array([]) + + # Sort by distance + sorted_indices = np.argsort(distances) + sorted_edge_ids = [edge_ids[i] for i in sorted_indices] + sorted_distances = distances[sorted_indices] + + return sorted_edge_ids, sorted_distances + + + +def update_loc_by_topic_nx(graph: Optional[nx.DiGraph], logger=None) -> Tuple[List[Dict[str, Any]], List[str]]: + """ + Extract and parse localise_by_topic configuration from NetworkX graph. + + This function scans all nodes in the graph for localise_by_topic configuration + strings, parses them as JSON, and returns structured configuration data. This + enables topic-based localization where nodes can be activated by ROS 2 topic + messages instead of geometric position checks. + + The function sets default values for missing configuration fields and handles + JSON parsing errors gracefully by logging warnings and skipping invalid nodes. + + Args: + graph: NetworkX DiGraph with node attributes including 'localise_by_topic'. + Can be None, in which case empty results are returned. + logger: Optional ROS 2 logger for warning/error messages. + + Returns: + Tuple of (nodes_by_topic, names_by_topic) where: + - nodes_by_topic: List of configuration dictionaries with structure: + { + 'name': str, # Node name (added by this function) + 'topic': str, # ROS 2 topic to subscribe to + 'msg_type': str, # Message type + 'localise_anywhere': bool, # True = no influence zone check (default: True) + 'persistency': int, # How long to persist (default: 10) + ... (other user-defined fields) + } + - names_by_topic: List of node names that have topic-based localization + Returns ([], []) if graph is None or has no nodes with valid config. + + JSON Configuration Format: + Node attribute 'localise_by_topic' should contain JSON string: + { + "topic": "/my_topic", + "msg_type": "std_msgs/String", + "localise_anywhere": true, + "persistency": 10 + } + + Default Values: + - localise_anywhere: true (if not specified) + - persistency: 10 (if not specified) + + Example: + >>> import networkx as nx + >>> import json + >>> + >>> # Create graph with topic-based localization + >>> G = nx.DiGraph() + >>> config = { + ... "topic": "/docking_station", + ... "msg_type": "std_msgs/Bool", + ... "localise_anywhere": False, + ... "persistency": 5 + ... } + >>> G.add_node('Dock', x=0.0, y=0.0, localise_by_topic=json.dumps(config)) + >>> G.add_node('WP1', x=5.0, y=0.0, localise_by_topic='') + >>> + >>> nodes, names = update_loc_by_topic_nx(G) + >>> print(names) # ['Dock'] + >>> print(nodes[0]['name']) # 'Dock' + >>> print(nodes[0]['localise_anywhere']) # False + + Edge Cases: + - graph is None: Returns ([], []) + - Empty graph: Returns ([], []) + - No nodes with localise_by_topic: Returns ([], []) + - Empty localise_by_topic string: Node skipped + - Invalid JSON: Node skipped with warning + - Non-dict JSON: Node skipped with warning + - Missing optional fields: Default values used + + Error Handling: + - JSON parsing errors: Logged as warning, node skipped + - Non-dictionary JSON: Logged as warning, node skipped + - Missing required fields: Node included with defaults + + Requirements: + Validates: Requirements 5.2, 5.3, 5.4, 17.5, 15.2, 15.3 + - 5.2: Prioritize topic-based localization over geometric + - 5.3: Handle localise_anywhere flag + - 5.4: Verify influence zone when localise_anywhere is false + - 17.5: Handle JSON parsing errors gracefully + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate graph + if graph is None: + return [], [] + + if graph.number_of_nodes() == 0: + return [], [] + + nodes_by_topic = [] + names_by_topic = [] + + for node_name in graph.nodes(): + node_attrs = graph.nodes[node_name] + loc_by_topic_str = node_attrs.get('localise_by_topic', '') + + # Skip nodes without localise_by_topic configuration + if not loc_by_topic_str: + continue + + try: + # Parse JSON configuration + config = json.loads(loc_by_topic_str) + + # Validate that config is a dictionary + if not isinstance(config, dict): + if logger: + logger.warning( + f"Invalid localise_by_topic for node {node_name}: " + f"Expected JSON object, got {type(config).__name__}" + ) + continue + + # Add node name to configuration + config['name'] = node_name + + # Set default values for optional fields + if 'localise_anywhere' not in config: + config['localise_anywhere'] = True + if 'persistency' not in config: + config['persistency'] = 10 + + nodes_by_topic.append(config) + names_by_topic.append(node_name) + + except json.JSONDecodeError as e: + if logger: + logger.warning( + f"Invalid JSON in localise_by_topic for node {node_name}: {e}" + ) + continue + + return nodes_by_topic, names_by_topic + + + +def determine_current_node(graph: Optional[nx.DiGraph], kdtree: Optional[KDTree], + node_names: List[str], pose, loc_by_topic: List[Dict[str, Any]], + nogos: List[str]) -> str: + """ + Determine current node based on influence zones and localization rules. + + This function implements the core localization logic to determine which topological + node the robot is currently within. It follows a priority-based approach: + + Priority 1: Topic-based localization (highest priority) + Priority 2: Geometric localization using KD-tree and influence zones + + For geometric localization, the function uses KD-tree to efficiently find the 3 + closest nodes, then checks their influence zones using point-in-polygon tests. + This avoids checking all nodes in the map, providing O(log n) performance. + + Args: + graph: NetworkX DiGraph with node attributes including position and influence zones. + Can be None, in which case 'none' is returned. + kdtree: scipy.spatial.KDTree for efficient spatial queries. + Can be None, in which case 'none' is returned. + node_names: List of node names corresponding to kdtree points. + Must be in same order as kdtree points. + pose: geometry_msgs.msg.Pose object with position attributes. + loc_by_topic: List of active topic-based localizations from update_loc_by_topic_nx(). + Each dict should have 'name' and 'localise_anywhere' fields. + nogos: List of no-go node names to exclude from geometric localization. + No-go nodes can only be current if robot is within their influence zone. + + Returns: + str: Current node name, or 'none' if robot is not within any influence zone. + + Algorithm: + 1. Check topic-based localization first (priority) + - If localise_anywhere=true: Return node immediately + - If localise_anywhere=false: Check influence zone + 2. Use KD-tree to find 3 closest nodes (O(log n)) + 3. For each of the 3 closest nodes: + - Skip if it's a topic-based node + - Skip if it's a no-go node + - Check if pose is within influence zone + - Return first match + 4. Return 'none' if no match found + + Performance: + - Time: O(log n + k*m) where k=3 closest nodes, m=avg polygon vertices + - Space: O(1) + - KD-tree query: O(log n) + - Point-in-polygon: O(m) per node + + Example: + >>> import networkx as nx + >>> from scipy.spatial import KDTree + >>> import numpy as np + >>> from geometry_msgs.msg import Pose + >>> + >>> # Create graph with influence zones + >>> G = nx.DiGraph() + >>> G.add_node('WP1', x=0.0, y=0.0, z=0.0, verts=[ + ... {'x': -1.0, 'y': -1.0}, {'x': 1.0, 'y': -1.0}, + ... {'x': 1.0, 'y': 1.0}, {'x': -1.0, 'y': 1.0} + ... ]) + >>> G.add_node('WP2', x=5.0, y=0.0, z=0.0, verts=[]) + >>> + >>> # Build KD-tree + >>> points = np.array([[0.0, 0.0], [5.0, 0.0]]) + >>> kdtree = KDTree(points) + >>> node_names = ['WP1', 'WP2'] + >>> + >>> # Test pose inside WP1 + >>> pose = Pose() + >>> pose.position.x = 0.5 + >>> pose.position.y = 0.5 + >>> current = determine_current_node(G, kdtree, node_names, pose, [], []) + >>> print(current) # 'WP1' + >>> + >>> # Test pose outside all zones + >>> pose.position.x = 10.0 + >>> pose.position.y = 10.0 + >>> current = determine_current_node(G, kdtree, node_names, pose, [], []) + >>> print(current) # 'none' + + Edge Cases: + - graph is None: Returns 'none' + - kdtree is None: Returns 'none' + - Empty node_names: Returns 'none' + - No influence zones defined: Returns 'none' + - Multiple nodes overlap: Returns first match from closest 3 + - Topic-based node with localise_anywhere=true: Returns immediately + - No-go nodes: Excluded from geometric search + + Requirements: + Validates: Requirements 5.1, 5.2, 5.3, 5.4, 5.5, 5.6, 5.7, 15.2, 15.3 + - 5.1: Identify node when robot pose is within influence zone + - 5.2: Prioritize topic-based localization over geometric + - 5.3: Handle localise_anywhere=true without influence zone check + - 5.4: Verify influence zone when localise_anywhere=false + - 5.5: Return 'none' when not within any influence zone + - 5.6: Skip no-go and topic-based nodes in geometric search + - 5.7: Use KD-tree to find 3 closest nodes, check only those + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate inputs + if graph is None or kdtree is None or not node_names: + return 'none' + + # Priority 1: Check localise by topic first + for topic_loc in loc_by_topic: + node_name = topic_loc.get('name') + + if not node_name or node_name not in graph.nodes: + continue + + if topic_loc.get('localise_anywhere', True): + # Localise anywhere - no influence zone check needed + return node_name + else: + # Check influence zone + if point_in_poly_nx(graph, node_name, pose): + return node_name + + # Priority 2: Check geometric localization using KD-tree + # Get 3 closest nodes efficiently using KD-tree + nearest_nodes = query_nearest_nodes(kdtree, node_names, pose, k=3) + + # Extract list of topic-based node names for filtering + names_by_topic = [x.get('name') for x in loc_by_topic if 'name' in x] + + # Check up to 3 closest nodes + for node_info in nearest_nodes: + node_name = node_info['node'] + + # Skip nodes that are localise by topic + if node_name in names_by_topic: + continue + + # Skip no-go nodes + if node_name in nogos: + continue + + # Check if pose is within influence zone + if point_in_poly_nx(graph, node_name, pose): + return node_name + + # No match found + return 'none' + + + +def determine_closest_node(kdtree: Optional[KDTree], node_names: List[str], + graph: Optional[nx.DiGraph], current_node: str, + nogos: List[str], names_by_topic: List[str], pose) -> Tuple[str, float]: + """ + Determine closest node by Euclidean distance with filtering rules. + + This function finds the topologically closest node to the robot's current position + using KD-tree for efficient O(log n) spatial search. It applies filtering rules + to exclude no-go nodes and topic-based nodes unless the robot is within their + influence zones. + + The function follows these rules: + 1. If robot has a current node, that node is also the closest + 2. Otherwise, find the closest node that isn't no-go or topic-based + 3. No-go and topic-based nodes can only be closest if robot is within their zone + + Args: + kdtree: scipy.spatial.KDTree for efficient spatial queries. + Can be None, in which case ('none', inf) is returned. + node_names: List of node names corresponding to kdtree points. + Must be in same order as kdtree points. + graph: NetworkX DiGraph with node attributes (used for distance calculation). + Can be None, in which case ('none', inf) is returned. + current_node: Current node name from determine_current_node(), or 'none'. + If not 'none', this is returned as the closest node. + nogos: List of no-go node names to exclude from closest node selection. + No-go nodes can only be closest if they are the current node. + names_by_topic: List of topic-based node names to exclude from selection. + Topic-based nodes can only be closest if they are current. + pose: geometry_msgs.msg.Pose object with position attributes. + + Returns: + Tuple of (closest_node_name, distance) where: + - closest_node_name: Name of closest node, or 'none' if no valid node found + - distance: Euclidean distance to closest node, or inf if no valid node + + Special cases: + - If current_node != 'none': Returns (current_node, distance_to_current) + - If all nodes filtered: Returns (first_node, distance) as fallback + - If kdtree is None: Returns ('none', inf) + + Performance: + - Time: O(log n) for KD-tree query, O(k) for filtering where k is query size + - Space: O(k) for storing k nearest neighbors + - Efficient even with many no-go/topic-based nodes + + Example: + >>> import networkx as nx + >>> from scipy.spatial import KDTree + >>> import numpy as np + >>> from geometry_msgs.msg import Pose + >>> + >>> # Create graph and KD-tree + >>> G = nx.DiGraph() + >>> G.add_node('WP1', x=0.0, y=0.0, z=0.0) + >>> G.add_node('WP2', x=5.0, y=0.0, z=0.0) + >>> G.add_node('NoGo', x=2.5, y=0.0, z=0.0) + >>> points = np.array([[0.0, 0.0], [5.0, 0.0], [2.5, 0.0]]) + >>> kdtree = KDTree(points) + >>> node_names = ['WP1', 'WP2', 'NoGo'] + >>> + >>> # Test closest node with no-go filtering + >>> pose = Pose() + >>> pose.position.x = 2.5 + >>> pose.position.y = 0.0 + >>> closest, dist = determine_closest_node( + ... kdtree, node_names, G, 'none', ['NoGo'], [], pose + ... ) + >>> print(closest) # 'WP1' or 'WP2' (NoGo is filtered out) + >>> + >>> # Test with current node + >>> closest, dist = determine_closest_node( + ... kdtree, node_names, G, 'WP1', ['NoGo'], [], pose + ... ) + >>> print(closest) # 'WP1' (current node is always closest) + + Edge Cases: + - kdtree is None: Returns ('none', inf) + - Empty node_names: Returns ('none', inf) + - current_node != 'none': Returns (current_node, distance) + - All nodes filtered: Returns first node as fallback + - Multiple nodes at same distance: Order determined by KD-tree + + Filtering Rules: + - No-go nodes: Excluded unless they are the current node + - Topic-based nodes: Excluded unless they are the current node + - Current node: Always returned as closest (Rule 1) + - Fallback: If all filtered, return first node from KD-tree + + Requirements: + Validates: Requirements 6.1, 6.2, 6.3, 6.4, 6.5, 6.6, 15.2, 15.3 + - 6.1: Use KD-tree nearest neighbor search for closest node + - 6.2: Exclude no-go nodes unless within influence zone + - 6.3: Exclude topic-based nodes unless within influence zone + - 6.4: Set both current and closest to same node when within zone + - 6.5: Publish Euclidean distance to closest node + - 6.6: Handle filtering efficiently with KD-tree + - 15.2: Comprehensive docstrings with parameter descriptions + - 15.3: Type hints for parameters and return values + """ + # Validate inputs + if kdtree is None or not node_names: + return 'none', float('inf') + + # Rule 1: If we have a current node, it's also the closest + if current_node != 'none': + # Get distance to current node using KD-tree + nearest = query_nearest_nodes(kdtree, node_names, pose, k=1) + if nearest and nearest[0]['node'] == current_node: + return current_node, nearest[0]['dist'] + # If current node is not in kdtree, calculate distance manually + # (This shouldn't happen in normal operation) + if graph and current_node in graph.nodes: + node_attrs = graph.nodes[current_node] + if 'x' in node_attrs and 'y' in node_attrs: + dx = pose.position.x - node_attrs['x'] + dy = pose.position.y - node_attrs['y'] + dist = np.sqrt(dx*dx + dy*dy) + return current_node, float(dist) + return current_node, 0.0 + + # Rule 2: Find closest node that isn't no-go or topic-based + # Query more nodes than needed to account for filtering + k = min(len(node_names), len(nogos) + len(names_by_topic) + 10) + nearest_nodes = query_nearest_nodes(kdtree, node_names, pose, k=k) + + for node_info in nearest_nodes: + node_name = node_info['node'] + + # Skip no-go nodes + if node_name in nogos: + continue + + # Skip topic-based nodes + if node_name in names_by_topic: + continue + + # Found a valid closest node + return node_name, node_info['dist'] + + # Fallback: return first node if all are filtered + if nearest_nodes: + return nearest_nodes[0]['node'], nearest_nodes[0]['dist'] + + return 'none', float('inf') + +def draw_topological_graph( + graph: Optional[nx.DiGraph], + highlight_nodes: Optional[List[str]] = None, + highlight_edges: Optional[List[Tuple[str, str]]] = None, + title: str = "Topological Map", + save_path: Optional[str] = None, + figsize: Tuple[float, float] = (14, 10), + logger=None, +) -> None: + """Draw the topological map graph for visual debugging. + + Uses ``matplotlib`` and ``nx.draw_networkx_*`` to render the graph with + real-world (x, y) positions stored on the nodes. Optionally highlights + a subset of nodes/edges (e.g. a planned route) and can save the figure + to disk instead of showing it interactively. + + Args: + graph: NetworkX DiGraph built by :func:`build_graph_from_tmap`. + If *None* or empty the call is a no-op. + highlight_nodes: Node names to draw in a distinct colour (default + colour: orange). Useful for marking the current node, goal, + or route waypoints. + highlight_edges: Edge tuples ``(source, target)`` to draw in a + distinct colour (default: red). Useful for showing a planned + route. + title: Figure title. + save_path: If given, the figure is saved to this path (e.g. + ``'/tmp/topo_map.png'``) instead of calling ``plt.show()``. + figsize: Matplotlib figure size in inches ``(width, height)``. + logger: Optional ROS 2 logger for error messages. + + Returns: + None. A matplotlib figure is either displayed or saved. + + Example: + >>> graph = build_graph_from_tmap(tmap_data) + >>> # Show full map + >>> draw_topological_graph(graph) + >>> # Highlight a route + >>> route_nodes = ['WP1', 'WP2', 'WP3'] + >>> route_edges = [('WP1', 'WP2'), ('WP2', 'WP3')] + >>> draw_topological_graph( + ... graph, + ... highlight_nodes=route_nodes, + ... highlight_edges=route_edges, + ... title='Planned route', + ... save_path='/tmp/route.png', + ... ) + """ + try: + import matplotlib + matplotlib.use("Agg") # non-interactive backend safe for headless + import matplotlib.pyplot as plt + except ImportError: + msg = ( + "matplotlib is required for draw_topological_graph. " + "Install it with: pip install matplotlib" + ) + if logger: + logger.error(msg) + else: + print(msg) + return + + if graph is None or graph.number_of_nodes() == 0: + if logger: + logger.warning("draw_topological_graph: graph is None or empty") + return + + highlight_nodes = set(highlight_nodes or []) + highlight_edges = set(highlight_edges or []) + + # Build position dict from node attributes (x, y) + pos: Dict[str, Tuple[float, float]] = {} + for node_name in graph.nodes(): + attrs = graph.nodes[node_name] + pos[node_name] = (attrs.get("x", 0.0), attrs.get("y", 0.0)) + + fig, ax = plt.subplots(figsize=figsize) + ax.set_title(title, fontsize=14) + ax.set_xlabel("x (m)") + ax.set_ylabel("y (m)") + ax.set_aspect("equal") + ax.grid(True, linestyle="--", alpha=0.4) + + # --- edges ----------------------------------------------------------- + normal_edges = [e for e in graph.edges() if e not in highlight_edges] + nx.draw_networkx_edges( + graph, pos, edgelist=normal_edges, ax=ax, + edge_color="grey", alpha=0.6, arrows=True, + arrowstyle="-|>", arrowsize=12, connectionstyle="arc3,rad=0.08", + ) + if highlight_edges: + present = [e for e in highlight_edges if graph.has_edge(*e)] + if present: + nx.draw_networkx_edges( + graph, pos, edgelist=present, ax=ax, + edge_color="red", width=2.2, arrows=True, + arrowstyle="-|>", arrowsize=14, + connectionstyle="arc3,rad=0.08", + ) + + # --- nodes ----------------------------------------------------------- + normal_nodes = [n for n in graph.nodes() if n not in highlight_nodes] + nx.draw_networkx_nodes( + graph, pos, nodelist=normal_nodes, ax=ax, + node_color="steelblue", node_size=200, alpha=0.85, + ) + if highlight_nodes: + present = [n for n in highlight_nodes if n in graph.nodes()] + if present: + nx.draw_networkx_nodes( + graph, pos, nodelist=present, ax=ax, + node_color="orange", node_size=320, alpha=0.95, + edgecolors="red", linewidths=1.5, + ) + + # --- labels ---------------------------------------------------------- + nx.draw_networkx_labels( + graph, pos, ax=ax, font_size=7, font_weight="bold", + ) + + plt.tight_layout() + if save_path: + fig.savefig(save_path, dpi=150, bbox_inches="tight") + if logger: + logger.info(f"Topological map figure saved to {save_path}") + else: + print(f"Saved to {save_path}") + else: + plt.show() + + plt.close(fig) + + +def main(): + """Load test fixture maps, build graphs, run basic queries, and visualise.""" + import argparse + import os + import sys + import yaml + + # ------------------------------------------------------------------ + # CLI + # ------------------------------------------------------------------ + parser = argparse.ArgumentParser( + description="Load a topological map, build a NetworkX graph, " + "run basic spatial queries, and save a visualisation." + ) + parser.add_argument( + "--map", "-m", + default=None, + help="Path to a .yaml topological map. " + "If omitted, the test fixtures are used.", + ) + parser.add_argument( + "--out-dir", "-o", + default="/tmp", + help="Directory for saved PNG figures (default: /tmp).", + ) + args = parser.parse_args() + + # ------------------------------------------------------------------ + # Resolve map file(s) + # ------------------------------------------------------------------ + this_dir = os.path.dirname(os.path.abspath(__file__)) + fixture_dir = os.path.normpath( + os.path.join(this_dir, "..", "test", "fixtures") + ) + config_dir = os.path.normpath( + os.path.join(this_dir, "..", "config") + ) + + if args.map: + map_files = [args.map] + else: + # Default: all fixture maps + the polytunnel config map + map_files = sorted( + os.path.join(fixture_dir, f) + for f in os.listdir(fixture_dir) + if f.endswith(".yaml") and not f.startswith("README") + ) + polytunnel = os.path.join(config_dir, "strawberry_polytunnel.tmap2.yaml") + if os.path.isfile(polytunnel): + map_files.append(polytunnel) + + if not map_files: + print("No map files found. Pass --map .") + sys.exit(1) + + os.makedirs(args.out_dir, exist_ok=True) + + # ------------------------------------------------------------------ + # Process each map + # ------------------------------------------------------------------ + for map_path in map_files: + map_name = os.path.splitext(os.path.basename(map_path))[0] + print(f"\n{'=' * 60}") + print(f"Map: {map_name} ({map_path})") + print("=" * 60) + + # 1. Load YAML + with open(map_path) as f: + tmap_data = yaml.safe_load(f) + + if tmap_data is None: + print(" [SKIP] empty YAML file") + continue + + # 2. Build NetworkX graph + graph = build_graph_from_tmap(tmap_data) + if graph is None: + print(" [SKIP] build_graph_from_tmap returned None") + continue + print(f" Nodes : {graph.number_of_nodes()}") + print(f" Edges : {graph.number_of_edges()}") + + # 3. Build KD-tree + kdtree, node_names = build_kdtree_from_graph(graph) + if kdtree is None: + print(" [SKIP] KD-tree build failed") + continue + print(f" KD-tree built with {len(node_names)} points") + + # 4. Pick a sample pose (first node's position) + sample_node = node_names[0] + sample_attrs = graph.nodes[sample_node] + + class _FakePose: + """Minimal pose-like object for testing.""" + class position: + x = sample_attrs.get("x", 0.0) + y = sample_attrs.get("y", 0.0) + z = sample_attrs.get("z", 0.0) + + pose = _FakePose() + print(f" Sample pose at node '{sample_node}': " + f"({pose.position.x:.2f}, {pose.position.y:.2f})") + + # 5. Query nearest nodes + nearest = query_nearest_nodes(kdtree, node_names, pose, k=3) + print(f" 3-nearest nodes:") + for entry in nearest: + print(f" {entry['node']:20s} dist={entry['dist']:.3f}") + + # 6. Point-in-polygon for sample node + inside = point_in_poly_nx(graph, sample_node, pose) + print(f" Pose inside '{sample_node}' influence zone: {inside}") + + # 7. Shortest path (first node -> last node) + if len(node_names) >= 2: + src, dst = node_names[0], node_names[-1] + path = compute_shortest_path(graph, src, dst) + if path: + length = compute_path_length(graph, src, dst) + print(f" Shortest path {src} -> {dst}: " + f"{' -> '.join(path)} (length={length:.3f})") + else: + print(f" No path from {src} to {dst}") + + # 8. Connectivity check + if len(node_names) >= 2: + connected = check_connectivity(graph, node_names[0], node_names[-1]) + print(f" Connected {node_names[0]} -> {node_names[-1]}: {connected}") + + # 9. Neighbours of first node + nbrs = get_neighbors(graph, sample_node) + print(f" Neighbors of '{sample_node}': {nbrs}") + + # 10. Visualise and save + png_path = os.path.join(args.out_dir, f"topo_{map_name}.png") + + hl_nodes = [n['node'] for n in nearest] if nearest else [] + hl_edges = [] + if path and len(path) >= 2: + hl_edges = list(zip(path[:-1], path[1:])) + + draw_topological_graph( + graph, + highlight_nodes=hl_nodes, + highlight_edges=hl_edges, + title=f"{map_name} ({graph.number_of_nodes()} nodes, " + f"{graph.number_of_edges()} edges)", + save_path=png_path, + ) + + print(f"\nDone. Figures saved under {args.out_dir}/") + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/topological_navigation/topological_navigation/node_controller.py b/topological_navigation/topological_navigation/node_controller.py deleted file mode 100644 index afe8abc2..00000000 --- a/topological_navigation/topological_navigation/node_controller.py +++ /dev/null @@ -1,117 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy - -import std_msgs.msg -from threading import Timer - -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point - -from visualization_msgs.msg import * -from interactive_markers.interactive_marker_server import * - -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation.topological_map import * - - - -class WaypointControllers(object): - - def __init__(self) : - self.timer = Timer(1.0, self.timer_callback) - #map_name = rospy.get_param('topological_map_name', 'top_map') - #print map_name - self._marker_server = InteractiveMarkerServer("/topological_map_markers") - self.map_update = rospy.Publisher('/update_map', std_msgs.msg.Time, queue_size=10) - - rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback) - - def update_map(self, msg) : - print("updating node controllers...") - self.topo_map = topological_map(msg.name, msg=msg) - self._marker_server.clear() - self._marker_server.applyChanges() - - for i in self.topo_map.nodes : - self._create_marker(i.name, i._get_pose(), i.name) - - """ - MapCallback - - This function receives the Topological Map - """ - def MapCallback(self, msg) : - self.update_map(msg) - - - - def _create_marker(self, marker_name, pose, marker_description="waypoint marker") : - # create an interactive marker for our server - marker = InteractiveMarker() - marker.header.frame_id = "map" - marker.name = marker_name - marker.description = marker_description - - # the marker in the middle - box_marker = Marker() - box_marker.type = Marker.ARROW - box_marker.scale.x = 0.45 - box_marker.scale.y = 0.25 - box_marker.scale.z = 0.15 - box_marker.color.r = 0.0 - box_marker.color.g = 0.5 - box_marker.color.b = 0.5 - box_marker.color.a = 1.0 - - # create an interactive control which contains the arrow and - # allows movement within the x-y plane. - box_control = InteractiveMarkerControl() - box_control.always_visible = True - box_control.orientation.w = 1 - box_control.orientation.x = 0 - box_control.orientation.y = 1 - box_control.orientation.z = 0 - box_control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE - box_control.markers.append( box_marker ) - marker.controls.append( box_control ) - - #rotate z - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 1 - control.orientation.z = 0 - control.name = "rotate_z" - control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS - marker.controls.append(control) - - self._marker_server.insert(marker, self._marker_feedback) - self._marker_server.applyChanges() - - if pose is not None: - pose.position.z=pose.position.z+0.15 - self._marker_server.setPose( marker.name, pose ) - self._marker_server.applyChanges() - - - def _marker_feedback(self, feedback): - self.info = feedback - self.timer.cancel() - del self.timer - self.timer = Timer(1.0, self.timer_callback) - self.timer.start() - - - def timer_callback(self) : - self.topo_map.update_node_waypoint(self.info.marker_name, self.info.pose) - self.map_update.publish(rospy.Time.now()) - - - def clear(): - self._marker_server.clear() - self._marker_server.applyChanges() - - def __del__(self): - del self._marker_server diff --git a/topological_navigation/topological_navigation/node_manager.py b/topological_navigation/topological_navigation/node_manager.py deleted file mode 100644 index 6446dba8..00000000 --- a/topological_navigation/topological_navigation/node_manager.py +++ /dev/null @@ -1,150 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import math -import tf -import actionlib -from threading import Timer - - -import std_msgs.msg -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point - -from visualization_msgs.msg import * -from interactive_markers.interactive_marker_server import * - - -import topological_navigation_msgs.srv - -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation.topological_map import * - - - -class node_manager(object): - - def __init__(self) : - self.in_feedback=False - #map_name = rospy.get_param('topological_map_name', 'top_map') - self._node_server = InteractiveMarkerServer("/topological_map_add_rm_node") - rospy.loginfo(" ... Init done") - self.map_update = rospy.Publisher('/update_map', std_msgs.msg.Time, queue_size=10) - rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback) - - - - def update_map(self, msg) : - print("updating node controllers...") - self.topo_map = topological_map(msg.name, msg=msg) - self._node_server.clear() - self._node_server.applyChanges() - self.create_marker() - - - """ - MapCallback - - This function receives the Topological Map - """ - def MapCallback(self, msg) : - self.update_map(msg) - - - - def makeEmptyMarker(self, dummyBox=True ) : - int_marker = InteractiveMarker() - int_marker.header.frame_id = "/base_link" - int_marker.scale = 1 - return int_marker - - - def makeBox(self, msg ): - marker = Marker() - - marker.type = Marker.CUBE - marker.scale.x = msg.scale * 0.125 - marker.scale.y = msg.scale * 0.25 - marker.scale.z = msg.scale * 0.1 - marker.lifetime.secs = 3 - marker.color.r = 0.1 - marker.color.g = 0.8 - marker.color.b = 0.1 - marker.color.a = 1.0 - return marker - - - def create_marker(self) : - - marker = self.makeEmptyMarker() - marker.name = "add_node" - marker.description = "add_node" - - control = InteractiveMarkerControl() - - control.interaction_mode = InteractiveMarkerControl.BUTTON - control.always_visible = True - - control.markers.append( self.makeBox( marker ) ) - marker.controls.append(control) - - self._node_server.insert(marker, self.feedback_cb) - self._node_server.applyChanges() - - pos=Pose() - pos.position.x=0.0 - pos.position.y=0.0 - pos.position.z=1.9 - pos.orientation.x=0.0 - pos.orientation.y=0.0 - pos.orientation.z=0.0 - pos.orientation.w=0.0 - - self._node_server.setPose( marker.name, pos ) - self._node_server.applyChanges() - - - - def feedback_cb(self, feedback): - if not self.in_feedback : - self.in_feedback=True - - try: - current = rospy.wait_for_message('current_node', std_msgs.msg.String, timeout=10.0) - except rospy.ROSException : - rospy.logwarn("Failed to get current node") - return - - print(current.data) - if current.data == 'none': - try: - pos = rospy.wait_for_message('robot_pose', Pose, timeout=10.0) - except rospy.ROSException : - rospy.logwarn("Failed to get robot pose") - return - - add_node = rospy.ServiceProxy('/topological_map_manager/add_topological_node', topological_navigation_msgs.srv.AddNode) - add_node('',pos, True) - - map_update = rospy.Publisher('/update_map', std_msgs.msg.Time) - map_update.publish(rospy.Time.now()) - - else: - rospy.loginfo("I will NOT add a node within the influence area of another! Solve this and try again!") - - - self.timer = Timer(1.0, self.timer_callback) - self.timer.start() - - - def timer_callback(self) : - self.in_feedback = False - - def clear(): - self._node_server.clear() - self._node_server.applyChanges() - - - def __del__(self): - del self._node_server diff --git a/topological_navigation/topological_navigation/point2line.py b/topological_navigation/topological_navigation/point2line.py deleted file mode 100644 index b8d9e3d8..00000000 --- a/topological_navigation/topological_navigation/point2line.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python -################################################################################################################### -from __future__ import division -import numpy as np - - -def pnt2line(pnt, start, end): - - pnt = (pnt[:, 0], pnt[:, 1], pnt[:, 2]) - start = (start[:, 0], start[:, 1], start[:, 2]) - end = (end[:, 0], end[:, 1], end[:, 2]) - - line_vec = vector(start, end) - pnt_vec = vector(start, pnt) - line_unitvec = unit(line_vec) - line_len = length(line_vec) - pnt_vec_scaled = scale(pnt_vec, 1.0/line_len) - - t = dot(line_unitvec, pnt_vec_scaled) - t[np.where(t < 0.0)[0]] = 0.0 - t[np.where(t > 1.0)[0]] = 1.0 - - nearest = scale(line_vec, t) - return distance(nearest, pnt_vec) - - -def vector(b, e): - return b[0]-e[0], b[1]-e[1], b[2]-e[2] - - -def unit(v): - mag = length(v) - return v[0]/mag, v[1]/mag, v[2]/mag - - -def length(v): - return np.sqrt(v[0]**2 + v[1]**2 + v[2]**2) - - -def scale(v, sc): - return v[0]*sc, v[1]*sc, v[2]*sc - - -def dot(v, w): - return v[0]*w[0] + v[1]*w[1] + v[2]*w[2] - - -def distance(p0, p1): - return length(vector(p0, p1)) -################################################################################################################### - - -################################################################################################################### -# pnt = [[x0, y0, z0], -# [x1, y1, z1], -# [..., ..., ...], -# [xn, yn, zn]] - -if __name__ == '__main__': - - pnt = np.array([[5, 4, 0], - [5, 4, 0]]) - - start = np.array([[3, 0, 0], - [2, 3, 0]]) - - end = np.array([[8, 5, 0], - [4, 7, 0]]) - - print(pnt2line(pnt, start, end)) -################################################################################################################### \ No newline at end of file diff --git a/topological_navigation/topological_navigation/policies.py b/topological_navigation/topological_navigation/policies.py deleted file mode 100644 index a63e55eb..00000000 --- a/topological_navigation/topological_navigation/policies.py +++ /dev/null @@ -1,183 +0,0 @@ -#!/usr/bin/env python - -import rospy -import math -import tf - - - - -from interactive_markers.interactive_marker_server import * -from visualization_msgs.msg import * - -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation.marker_arrays import * -from topological_navigation_msgs.msg import NavRoute - - -class PoliciesVis(object): -# _killall_timers=False - - def __init__(self) : - self._killall=False - #rospy.on_shutdown(self._on_node_shutdown) - self.route_nodes = NavRoute() - self.map_edges = MarkerArray() - #self.update_needed=False - - rospy.loginfo("Creating Publishers ...") - self.policies_pub = rospy.Publisher('topological_edges_policies', MarkerArray, queue_size=10) - rospy.loginfo("Done ...") - - - rospy.loginfo("Creating subscriber ...") - self.subs = rospy.Subscriber("mdp_plan_exec/current_policy_mode", NavRoute, self.policies_callback) - rospy.loginfo("Done ...") - - #Waiting for Topological Map - self.map_received=False - rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) - rospy.loginfo("Waiting for Topological map ...") - while not self.map_received and not self._killall : - rospy.sleep(rospy.Duration.from_sec(0.05)) - rospy.loginfo(" ...done") - - - rospy.loginfo("All Done ...") - - - def _update_everything(self) : - print("updating ...") - print(self.route_nodes) - - self.map_edges.markers=[] - - counter=0 - total = len(self.route_nodes.source) - - print('updating '+str(total)+' edges') - while counter < total : - print('Creating edge '+str(counter)) - source = self.route_nodes.source[counter] - ori = self.get_node(self.lnodes, source) - targ = self.find_action(ori.name, self.route_nodes.edge_id[counter]) - if targ: - #print ori.name,ori.pose.position - target = self.get_node(self.lnodes, targ) - self.added_sources.append(source) - #print target.name,target.pose.position - if targ in self.added_sources: - self.create_edge(ori.pose.position, target.pose.position, "blue") - else: - self.create_edge(ori.pose.position, target.pose.position, "red") - counter+=1 - - - idn = 0 - for m in self.map_edges.markers: - m.id = idn - idn += 1 - self.policies_pub.publish(self.map_edges) - print("All Done") - - - def create_edge(self, point1, point2, color="red"): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.ARROW - - - pose = Pose() - - pose.position.x = point1.x - pose.position.y = point1.y - pose.position.z = point1.z - angle = math.atan2((point2.y-point1.y),(point2.x-point1.x)) - - qat = tf.transformations.quaternion_from_euler(0, 0, angle) - pose.orientation.w = qat[3] - pose.orientation.x = qat[0] - pose.orientation.y = qat[1] - pose.orientation.z = qat[2] - - r = math.hypot((point2.y-point1.y),(point2.x-point1.x))#/3.0 - marker.scale.x = r - marker.scale.y = 0.15 - marker.scale.z = 0.15 - marker.color.a = 0.95 - marker.color.r = 0.9 - marker.color.g = 0.1 - marker.color.b = 0.1 - if color == "red": - marker.color.r = 0.9 - marker.color.g = 0.1 - marker.color.b = 0.1 - if color == "blue": - marker.color.r = 0.1 - marker.color.g = 0.1 - marker.color.b = 0.9 - marker.pose = pose - self.map_edges.markers.append(marker) - - - - """ - get_node - - Given a topological map and a node name it returns the node object - """ - def get_node(self, top_map, node_name): - print('looking for: '+node_name) - for i in top_map.nodes: - #print i.name - if i.name == node_name: - return i - return None - - - """ - Find Action - - """ -# def find_action(self, source, target): - def find_action(self, source, edge_id): - #print 'Searching for action between: %s -> %s' %(source, target) - found = False - #action = 'none' - target = 'none' - for i in self.lnodes.nodes : - if i.name == source : - for j in i.edges: - if j.edge_id == edge_id: - #action = j.action - target = j.node - found = True - if not found: - rospy.logwarn("source node not found") - return None - return target - - - - def policies_callback(self, msg) : - print("GOT policies") - self.added_sources = [] - self.route_nodes = msg - self._update_everything() - - - """ - MapCallback - - This function receives the Topological Map - """ - def MapCallback(self, msg) : - print("got map") - self.lnodes = msg -# print self.lnodes.nodes - self.map_received = True - - - def on_node_shutdown(self): - self._killall=True - #sleep(2) \ No newline at end of file diff --git a/topological_navigation/topological_navigation/policy_marker.py b/topological_navigation/topological_navigation/policy_marker.py deleted file mode 100644 index b8b82bf0..00000000 --- a/topological_navigation/topological_navigation/policy_marker.py +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/env python3 - -import math - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy - -from tf_transformations import quaternion_from_euler - -from interactive_markers.interactive_marker_server import * -from visualization_msgs.msg import * - -from topological_navigation_msgs.msg import TopologicalMap, NavRoute - - -class PoliciesVis(Node): - - def __init__(self): - super().__init__('topological_policy_markers') - self.map = None - self.policy = MarkerArray() - self.route_nodes = NavRoute() - self.marker_pub = self.create_publisher(MarkerArray, '~/vis', 10) - self.marker_pub.publish(self.policy) - qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.map_sub = self.create_subscription(TopologicalMap, '/topological_map', self.map_callback, qos) - self.policy_sub = self.create_subscription(NavRoute, 'mdp_plan_exec/current_policy_mode', self.policies_callback, 10) - self.get_logger().info('policy visualiser init complete') - - def map_callback(self, msg): - self.get_logger().info('map callback triggered') - self.map = msg - - def policies_callback(self, msg): - self.get_logger().info('policies callback triggered') - self.get_logger().info(str(msg)) - if not self.map: return - - self.added_sources = [] - self.route_nodes = msg - print(self.route_nodes) - - self.map_edges.markers=[] - - counter=0 - total = len(self.route_nodes.source) - - for i in range(0, total): - # Find source and edge - source = self.route_nodes.source[i] - edge_id = self.route_nodes.edge_id[i] - - # Get positions for start and end - ori = tmap_utils.get_node(self.map, source) - targ = tmap_utils.get_edge_from_id(self.map, ori.name, edge_id).node - if not targ: continue - - # Edge - target = self.get_node(self.map, targ) - self.added_sources.append(source) - colour = [0.1,0.1,0.9] if targ in self.added_sources else [0.9,0.1,0.1] - self.map_edges.markers.append(self.create_edge(ori.pose.position, target.pose.position, colour)) - - # Publish - for i, marker in enumerate(self.policy.markers): - marker.id = i - self.policies_pub.publish(self.policy) - - def create_edge(self, point1, point2, color): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.ARROW - - pose = Pose() - - pose.position.x = point1.x - pose.position.y = point1.y - pose.position.z = point1.z - angle = math.atan2((point2.y-point1.y),(point2.x-point1.x)) - - qat = quaternion_from_euler(0, 0, angle) - pose.orientation.w = qat[3] - pose.orientation.x = qat[0] - pose.orientation.y = qat[1] - pose.orientation.z = qat[2] - - r = math.hypot((point2.y-point1.y),(point2.x-point1.x))#/3.0 - marker.scale.x = r - marker.scale.y = 0.15 - marker.scale.z = 0.15 - marker.color.a = 0.95 - marker.color.r = colour[0] - marker.color.g = colour[1] - marker.color.b = colour[2] - marker.pose = pose - self.policy.markers.append(marker) - - -def main(args=None): - rclpy.init(args=args) - - PV = PoliciesVis() - rclpy.spin(PV) - - PV.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/topological_navigation/topological_navigation/policy_marker2.py b/topological_navigation/topological_navigation/policy_marker2.py deleted file mode 100644 index 5e7eaf31..00000000 --- a/topological_navigation/topological_navigation/policy_marker2.py +++ /dev/null @@ -1,134 +0,0 @@ -#!/usr/bin/env python3 - -import math -import yaml -import rclpy, json -from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy -from topological_navigation.route_search2 import RouteChecker, TopologicalRouteSearch2, get_route_distance -from tf_transformations import quaternion_from_euler - -from geometry_msgs.msg import Point, Pose - -from interactive_markers.interactive_marker_server import * -from visualization_msgs.msg import * -from topological_navigation.tmap_utils import get_edge_from_id_tmap2 - -from topological_navigation_msgs.msg import TopologicalMap, NavRoute -from std_msgs.msg import String - -# this ensures that all the poses and translates -# are float-type and not int-type as there is an -# assertion in ros2 messages (vector3, pose etc.) -# for float-type [x,y,z,w] keys. -class CustomSafeLoader(yaml.SafeLoader): - def construct_mapping(self, node, deep=False): - mapping = super().construct_mapping(node, deep=deep) - - # this can be extended to test the validity of the tmap2 - # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w']: - if key in mapping and isinstance(mapping[key], int): - mapping[key] = float(mapping[key]) - - return mapping - - -class PoliciesVis(Node): - - def __init__(self): - super().__init__('topological_policy_markers') - self.topol_map = None - self.policy = MarkerArray() - self.route_nodes = NavRoute() - self.marker_pub = self.create_publisher(MarkerArray, '~/vis', 10) - self.marker_pub.publish(self.policy) - qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.map_sub = self.create_subscription(String, '/topological_map_2', self.map_callback, qos) - self.policy_sub = self.create_subscription(NavRoute, 'mdp_plan_exec/current_policy_mode', self.policies_callback, qos) - self.get_logger().info('policy visualiser init complete') - - def map_callback(self, msg): - self.get_logger().info('map callback triggered') - # self.map = msg - # self.lnodes = json.loads(msg.data) - self.lnodes = yaml.load( msg.data, Loader = CustomSafeLoader) - self.topol_map = self.lnodes["pointset"] - self.rsearch = TopologicalRouteSearch2(self.lnodes) - - def policies_callback(self, msg): - self.get_logger().info('policies callback triggered') - self.get_logger().info(str(msg)) - if not self.topol_map: return - - self.added_sources = [] - self.route_nodes = msg - self.get_logger().info(self.route_nodes) - - self.map_edges.markers=[] - - counter = 0 - total = len(self.route_nodes.source) - - for i in range(0, total): - # Find source and edge - source = self.route_nodes.source[i] - edge_id = self.route_nodes.edge_id[i] - - # Get positions for start and end - ori = self.rsearch.get_node_from_tmap2(source) - targ = get_edge_from_id_tmap2(self.topol_map, ori.name, edge_id).node - if not targ: continue - - # Edge - target = self.rsearch.get_node_from_tmap2(targ["node"]) - self.added_sources.append(source) - colour = [0.1,0.1,0.9] if targ in self.added_sources else [0.9,0.1,0.1] - self.map_edges.markers.append(self.create_edge(ori.pose.position, target.pose.position, colour)) - - # Publish - for i, marker in enumerate(self.policy.markers): - marker.id = i - self.policies_pub.publish(self.policy) - - def create_edge(self, point1, point2, colour): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.ARROW - - pose = Pose() - - pose.position.x = point1.x - pose.position.y = point1.y - pose.position.z = point1.z - angle = math.atan2((point2.y-point1.y),(point2.x-point1.x)) - - qat = quaternion_from_euler(0, 0, angle) - pose.orientation.w = qat[3] - pose.orientation.x = qat[0] - pose.orientation.y = qat[1] - pose.orientation.z = qat[2] - - r = math.hypot((point2.y-point1.y),(point2.x-point1.x))#/3.0 - marker.scale.x = r - marker.scale.y = 0.15 - marker.scale.z = 0.15 - marker.color.a = 0.95 - marker.color.r = colour[0] - marker.color.g = colour[1] - marker.color.b = colour[2] - marker.pose = pose - self.policy.markers.append(marker) - - -def main(args=None): - rclpy.init(args=args) - - PV = PoliciesVis() - rclpy.spin(PV) - - PV.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/topological_navigation/topological_navigation/publisher.py b/topological_navigation/topological_navigation/publisher.py deleted file mode 100644 index 549b0024..00000000 --- a/topological_navigation/topological_navigation/publisher.py +++ /dev/null @@ -1,81 +0,0 @@ -#!/usr/bin/env python -import math -import rospy -import sys - -import std_msgs.msg -from topological_navigation.topological_node import * -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation_msgs.srv import GetTopologicalMap - -from mongodb_store.message_store import MessageStoreProxy - - -class map_publisher(object): - - def __init__(self, name) : - self.name = name - self.nodes = self.loadMap(name) - self.map_pub = rospy.Publisher('/topological_map', TopologicalMap, latch=True, queue_size=1) - self.last_updated = rospy.Time.now() - self.map_pub.publish(self.nodes) - rospy.Subscriber('/update_map', std_msgs.msg.Time, self.updateCallback) - self.get_map_srv=rospy.Service('/topological_map_publisher/get_topological_map', GetTopologicalMap, self.get_topological_map_cb) - - - - def updateCallback(self, msg) : -# if msg.data > self.last_updated : - self.nodes = self.loadMap(self.name) - self.last_updated = rospy.Time.now() - self.map_pub.publish(self.nodes) - - - def get_topological_map_cb(self, req): - nodes = self.loadMap(req.pointset) - print("Returning Map %s"%req.pointset) - return nodes - - - def loadMap(self, point_set) : - msg_store = MessageStoreProxy(collection='topological_maps') - - query_meta = {} - query_meta["pointset"] = point_set - - # waiting for the map to be in the mongodb_store - ntries=1 - map_found=False - - while not map_found : - available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 - #print available - if available <= 0 : - rospy.logerr("Desired pointset '"+point_set+"' not in datacentre, try :"+str(ntries)) - #rospy.logerr("Available pointsets: "+str(available)) - if ntries <=10 : - ntries+=1 - rospy.sleep(rospy.Duration.from_sec(6)) - else : - raise Exception("Can't find waypoints.") - sys.exit(2) - else: - map_found=True - - - query_meta = {} - query_meta["pointset"] = point_set - - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - - points = TopologicalMap() - points.name = point_set - points.map = point_set - points.pointset = point_set - #string last_updated - for i in message_list: - b = i[0] - points.nodes.append(b) - - return points \ No newline at end of file diff --git a/topological_navigation/topological_navigation/restrictions_impl.py b/topological_navigation/topological_navigation/restrictions_impl.py deleted file mode 100644 index 28ff6e78..00000000 --- a/topological_navigation/topological_navigation/restrictions_impl.py +++ /dev/null @@ -1,215 +0,0 @@ -import tf -import rospy -import dynamic_reconfigure.client -import numpy as np - -from abc import ABCMeta, abstractmethod, abstractproperty -from geometry_msgs.msg import Pose, Quaternion -from topological_navigation.point2line import pnt2line -from topological_navigation.tmap_utils import * -from std_msgs.msg import String - - - - - -############################### -## -# Here extend the class AbstractRestriction to create a new restriction. -# Once its defined here it will be automatically imported by the RestrictionsManager. -## -class AbstractRestriction(): - """ Abstract definition of a restriction """ - __metaclass__ = ABCMeta - - robot_state = {} - DEFAULT_EVALUATION = True - - @abstractproperty - def name(self): - """ The name must correspond to the restriction variable name in the map definition """ - raise NotImplementedError() - - @abstractmethod - def evaluate_node(self, node=None, value=None, robot_state={}, tmap=None): - """ Returns the value of the restriction associated with the given entity, must return a boolean value """ - raise NotImplementedError() - - @abstractmethod - def evaluate_edge(self, edge=None, value=None, robot_state={}, tmap=None): - """ Returns the value of the restriction associated with the given entity, must return a boolean value """ - raise NotImplementedError() - - @abstractmethod - def ground_to_robot(self): - """ Retrieves and saves internally the state of the robot necessary for evaluating the restriction """ - raise NotImplementedError() - -class RobotType(AbstractRestriction): - name = "robot" - - def __init__(self): - super(AbstractRestriction, self).__init__() - - # subscribe to task topic - def _save_robot_type(msg): - self.robot_state.update({ - "type": msg.data - }) - - rospy.Subscriber("type", - String, - lambda msg: _save_robot_type(msg) - ) - - def _evaluate(self, value, robot_state): - evaluation = self.DEFAULT_EVALUATION - - # default state - if "type" not in robot_state: - robot_state = self.robot_state - - if "type" in robot_state: - evaluation = value.lower() == robot_state["type"].lower() - - return evaluation - - def evaluate_node(self, node, value, robot_state={}, tmap=None): - """ Returns the value of the restriction associated with the given entity, must return a boolean value """ - return self._evaluate(value, robot_state) - - def evaluate_edge(self, edge, value, robot_state={}, tmap=None): - """ Returns the value of the restriction associated with the given entity, must return a boolean value """ - return self._evaluate(value, robot_state) - - def ground_to_robot(self): - pass - -class TaskName(AbstractRestriction): - name = "task" - - def __init__(self): - super(AbstractRestriction, self).__init__() - - # subscribe to task topic - def _save_robot_task(msg): - self.robot_state.update({ - "task": msg.data - }) - - rospy.Subscriber("task", - String, - lambda msg: _save_robot_task(msg) - ) - - def _evaluate(self, value, robot_state): - evaluation = self.DEFAULT_EVALUATION - - # default state - if "task" not in robot_state: - robot_state = self.robot_state - - if "task" in robot_state: - evaluation = value.lower() == robot_state["task"].lower() - - return evaluation - - def evaluate_node(self, node, value, robot_state={}, tmap=None): - """ Returns the value of the restriction associated with the given entity, must return a boolean value """ - return self._evaluate(value, robot_state) - - def evaluate_edge(self, edge, value, robot_state={}, tmap=None): - """ Returns the value of the restriction associated with the given entity, must return a boolean value """ - return self._evaluate(value, robot_state) - - def ground_to_robot(self): - pass - -class ObstacleFree(AbstractRestriction): - name = "obstacleFree" - - def __init__(self, robots): - super(AbstractRestriction, self).__init__() - # keeps the position of each robot with timestamp - self.robot_nodes = {} - self.active = True - if len(robots) == 0: - rospy.logwarn("No robots provided for obstacleFree restriction.") - self.active = False - - def _save_robot_nodes(msg, robot): - self.robot_nodes[robot] = { - "node": msg.data, - "timestamp_secs": rospy.Time.now().to_sec() - } - for robot in robots: - rospy.Subscriber("/{}/closest_node".format(robot), - String, - lambda msg, arg: _save_robot_nodes(msg, arg), - callback_args=(robot) - ) - - def _get_closest_node(self, robot, secs_thr=np.inf): - node = [] - if robot in self.robot_nodes and \ - rospy.Time.now().to_sec() - self.robot_nodes[robot]["timestamp_secs"] < secs_thr: - node = self.robot_nodes[robot]["node"] - return node - - # value is the min distance [m] from the obstacles that the robot must have for the node to be obstacleFree - def evaluate_node(self, node, value, robot_state={}, tmap=None): - """ Returns the evaluation of the restriction associated with the given entity, must return a boolean value """ - evaluation = self.DEFAULT_EVALUATION - - if tmap is not None and self.active: - _node_pos = get_node_from_tmap2(tmap, node)["node"]["pose"] - node_pos = np.array([_node_pos["position"]["x"], _node_pos["position"]["y"]]) - - all_nodes = [self._get_closest_node(r) for r in self.robot_nodes if r != rospy.get_namespace().strip("/")] - distances = [] - for node_a in all_nodes: - _node_pos_a = get_node_from_tmap2(tmap, node_a)["node"]["pose"] - node_pos_a = np.array([_node_pos_a["position"]["x"], _node_pos_a["position"]["y"]]) - - distances.append( - np.sqrt(np.sum(np.power(node_pos - node_pos_a, 2))) - ) - if len(distances) > 0: - evaluation = np.min(distances) > float(value) - - return evaluation - - # value is the min distance [m] from the obstacle that the robot must have for the edge to be obstacleFree - def evaluate_edge(self, edge, value, robot_state={}, tmap=None): - """ Returns the value of the restriction associated with the given entity, must return a boolean value """ - evaluation = self.DEFAULT_EVALUATION - - # print(self.robot_nodes, rospy.get_namespace().strip("/")) - if tmap is not None and self.active: - distances = [] - - orig, dest = get_node_names_from_edge_id_2(tmap, edge) - _edge_pos_a = get_node_from_tmap2(tmap, orig)["node"]["pose"] - edge_pos_a = np.array([[_edge_pos_a["position"]["x"], _edge_pos_a["position"]["y"], 0.]]) - _edge_pos_b = get_node_from_tmap2(tmap, dest)["node"]["pose"] - edge_pos_b = np.array([[_edge_pos_b["position"]["x"], _edge_pos_b["position"]["y"], 0.]]) - - all_nodes = [self._get_closest_node(r) for r in self.robot_nodes if r != rospy.get_namespace().strip("/")] - - for node_a in all_nodes: - _node_pos = get_node_from_tmap2(tmap, node_a)["node"]["pose"] - node_pos = np.array([[_node_pos["position"]["x"], _node_pos["position"]["y"], 0.]]) - - distance = pnt2line(node_pos, edge_pos_a, edge_pos_b) - - distances.append(distance) - - if len(distances) > 0: - evaluation = np.min(distances) > float(value) - - return evaluation - - def ground_to_robot(self): - pass -############################### - diff --git a/topological_navigation/topological_navigation/route_search.py b/topological_navigation/topological_navigation/route_search.py deleted file mode 100644 index 25ea40f2..00000000 --- a/topological_navigation/topological_navigation/route_search.py +++ /dev/null @@ -1,153 +0,0 @@ -#!/usr/bin/env python - -import rospy -from topological_navigation.tmap_utils import * -from topological_navigation_msgs.msg import NavRoute - - -class NodeToExpand(object): - def __init__(self, name, father, current_distance, dist_to_target): - self.name = name - self.expanded = False - self.father = father - self.current_distance = current_distance - self.dist_to_target = dist_to_target - self.cost = self.current_distance + self.dist_to_target - #print self.name, ":\n", "\tcurrent dist: ", self.current_distance, "\n\tdist_to_target: ", self.dist_to_target, "\n\tcost: ", self.cost - - def __repr__(self): - return "-------\n\t Node: \n\t name:%s \n\t Father:%s \n\t current_distance:%f \n\t distance to target: %f \n\t cost %f \n" % (self.name, self.father, self.current_distance, self.dist_to_target, self.cost) - - -class TopologicalRouteSearch(object): - - def __init__(self, top_map): - # rospy.loginfo("Waiting for Topological map ...") - self.top_map = top_map - - """ - search_route - - This function searches the route to reach the goal - """ - - def search_route(self, origin, target): - if origin == "none" or target == "none" or origin == target: - return None - - goal = get_node(self.top_map, target) - orig = get_node(self.top_map, origin) - to_expand = [] - children = [] - expanded = [] - - #print 'searching route from %s to %s' %(orig.name, goal.name) - - #self.get_distance_to_node(goal, orig) - nte = NodeToExpand(orig.name, 'none', 0.0, - get_distance_to_node(goal, orig)) # Node to Expand - expanded.append(nte) - #to_expand.append(nte) - -# exp_index=0 - cen = orig # currently expanded node - - # nodes current node is connected to - children = get_conected_nodes(cen) - #print "children\n", children - not_goal = True - route_found = False - while not_goal: - if target in children: - not_goal = False - route_found = True - cdist = get_distance_to_node(cen, goal) - cnte = NodeToExpand( - goal.name, nte.name, nte.current_distance+cdist, 0.0) # Node to Expand - expanded.append(cnte) - #print "goal found" - else: - #print "Goal NOT found" - for i in children: - been_expanded = False - to_be_expanded = False - for j in expanded: - # search in expanded - if i == j.name: - been_expanded = True - old_expand_node = j - # found it. skip remaining - break - if not been_expanded: - # not in expanded, search in to_expand. can't be in both - for j in to_expand: - if i == j.name: - been_expanded = True - to_be_expanded = True - old_expand_node = j - # found it. skip remaining - break - - if not been_expanded: - nnn = get_node(self.top_map, i) - tdist = get_distance_to_node(goal, nnn) - cdist = get_distance_to_node(cen, nnn) - cnte = NodeToExpand( - nnn.name, nte.name, nte.current_distance+cdist, tdist) # Node to Expand - to_expand.append(cnte) - to_expand = sorted( - to_expand, key=lambda node: node.cost) - else: - nnn = get_node(self.top_map, i) - tdist = get_distance_to_node(goal, nnn) - cdist = get_distance_to_node(cen, nnn) - # update existing NTE with new data if a shorter route to it is found - if nte.current_distance+cdist < old_expand_node.current_distance: - old_expand_node.father = nte.name - old_expand_node.current_distance = nte.current_distance+cdist - old_expand_node.dist_to_target = tdist - old_expand_node.cost = old_expand_node.current_distance + \ - old_expand_node.dist_to_target - if to_be_expanded: - # re-sort to_expand with new costs - to_expand = sorted( - to_expand, key=lambda node: node.cost) - - if len(to_expand) > 0: - nte = to_expand.pop(0) - #print 'expanding node %s (%d)' %(nte.name,len(to_expand)) - cen = get_node(self.top_map, nte.name) - expanded.append(nte) - children = get_conected_nodes(cen) - else: - not_goal = False - route_found = False - - route = NavRoute() -# print "===== RESULT =====" - if route_found: - steps = [] - val = len(expanded)-1 - steps.append(expanded[val]) - next_node = expanded[val].father - #print next_node - while next_node != 'none': - for i in expanded: - if i.name == next_node: - steps.append(i) - next_node = i.father - break - - steps.reverse() - val = len(steps) - for i in range(1, val): - edg = get_edges_between( - self.top_map, steps[i].father, steps[i].name) - route.source.append(steps[i].father) - route.edge_id.append(edg[0].edge_id) - #route.append(r) - - #print route - return route - else: - return None diff --git a/topological_navigation/topological_navigation/route_search2.py b/topological_navigation/topological_navigation/route_search2.py deleted file mode 100644 index 0475ad36..00000000 --- a/topological_navigation/topological_navigation/route_search2.py +++ /dev/null @@ -1,280 +0,0 @@ -#!/usr/bin/env python -######################################################################################################### -import rclpy -from topological_navigation.tmap_utils import * -from topological_navigation_msgs.msg import NavRoute -from rclpy.node import Node - -class NodeToExpand(object): - - - def __init__(self, name, father, current_distance, dist_to_target): - - self.name = name - self.expanded=False - self.father=father - self.current_distance = current_distance - self.dist_to_target = dist_to_target - self.cost = self.current_distance + self.dist_to_target - - - def __repr__(self): - return "-------\n\t Node: \n\t name:%s \n\t Father:%s \n\t current_distance:%f \n\t distance to target: %f \n\t cost %f \n" %(self.name, self.father, self.current_distance, self.dist_to_target, self.cost) -######################################################################################################### - - -######################################################################################################### -class TopologicalRouteSearch2(object): - def __init__(self, top_map) : - - self.top_map = top_map - self.nodes = {} - for node in self.top_map["nodes"]: - name = node["node"]["name"] - self.nodes[name] = node - - self.children = {} - for node in self.top_map["nodes"]: - name = node["node"]["name"] - self.children[name] = [] - for edge in node["node"]["edges"]: - dist = get_distance_to_node_tmap2(self.nodes[name], self.nodes[edge["node"]]) - item = {"name": edge["node"], "distance": dist} - self.children[name].append(item) - - - def search_route(self, origin, target, avoid_edges=[]): - """ - This function searches the route to reach the goal - """ - route = NavRoute() - - if origin == "none" or target == "none" or origin == target: - return route - - goal = self.get_node_from_tmap2(target) - orig = self.get_node_from_tmap2(origin) - to_expand=[] - children=[] - expanded=[] - - nte = NodeToExpand(orig["node"]["name"], 'none', 0.0, get_distance_to_node_tmap2(goal, orig)) # Node to Expand - expanded.append(nte) - - cen = orig # currently expanded node - children = self.get_connected_nodes_tmap2(cen) # nodes current node is connected to - - # get the pair of node (orig-dest) of the edges we need to avoid - avoid_pairs = [] - for _edge in avoid_edges: - avoid_pairs.append( - get_node_names_from_edge_id_2(self.top_map, _edge) - ) - - # remove the nodes we want to avoid - if origin in [p[0] for p in avoid_pairs]: - for (_, _dest) in avoid_pairs: - if _dest in children: - children.remove(_dest) - - not_goal=True - route_found=False - while not_goal: - if target in children: - not_goal=False - route_found=True - cdist = self.get_distance_to_node_tmap2(cen, goal) - cnte = NodeToExpand(goal["node"]["name"], nte.name, nte.current_distance+cdist, 0.0) # Node to Expand - expanded.append(cnte) - else : - for i in children: - been_expanded = False - to_be_expanded = False - for j in expanded: # search in expanded - if i == j.name: - been_expanded = True - old_expand_node = j # found it. skip remaining - break - if not been_expanded: - # not in expanded, search in to_expand. can't be in both - for j in to_expand: - if i == j.name: - been_expanded = True - to_be_expanded = True - old_expand_node = j - # found it. skip remaining - break - - if not been_expanded: - nnn = self.get_node_from_tmap2(i) - tdist = get_distance_to_node_tmap2(goal, nnn) - cdist = self.get_distance_to_node_tmap2(cen, nnn) - cnte = NodeToExpand(nnn["node"]["name"], nte.name, nte.current_distance+cdist, tdist) # Node to Expand - to_expand.append(cnte) - to_expand = sorted(to_expand, key=lambda node: node.cost) - else: - nnn = self.get_node_from_tmap2(i) - tdist = get_distance_to_node_tmap2(goal, nnn) - cdist = self.get_distance_to_node_tmap2(cen, nnn) - # update existing NTE with new data if a shorter route to it is found - if nte.current_distance+cdist < old_expand_node.current_distance: - old_expand_node.father = nte.name - old_expand_node.current_distance = nte.current_distance+cdist - old_expand_node.dist_to_target = tdist - old_expand_node.cost = old_expand_node.current_distance + old_expand_node.dist_to_target - if to_be_expanded: # re-sort to_expand with new costs - to_expand = sorted(to_expand, key=lambda node: node.cost) - - if len(to_expand)>0: - nte = to_expand.pop(0) - cen = self.get_node_from_tmap2(nte.name) - expanded.append(nte) - children = self.get_connected_nodes_tmap2(cen) - # remove the nodes we want to avoid - if nte.name in [p[0] for p in avoid_pairs]: - for (_, _dest) in avoid_pairs: - if _dest in children: - children.remove(_dest) - else: - not_goal=False - route_found=False - -# print "===== RESULT =====" - if route_found: - steps=[] - val = len(expanded)-1 - steps.append(expanded[val]) - next_node = expanded[val].father - - while next_node != 'none': - for i in expanded: - if i.name == next_node : - steps.append(i) - next_node = i.father - break - - steps.reverse() - val = len(steps) - for i in range(1, val): - edg=self.get_edges_between_tmap2(steps[i].father, steps[i].name) - route.source.append(steps[i].father) - route.edge_id.append(edg[0]["edge_id"]) - - return route - - - def get_node_from_tmap2(self, node_name): - - try: - node = self.nodes[node_name] - except Exception: - node = None - return node - - - def get_edges_between_tmap2(self, nodea, nodeb): - - ab = [] - noda = self.get_node_from_tmap2(nodea) - for j in noda["node"]["edges"]: - if j["node"] == nodeb: - ab.append(j) - return ab - - - def get_connected_nodes_tmap2(self, node): - - items = self.children[node["node"]["name"]] - return [item["name"] for item in items] - - - def get_distance_to_node_tmap2(self, nodea, nodeb): - - items = self.children[nodea["node"]["name"]] - for item in items: - if item["name"] == nodeb["node"]["name"]: - return item["distance"] -######################################################################################################### - - -######################################################################################################### -class RouteChecker(Node): - def __init__(self, tmap): - super().__init__("route_checker") - self.edge_dict = {} - for node in tmap["nodes"]: - self.edge_dict[node["node"]["name"]] = [] - for edge in node["node"]["edges"]: - item = {"node": edge["node"], "edge_id": edge["edge_id"]} - self.edge_dict[node["node"]["name"]].append(item) - - def check_route(self, route): - self.get_logger().info("Checking Route...") - N = len(route.source) - if N < 1 or N != len(route.edge_id): - self.get_logger().error("Invalid Route: Either the route is empty or the number of nodes do not equal the number of edge ids") - return False - - if "" in route.source or "" in route.edge_id: - self.get_logger().error("Invalid Route: Empty string found in nodes or edge ids or both") - return False - - for i in range(N-1): - node = route.source[i] - if node not in self.edge_dict: - self.get_logger().error("Invalid Route: Node {} does not exist".format(node)) - return False - n = 0 - edge_id = route.edge_id[i] - next_node = route.source[i+1] - for edge in self.edge_dict[node]: - if edge["edge_id"] == edge_id and edge["node"] == next_node: - n += 1 - if n != 1: - self.get_logger().error("Invalid Route: No edge from {} to {} with id {} found or multiple edges found".format(node, next_node, edge_id)) - return False - - final_node = route.source[-1] - if final_node not in self.edge_dict: - self.get_logger().error("Invalid Route: Node {} does not exist".format(final_node)) - return False - - n = 0 - final_edge_id = route.edge_id[-1] - for edge in self.edge_dict[final_node]: - if edge["edge_id"] == final_edge_id and edge["node"] in self.edge_dict: - n += 1 - - if n != 1: - self.get_logger().error("Invalid Route: No edge from {} with id {} found or its destination node does not exist or multiple edges found".format(final_node, final_edge_id)) - return False - - self.get_logger().info("Route is Valid") - return True -######################################################################################################### - - -######################################################################################################### -def get_route_distance(tmap, node_a, node_b): - - if node_a is None or node_b is None: - return 10e5-1 - - if node_a["node"]["name"] == node_b["node"]["name"]: - return 0.0 - - rsearch = TopologicalRouteSearch2(tmap) - route = rsearch.search_route(node_a["node"]["name"], node_b["node"]["name"]) - - if not route.source: - return 10e5-1 - - dist = 0 - for i in range(len(route.source)-1): - node_1 = rsearch.get_node_from_tmap2(route.source[i]) - node_2 = rsearch.get_node_from_tmap2(route.source[i+1]) - dist += get_distance_to_node_tmap2(node_1, node_2) - - dist += get_distance_to_node_tmap2(rsearch.get_node_from_tmap2(route.source[-1]), node_b) - return dist -######################################################################################################### diff --git a/topological_navigation/topological_navigation/scripts/actions_bt.py b/topological_navigation/topological_navigation/scripts/actions_bt.py deleted file mode 100644 index 97ca8c6f..00000000 --- a/topological_navigation/topological_navigation/scripts/actions_bt.py +++ /dev/null @@ -1,169 +0,0 @@ - -class ActionsType: - def __init__(self): - - self.NAVIGATE_TO_POSE = "NavigateToPose" - self.NAVIGATE_THROUGH_POSES = "NavigateThroughPoses" - self.DRIVE_ON_HEADING = "DriveOnHeading" - - self.ROW_TRAVERSAL = "row_traversal" - self.ROW_OPERATION = "row_operation" - self.ROW_RECOVERY = "row_recovery" - self.ROW_CHANGE = "row_change" - self.GOAL_ALIGN = "goal_align" - self.GOAL_ALIGN_INDEX = ["ca"] - self.GOAL_ALIGN_GOAL = ["cb"] - self.ROW_START_INDEX = "a" - self.ROW_COLUMN_START_INDEX = "c" - self.ROW_COLUMN_START_NEXT_INDEX = "b" - self.OUTSIDE_EDGE_START_INDEX = "WayPoint" - - self.INSIDE_POLYTUNNEL = "INSIDE_POLYTUNNEL" - self.OUTSIDE_POLYTUNNEL = "OUTSIDE_POLYTUNNEL" - self.TRANSITION_INTO_POLYTUNNEL = "TRANSITION_INTO_POLYTUNNEL" - - - self.ROBOT_STATUS_PREPARATION_STATE = "PREPARATION_STATE" - self.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE = "AUTONOMOUS_NAVIGATION_STATE" - self.ROBOT_STATUS_AUTONOMOUS_HARVESTING_STATE = "AUTONOMOUS_HARVESTING_STATE" - self.ROBOT_STATUS_AUTONOMOUS_RECOVERY_STATE = "AUTONOMOUS_RECOVERY_STATE" - self.ROBOT_STATUS_DISABLE_STATE = "DISABLE_STATE" - self.ROBOT_STATUS_NATURAL_STATE = "NATURAL_STATE" - - self.ROBOT_CURRENT_STATUS = {} - self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_PREPARATION_STATE] = 0 - self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE] = 1 - self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_AUTONOMOUS_HARVESTING_STATE] = 2 - self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_AUTONOMOUS_RECOVERY_STATE] = 3 - self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_DISABLE_STATE] = 4 - self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_NATURAL_STATE] = 5 - - self.ABORT_NOT_CONTINUE = [self.GOAL_ALIGN, self.ROW_CHANGE, self.ROW_TRAVERSAL - , self.NAVIGATE_TO_POSE, self.NAVIGATE_THROUGH_POSES - , self.ROW_OPERATION, self.ROW_RECOVERY] - - self.BT_DEFAULT = "bt_tree_default" - self.BT_GOAL_ALIGN = "bt_tree_goal_align" - self.BT_IN_ROW = "bt_tree_in_row" - self.BT_IN_ROW_OPERATION = "bt_tree_in_row_operation" - self.BT_IN_ROW_RECOVERY = "bt_tree_in_row_recovery" - - self.navigation_actions = [ - self.NAVIGATE_TO_POSE, - self.NAVIGATE_THROUGH_POSES, - self.DRIVE_ON_HEADING, - self.ROW_CHANGE, - self.ROW_TRAVERSAL, - self.ROW_OPERATION, - self.ROW_RECOVERY - ] - - self.bt_tree_types = [ - self.BT_IN_ROW, - self.BT_DEFAULT, - self.BT_GOAL_ALIGN, - self.BT_IN_ROW_OPERATION, - self.BT_IN_ROW_RECOVERY, - ] - - self.bt_tree_with_actions = {} - self.bt_tree_with_actions[self.NAVIGATE_THROUGH_POSES] = self.BT_DEFAULT - self.bt_tree_with_actions[self.NAVIGATE_TO_POSE] = self.BT_DEFAULT - self.bt_tree_with_actions[self.GOAL_ALIGN] = self.BT_GOAL_ALIGN - self.bt_tree_with_actions[self.ROW_TRAVERSAL] = self.BT_IN_ROW - self.bt_tree_with_actions[self.ROW_OPERATION] = self.BT_IN_ROW_OPERATION - self.bt_tree_with_actions[self.ROW_RECOVERY] = self.BT_IN_ROW_RECOVERY - - self.status_mapping = {} - self.status_mapping[0] = "STATUS_UNKNOWN" - self.status_mapping[1] = "STATUS_ACCEPTED" - self.status_mapping[2] = "STATUS_EXECUTING" - self.status_mapping[3] = "STATUS_CANCELING" - self.status_mapping[4] = "STATUS_SUCCEEDED" - self.status_mapping[5] = "STATUS_CANCELED" - self.status_mapping[6] = "STATUS_ABORTED" - - self.goal_cancel_error_codes = {} - self.goal_cancel_error_codes[0] = "ERROR_NONE" - self.goal_cancel_error_codes[1] = "ERROR_REJECTED" - self.goal_cancel_error_codes[2] = "ERROR_UNKNOWN_GOAL_ID" - self.goal_cancel_error_codes[3] = "ERROR_GOAL_TERMINATED" - - self.planner_with_goal_checker_config = { - "dwb_core::DWBLocalPlanner": { - "goal_checker.xy_goal_tolerance": 0.78, - "goal_checker.yaw_goal_tolerance": 0.25, - }, - "teb_local_planner::TebLocalPlannerROS": { - "goal_checker.xy_goal_tolerance": 0.1, - "goal_checker.yaw_goal_tolerance": 0.05, - }, - "rownav_local_planner::TebLocalPlannerROS":{ - "goal_checker.xy_goal_tolerance": 0.1, - "goal_checker.yaw_goal_tolerance": 0.6, - }, - "behavioral_controller::BehavioralController":{ - "goal_checker.xy_goal_tolerance": 0.1, - "goal_checker.yaw_goal_tolerance": 0.6, - } - } - - self.bt_tree_with_control_server_config = {} - self.bt_tree_with_control_server_config[self.ROW_TRAVERSAL] = "dwb_core::DWBLocalPlanner" - self.bt_tree_with_control_server_config[self.NAVIGATE_THROUGH_POSES] = "dwb_core::DWBLocalPlanner" - self.bt_tree_with_control_server_config[self.NAVIGATE_TO_POSE] = "dwb_core::DWBLocalPlanner" - self.bt_tree_with_control_server_config[self.GOAL_ALIGN] = "dwb_core::DWBLocalPlanner" - self.bt_tree_with_control_server_config[self.ROW_RECOVERY] = "dwb_core::DWBLocalPlanner" - - self.planner_with_pd_regulator_config = { - "dwb_core::DWBLocalPlanner": { - "kp_v": 40.0, - "kd_v": 0.1, - "kp_w": 12.0, - "kd_w": 0.1, - }, - "teb_local_planner::TebLocalPlannerROS": { - "kp_v": 40.0, - "kd_v": 0.1, - "kp_w": 12.0, - "kd_w": 0.1, - }, - "rownav_local_planner::TebLocalPlannerROS":{ - "kp_v": 40.0, - "kd_v": 0.1, - "kp_w": 12.0, - "kd_w": 0.1, - }, - "behavioral_controller::BehavioralController":{ - "kp_v": 40.0, - "kd_v": 0.1, - "kp_w": 12.0, - "kd_w": 0.1, - } - } - - def getCodeForRobotCurrentStatus(self, msg): - if msg not in self.ROBOT_CURRENT_STATUS: - print("The {} is not one of configured robot status types".format(msg)) - return self.ROBOT_CURRENT_STATUS[self.ROBOT_STATUS_DISABLE_STATE] - return self.ROBOT_CURRENT_STATUS[msg] - - def setPlanner(self, planner_name, action_type): - self.bt_tree_with_control_server_config[action_type] = planner_name - - def setPlannerParams(self, planner_name, xy_goal_tolerance, yaw_goal_tolerance): - if not planner_name in self.planner_with_goal_checker_config: - print("The planner {} is not one of configured types".format(planner_name)) - return - self.planner_with_goal_checker_config[planner_name]["goal_checker.xy_goal_tolerance"] = xy_goal_tolerance - self.planner_with_goal_checker_config[planner_name]["goal_checker.yaw_goal_tolerance"] = yaw_goal_tolerance - print("Setting planner {} params xy_goal_tolerance: {}, yaw_goal_tolerance: {}".format(planner_name, xy_goal_tolerance, yaw_goal_tolerance)) - - def setPDRegulstorParams(self, planner_name, pd_params, robot_controller_name): - if not planner_name in self.planner_with_goal_checker_config: - print("The planner {} is not one of configured types".format(planner_name)) - return - parmas_names = ["kp_v", "kd_v", "kp_w", "kd_w"] - for index, val in zip(parmas_names, pd_params): - self.planner_with_pd_regulator_config[planner_name][index] = val - print("Setting planner {} params kp_v: {} kd_v: {} kp_w: {} kd_w: {} ".format(planner_name, pd_params[0], pd_params[1], pd_params[2], pd_params[3])) diff --git a/topological_navigation/topological_navigation/scripts/evaluate_top_pred.py b/topological_navigation/topological_navigation/scripts/evaluate_top_pred.py deleted file mode 100755 index 114d7244..00000000 --- a/topological_navigation/topological_navigation/scripts/evaluate_top_pred.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -from topological_navigation_msgs.srv import * - - -def predict_edges(seconds_from_now): - rospy.wait_for_service('topological_prediction/predict_edges') - try: - get_prediction = rospy.ServiceProxy('topological_prediction/predict_edges', topological_navigation_msgs.srv.PredictEdgeState) - now = rospy.Time.now() - prediction_time = now + rospy.Duration(seconds_from_now) - print("Requesting prediction for %f"%prediction_time.secs) - resp1 = get_prediction(prediction_time) - return resp1 - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - -def usage(): - return "%s seconds_from_now"%sys.argv[0] - -if __name__ == "__main__": - if len(sys.argv) == 2: - seconds_from_now = int(sys.argv[1]) - else: - print(usage()) - sys.exit(1) - rospy.init_node('topological_prediction_test') - est = predict_edges(seconds_from_now) - print("set of repeated edges:") - print(set([x for x in est.edge_ids if est.edge_ids.count(x) > 1])) - #print est.edge_id - - if len(est.edge_ids) == len(est.probs) and len(est.edge_ids) == len(est.durations): - print("Good Answer!!! %d" %len(est.probs)) - for i in range(len(est.edge_ids)): - print(est.edge_ids[i], est.probs[i], est.durations[i].secs) - else: - print("something failed") - print(len(est.edge_ids), len(est.probs), len(est.durations)) diff --git a/topological_navigation/topological_navigation/scripts/get_simple_policy.py b/topological_navigation/topological_navigation/scripts/get_simple_policy.py deleted file mode 100755 index 9d6ad179..00000000 --- a/topological_navigation/topological_navigation/scripts/get_simple_policy.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -from std_msgs.msg import String - -import topological_navigation_msgs.srv -from topological_navigation_msgs.msg import TopologicalMap -import topological_navigation.route_search -#from topological_navigation.route_search import * - - -class SearchPolicyServer(object): - - def __init__(self) : - - #Waiting for Topological Map - self._map_received=False - rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) - rospy.loginfo("Waiting for Topological map ...") - while not self._map_received : - rospy.sleep(rospy.Duration.from_sec(0.05)) - rospy.loginfo(" ...done") - - self._top_loc=False - rospy.loginfo("Waiting for Topological localisation ...") - rospy.Subscriber('closest_node', String, self.closestNodeCallback) - while not self._top_loc : - rospy.sleep(rospy.Duration.from_sec(0.05)) - rospy.loginfo(" ...done") - - - #This service returns any given map - self.get_map_srv=rospy.Service('get_simple_policy/get_route_to', topological_navigation_msgs.srv.GetRouteTo, self.get_route_cb) - self.get_map_srv=rospy.Service('get_simple_policy/get_route_between', topological_navigation_msgs.srv.GetRouteBetween, self.get_routeb_cb) - rospy.loginfo("All Done ...") - rospy.spin() - - - def get_route_cb(self, req): - rsearch = topological_navigation.route_search.TopologicalRouteSearch(self.top_map) - route = rsearch.search_route(self.closest_node, req.goal) - print(route) - return route - - def get_routeb_cb(self, req): - rsearch = topological_navigation.route_search.TopologicalRouteSearch(self.top_map) - route = rsearch.search_route(req.origin, req.goal) - print(route) - return route - - - """ - Update Map CallBack - - This Function updates the Topological Map everytime it is called - """ - def MapCallback(self, msg) : - self.top_map = msg - self._map_received=True - - - """ - Closest Node CallBack - - """ - def closestNodeCallback(self, msg): - self.closest_node=msg.data - self._top_loc=True - - - - - -if __name__ == '__main__': - rospy.init_node('route_search') - searcher = SearchPolicyServer() diff --git a/topological_navigation/topological_navigation/scripts/get_simple_policy2.py b/topological_navigation/topological_navigation/scripts/get_simple_policy2.py deleted file mode 100755 index f44a5210..00000000 --- a/topological_navigation/topological_navigation/scripts/get_simple_policy2.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python - -""" -Created on Tue Nov 17 22:02:24 2023 -@author: Geesara Kulathunga (ggeesara@gmail.com) - -""" - -import sys -import rclpy, json , yaml -from std_msgs.msg import String -from topological_navigation_msgs.srv import GetRouteTo, GetRouteBetween -from topological_navigation.route_search2 import RouteChecker, TopologicalRouteSearch2, get_route_distance -from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy -from rclpy.executors import MultiThreadedExecutor - -# this ensures that all the poses and translates -# are float-type and not int-type as there is an -# assertion in ros2 messages (vector3, pose etc.) -# for float-type [x,y,z,w] keys. -class CustomSafeLoader(yaml.SafeLoader): - def construct_mapping(self, node, deep=False): - mapping = super().construct_mapping(node, deep=deep) - - # this can be extended to test the validity of the tmap2 - # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w']: - if key in mapping and isinstance(mapping[key], int): - mapping[key] = float(mapping[key]) - - return mapping - - -class SearchPolicyServer(rclpy.node.Node): - - def __init__(self, name): - super().__init__(name) - self._map_received = False - self.qos = QoSProfile(depth=1, - reliability=ReliabilityPolicy.RELIABLE, - history=HistoryPolicy.KEEP_LAST, - durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.mapper_sub = self.create_subscription(String, '/topological_map_2', self.map_callback, qos_profile=self.qos) - self.get_logger().info("Waiting for Topological map ...") - while rclpy.ok(): - rclpy.spin_once(self) - if self._map_received: - self.get_logger().info("Navigation received the Topological Map") - break - self._top_loc=False - self.get_logger().info("Waiting for Topological localisation ...") - self.closest_node_sub = self.create_subscription(String, 'closest_node', self.closest_node_callback, qos_profile=self.qos) - while rclpy.ok(): - rclpy.spin_once(self) - if self._top_loc: - self.get_logger().info("Localisation model is up...") - break - - self.service_to_get_goal = self.create_service(GetRouteTo, 'get_simple_policy/get_route_to', self.get_route_cb) - self.service_to_get_path = self.create_service(GetRouteBetween, 'get_simple_policy/get_route_between', self.get_routeb_cb) - self.get_logger().info("All Done ...") - - - def get_route_cb(self, req, res): - route = self.rsearch.search_route(self.closest_node, req.goal) - print(route) - res.route = route - return res - - def get_routeb_cb(self, req, res): - route = self.rsearch.search_route(req.origin, req.goal) - print(route) - res.route = route - return res - - def map_callback(self, msg) : - # self.lnodes = json.loads(msg.data) - self.lnodes = yaml.load( msg.data, Loader=CustomSafeLoader) - self.topol_map = self.lnodes["pointset"] - self.rsearch = TopologicalRouteSearch2(self.lnodes) - self._map_received = True - - def closest_node_callback(self, msg): - self.closest_node=msg.data - self._top_loc=True - -def main(args=None): - rclpy.init(args=args) - node = SearchPolicyServer('simple_policy_server') - executor = MultiThreadedExecutor() - executor.add_node(node) - try: - executor.spin() - except KeyboardInterrupt: - node.get_logger().info('shutting down simple_policy_server node\n') - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__' : - main() - - diff --git a/topological_navigation/topological_navigation/scripts/in_row_operations.py b/topological_navigation/topological_navigation/scripts/in_row_operations.py deleted file mode 100644 index 986d31a7..00000000 --- a/topological_navigation/topological_navigation/scripts/in_row_operations.py +++ /dev/null @@ -1,115 +0,0 @@ -import math -import numpy as np -from geometry_msgs.msg import PoseStamped - -class RowOperations: - def __init__(self, initial_edges, step_size=2.0, intermediate_dis=0.6): - self.initial_edges = initial_edges - self.initial_path = False - self.step_size = step_size - self.intermediate_dis = intermediate_dis - if len(self.initial_edges) > 1: - self.terminal_pose = self.getPoseSE2(self.initial_edges[-1]) - self.getApproximatedLine() - else: - print("Can not creat a projeted path...") - self.initial_path = False - - def isPlanCalculated(self, ): - return self.initial_path - - def shortestDistance(x1, y1, a, b, c): - d = abs((a * x1 + b * y1 + c)) / (math.sqrt(a * a + b * b)) - return d - - def getPoseSE2(self, point): - return np.array([point.pose.position.x, point.pose.position.y]) - - def getSE2Pose(self, point, orientation): - target_pose = PoseStamped() - target_pose.pose.position.x = point[0] - target_pose.pose.position.y = point[1] - target_pose.pose.position.z = 0.3 - target_pose.pose.orientation = orientation - return target_pose - - def getApproximatedLine(self): - """ - (a,b,c) to represent the linear equation ax+by+c=0. - """ - p1 = self.getPoseSE2(self.initial_edges[0]) - p2 = self.terminal_pose - self.a = p2[1] - p1[1] - self.b = p1[0] - p2[0] - self.c = -1.0*(self.a * (p1[0]) + self.b * (p1[1])) - self.initial_path = True - - def vecScale(self, v, a): - return (a * v[0], a * v[1]) - - def vecMulAdd(self, v1, v2, a=1): - """calc v1+a*v2""" - return (v1[0] + a * v2[0], v1[1] + a * v2[1]) - - def innerProd(self, v1, v2): - """v1^T * v2""" - return v1[0] * v2[0] + v1[1] * v2[1] - - def getClosestPoint(self, p): - """ - Params: - coef: 3-tuple (a,b,c) to represent the linear equation ax+by+c=0. - p: 2-tuple (px,py) to represent the point. - https://math.stackexchange.com/questions/62633/orthogonal-projection-of-a-point-onto-a-line - """ - a = self.a - b = self.b - c = self.c - # find p0 at the line - if b == 0: - p0 = (-c / a, 0) - else: - p0 = (0, -c / b) - if a == 0: - v = (1, 0) - elif b == 0: - v = (0, 1) - else: - v = (b, -a) - target_pose = self.vecMulAdd( - p0, self.vecScale(v, self.innerProd(self.vecMulAdd(p, p0, -1), v) / self.innerProd(v, v)) - ) - return np.array([target_pose[0], target_pose[1]]) - - def getNextGoal(self, current_pose_original): - current_pose = self.getPoseSE2(current_pose_original) - closest_pose = self.getClosestPoint(current_pose) - norm_dis = np.linalg.norm(self.terminal_pose-closest_pose) - updated_pose = None - intermediate_pose = None - if(norm_dis < self.step_size): - updated_pose = self.getSE2Pose(self.terminal_pose, current_pose_original.pose.orientation) - intermediate_pose = current_pose + self.intermediate_dis*((self.terminal_pose-closest_pose)/norm_dis) - intermediate_pose = self.getSE2Pose(intermediate_pose, current_pose_original.pose.orientation) - return updated_pose, intermediate_pose, True - else: - updated_pose = current_pose + self.step_size*((self.terminal_pose-closest_pose)/norm_dis) - updated_pose = self.getSE2Pose(updated_pose, current_pose_original.pose.orientation) - - intermediate_pose = current_pose + self.intermediate_dis*((self.terminal_pose-closest_pose)/norm_dis) - intermediate_pose = self.getSE2Pose(intermediate_pose, current_pose_original.pose.orientation) - return updated_pose, intermediate_pose, False - - def getNextIntermediateGoal(self, current_pose_original): - current_pose = self.getPoseSE2(current_pose_original) - closest_pose = self.getClosestPoint(current_pose) - norm_dis = np.linalg.norm(self.terminal_pose-closest_pose) - intermediate_pose = None - if(norm_dis < self.step_size): - intermediate_pose = current_pose + self.intermediate_dis*((self.terminal_pose-closest_pose)/norm_dis) - intermediate_pose = self.getSE2Pose(intermediate_pose, current_pose_original.pose.orientation) - return intermediate_pose, True - else: - intermediate_pose = current_pose + self.intermediate_dis*((self.terminal_pose-closest_pose)/norm_dis) - intermediate_pose = self.getSE2Pose(intermediate_pose, current_pose_original.pose.orientation) - return intermediate_pose, False diff --git a/topological_navigation/topological_navigation/scripts/localisation.py b/topological_navigation/topological_navigation/scripts/localisation.py deleted file mode 100755 index c0340fbd..00000000 --- a/topological_navigation/topological_navigation/scripts/localisation.py +++ /dev/null @@ -1,530 +0,0 @@ -#!/usr/bin/env python -################################################################################################################### -import sys, json, numpy as np -import rospy, rostopic, tf -import topological_navigation_msgs.srv - -from geometry_msgs.msg import Pose -from std_msgs.msg import String, Float32 -from topological_navigation_msgs.msg import ClosestEdges - -from topological_navigation.tmap_utils import * -from topological_navigation.point2line import pnt2line - -from threading import Thread - - -class LocaliseByTopicSubscriber(object): - """ - Helper class for localise by topic subcription. Callable to start subsriber - thread. - """ - def __init__(self, topic, callback, callback_args): - - self.topic = rospy.get_namespace() + topic - self.callback = callback - self.callback_args = callback_args - self.sub = None - self.t = None - - - def get_topic_type(self, topic, blocking=False): - """ - Get the topic type. - !!! Overriden from rostopic !!! - - :param topic: topic name, ``str`` - :param blocking: (default False) block until topic becomes available, ``bool`` - - :returns: topic type, real topic name and fn to evaluate the message instance - if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)`` - :raises: :exc:`ROSTopicException` If master cannot be contacted - """ - topic_type, real_topic, msg_eval = rostopic._get_topic_type(topic) - if topic_type: - return topic_type, real_topic, msg_eval - elif blocking: - sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n"%topic) - while not rospy.is_shutdown(): - topic_type, real_topic, msg_eval = rostopic._get_topic_type(topic) - if topic_type: - return topic_type, real_topic, msg_eval - else: - rostopic._sleep(10.) # Change! Waiting for 10 seconds instead of 0.1 to reduce load - - return None, None, None - - - def __call__(self): - """ - When called start a new thread that waits for the topic type and then - subscribes. This is therefore non blocking and waits in the background. - """ - self.t = Thread(target=self.subscribe) - self.t.start() - - - def subscribe(self): - """ - Get the topic type and subscribe to topic. Subscriber is kept alive as - long as the instance of the class is alive. - """ - rostopic.get_topic_type = self.get_topic_type # Monkey patch - topic_type = rostopic.get_topic_class(self.topic, True)[0] - rospy.loginfo("Subscribing to %s" % self.topic) - self.sub = rospy.Subscriber( - name=self.topic, - data_class=topic_type, - callback=self.callback, - callback_args=self.callback_args - ) - - - def close(self): - self.sub.unregister() - - - def __del__(self): - self.close() -################################################################################################################### - - -################################################################################################################### -class TopologicalNavLoc(object): - - - def __init__(self, name, wtags): - - self.throttle_val = rospy.get_param("~LocalisationThrottle", 3) - self.only_latched = rospy.get_param("~OnlyLatched", True) - - self.throttle = self.throttle_val - self.node="Unknown" - self.wpstr="Unknown" - self.closest_dist = 10e5-1 - self.cnstr="Unknown" - self.closest_edge_ids = [] - self.closest_edge_dists = [] - self.node_poses = {} - - # TODO: remove Temporary arg until tags functionality is MongoDB independent - self.with_tags = wtags - - self.subscribers=[] - self.wp_pub = rospy.Publisher('closest_node', String, latch=True, queue_size=1) - self.wd_pub = rospy.Publisher('closest_node_distance', Float32, latch=True, queue_size=1) - self.cn_pub = rospy.Publisher('current_node', String, latch=True, queue_size=1) - self.ce_pub = rospy.Publisher('closest_edges', ClosestEdges, latch=True, queue_size=1) - - self.force_check = True - self.rec_map = False - self.loc_by_topic = [] - self.persist = {} - - self.current_pose=Pose() - self.previous_pose=Pose() - self.previous_pose.position.x=1000 #just give a random big value so this is tested - - #This service returns a list of nodes that have a given tag - self.get_tagged_srv=rospy.Service('topological_localisation/get_nodes_with_tag', topological_navigation_msgs.srv.GetTaggedNodes, self.get_nodes_wtag_cb) - self.loc_pos_srv=rospy.Service('topological_localisation/localise_pose', topological_navigation_msgs.srv.LocalisePose, self.localise_pose_cb) - - rospy.Subscriber('topological_map_2', String, self.MapCallback) - - rospy.loginfo("Localisation waiting for the Topological Map...") - while not self.rec_map : - rospy.sleep(rospy.Duration.from_sec(0.1)) - - self.base_frame = rospy.get_param("~base_frame", "base_link") - - rospy.loginfo("Listening to the tf transform between {} and {}".format(self.tmap_frame, self.base_frame)) - self.listener = tf.TransformListener() - self.rate = rospy.Rate(10.0) - self.PoseCallback() - - rospy.spin() - - - def get_distances_to_pose(self, pose): - """ - This function returns the distance from each waypoint to a pose in an organised way - """ - distances = [] - for node in self.tmap["nodes"]: - dist = get_distance_node_pose_from_tmap2(node, pose) - a = {} - a["node"] = node - a["dist"] = dist - distances.append(a) - - distances = sorted(distances, key=lambda k: k["dist"]) - return distances - - - def get_edge_distances_to_pose(self, pose): - """ - This function returns the distance from each edge to a pose in an organised way - """ - try: - pnts = np.array(self.vectors_start.shape[0] * [[pose.position.x, pose.position.y, 0]]) - distances = pnt2line(pnts, self.vectors_start, self.vectors_end) - closest_edges = [self.dist_edge_ids[index] for index in np.argsort(distances)] - except Exception as e: - rospy.logwarn("Cannot get distance to edges: {}".format(e)) - closest_edges = [] - distances = np.array([]) - - return closest_edges, np.sort(distances) - - - def PoseCallback(self): - """ - This function receives the topo_map to base_link tf transform and localises - the robot in topological space - """ - while not rospy.is_shutdown(): - - try: - now = rospy.Time(0) - self.listener.waitForTransform(self.tmap_frame, self.base_frame, now, rospy.Duration(2.0)) - (trans,rot) = self.listener.lookupTransform(self.tmap_frame, self.base_frame, now) - except Exception as e: - rospy.logerr(e) - self._sleep() - continue - - msg = Pose() - msg.position.x = trans[0] - msg.position.y = trans[1] - msg.position.z = trans[2] - msg.orientation.x = rot[0] - msg.orientation.y = rot[1] - msg.orientation.z = rot[2] - msg.orientation.w = rot[3] - - self.current_pose = msg - if(self.throttle%self.throttle_val==0): - self.distances =[] - self.distances = self.get_distances_to_pose(msg) - closeststr='none' - currentstr='none' - - closest_edges, edge_dists = self.get_edge_distances_to_pose(msg) - if len(closest_edges) > 1: - closest_edges = closest_edges[:2] - edge_dists = edge_dists[:2] - - not_loc=True - if self.loc_by_topic: - for i in self.loc_by_topic: - if not_loc: - if not i['localise_anywhere']: #If it should check the influence zone to localise by topic - test_node = get_node_from_tmap2(self.tmap, i['name']) - if self.point_in_poly(test_node, msg): - not_loc=False - closeststr=str(i['name']) - currentstr=str(i['name']) - self.force_check = False - else: # If not, it is localised!!! - not_loc=False - closeststr=str(i['name']) - currentstr=str(i['name']) - self.force_check = False - else: - self.force_check = True - - if not_loc: - ind = 0 - while not_loc and ind0.10: - val = getattr(msg, item['field']) - - if val == item['val']: - if self.persist.has_key(item['name']): - if self.persist[item['name']] < item['persistency']: - self.persist[item['name']]+=1 - else: - self.persist[item['name']]=0 - - if item['name'] not in [x['name'] for x in self.loc_by_topic] and self.persist[item['name']] < item['persistency']: - self.loc_by_topic.append(item) - self.previous_pose = self.current_pose - - else: - if item['name'] in self.persist: - self.persist.pop(item['name']) - - if item['name'] in [x['name'] for x in self.loc_by_topic]: - self.loc_by_topic.remove(item) - self.previous_pose = self.current_pose - - - def get_nodes_wtag_cb(self,req): - - tlist = [] - rlist=[] - - try: - rospy.wait_for_service('/topological_map_manager2/get_tagged_nodes', timeout=3) - cont = rospy.ServiceProxy('/topological_map_manager2/get_tagged_nodes', topological_navigation_msgs.srv.GetTaggedNodes) - - resp1 = cont(req.tag) - tagnodes = resp1.nodes - - except (rospy.ServiceException) as e: - rospy.logerr("Service call failed: %s"%e) - - ldis = [x["node"]["node"]["name"] for x in self.distances] - for i in ldis: - if i in tagnodes: - tlist.append(i) - rlist.append(tlist) - - return rlist - - - def localise_pose_cb(self, req): - """ - This function gets the node and closest node for a pose - """ - not_loc=True - distances=[] - distances=self.get_distances_to_pose(req.pose) - closeststr='none' - currentstr='none' - - ind = 0 - while not_loc and ind min(p1y,p2y): - if y <= max(p1y,p2y): - if x <= max(p1x,p2x): - if p1y != p2y: - xints = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x - if p1x == p2x or x <= xints: - inside = not inside - p1x,p1y = p2x,p2y - - return inside -################################################################################################################### - - -################################################################################################################### -if __name__ == '__main__': - rospy.init_node('topological_localisation') - wtags=True - argc = len(sys.argv) - if argc > 1: - if '-notags' in sys.argv: - wtags = False - server = TopologicalNavLoc(rospy.get_name(), wtags) -################################################################################################################### \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/localisation2.py b/topological_navigation/topological_navigation/scripts/localisation2.py index 77b0c73a..e4d9603e 100755 --- a/topological_navigation/topological_navigation/scripts/localisation2.py +++ b/topological_navigation/topological_navigation/scripts/localisation2.py @@ -1,641 +1,519 @@ #!/usr/bin/env python +"""Topological localisation node for ROS 2. + +Determines the robot's current topological node and closest node by +subscribing to TF transforms and the topological map topic. Uses a +NetworkX directed graph and a KD-tree spatial index for efficient +O(log n) nearest-neighbour queries. + +Publishers: + ~/closest_node (std_msgs/String) - name of the closest node + ~/closest_node_distance (std_msgs/Float32) - distance to closest node + ~/current_node (std_msgs/String) - node whose influence zone + the robot currently occupies + ~/closest_edges (topological_navigation_msgs/ClosestEdges) + ~/current_node/tag (std_msgs/String) - tag of the current node + +Services: + /topological_localisation/localise_pose (LocalisePose) + +Subscriptions: + /topological_map_2 (std_msgs/String) - YAML-encoded topological map """ -Created on Tue Nov 5 22:02:24 2023 -@author: Geesara Kulathunga (ggeesara@gmail.com) -""" -################################################################################################################### -import sys, json, numpy as np -import rclpy, tf2_ros +import threading + +import numpy as np +import rclpy import yaml -import topological_navigation_msgs.srv -from rclpy.parameter import Parameter + from geometry_msgs.msg import Pose -from std_msgs.msg import String, Float32 -from topological_navigation_msgs.msg import ClosestEdges -from topological_navigation_msgs.srv import GetTaggedNodes, LocalisePose -from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy -from topological_navigation.tmap_utils import * -from topological_navigation.point2line import pnt2line +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.parameter import Parameter +from rclpy.qos import ( + DurabilityPolicy, + HistoryPolicy, + QoSProfile, + ReliabilityPolicy, +) +from std_msgs.msg import Float32, String from tf2_ros import TransformException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener -import time -from std_msgs.msg import Bool -from threading import Thread, Event -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup -from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor -from topological_navigation.scripts.actions_bt import ActionsType - -# this ensures that all the poses and translates -# are float-type and not int-type as there is an -# assertion in ros2 messages (vector3, pose etc.) -# for float-type [x,y,z,w] keys. -class CustomSafeLoader(yaml.SafeLoader): - def construct_mapping(self, node, deep=False): - mapping = super().construct_mapping(node, deep=deep) - - # this can be extended to test the validity of the tmap2 - # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w']: - if key in mapping and isinstance(mapping[key], int): - mapping[key] = float(mapping[key]) - - return mapping -################################################################################################################### +from topological_navigation.tmap_utils import CustomSafeLoader +from topological_navigation.networkx_utils import ( + build_graph_from_tmap, + build_kdtree_from_graph, + determine_closest_node, + determine_current_node, + get_edge_distances_nx, + update_loc_by_topic_nx, +) +from topological_navigation_msgs.msg import ClosestEdges +from topological_navigation_msgs.srv import LocalisePose + +try: + from topological_navigation_msgs.srv import GetTaggedNodes + _HAS_GET_TAGGED_NODES = True +except ImportError: + _HAS_GET_TAGGED_NODES = False + class TopologicalNavLoc(rclpy.node.Node): + """ROS 2 node for topological localisation. + + Determines which topological node the robot currently occupies + (influence-zone check) and which node is closest (KD-tree query). + Publishes the results on latched topics. + """ - def __init__(self, name, wtags): + def __init__(self, name: str, with_tags: bool = True): super().__init__(name) - - self.declare_parameter('LocalisationThrottle', rclpy.Parameter.Type.INTEGER) - self.declare_parameter('OnlyLatched', rclpy.Parameter.Type.BOOL) - self.declare_parameter('base_frame', rclpy.Parameter.Type.STRING) - self.throttle_val = self.get_parameter_or("LocalisationThrottle", Parameter('int', Parameter.Type.INTEGER, 3)).value - self.only_latched = self.get_parameter_or("OnlyLatched", Parameter('bool', Parameter.Type.BOOL, True)).value - self.base_frame = self.get_parameter_or("base_frame", Parameter('str', Parameter.Type.STRING, "base_link")).value + # -- ROS parameters -------------------------------------------------- + self.declare_parameter('LocalisationThrottle', rclpy.Parameter.Type.INTEGER) + self.declare_parameter('OnlyLatched', rclpy.Parameter.Type.BOOL) + self.declare_parameter('base_frame', rclpy.Parameter.Type.STRING) + self.throttle_val = self.get_parameter_or( + "LocalisationThrottle", + Parameter('int', Parameter.Type.INTEGER, 1), + ).value + self.only_latched = self.get_parameter_or( + "OnlyLatched", + Parameter('bool', Parameter.Type.BOOL, True), + ).value + self.base_frame = self.get_parameter_or( + "base_frame", + Parameter('str', Parameter.Type.STRING, "base_link"), + ).value + + # -- Internal state --------------------------------------------------- self.throttle = self.throttle_val - self.node="Unknown" - self.wpstr="Unknown" - self.closest_dist = 10e5-1 - self.cnstr="Unknown" - self.nodetag="Unknown" #current node tag - self.closest_edge_ids = [] - self.closest_edge_dists = [] - self.node_poses = {} - self.current_closest_node_name = "" - - # TODO: remove Temporary arg until tags functionality is MongoDB independent - self.with_tags = wtags - - self.subscribers=[] - - self.qos = QoSProfile(depth=1, - reliability=ReliabilityPolicy.RELIABLE, - history=HistoryPolicy.KEEP_LAST, - durability=DurabilityPolicy.TRANSIENT_LOCAL) - + self.wpstr = "Unknown" + self.closest_dist = 1e6 - 1 + self.cnstr = "Unknown" + self.nodetag = "Unknown" + self.closest_edge_ids: list = [] + self.closest_edge_dists: list = [] + + # NetworkX graph and KD-tree data structures + self._graph = None + self._kdtree = None + self._kdtree_node_names: list = [] + + # Lock protects _graph, _kdtree, _kdtree_node_names, loc_by_topic, + # names_by_topic, and nogos during concurrent map rebuilds. + self._map_lock = threading.Lock() + + self.with_tags = with_tags + + # -- QoS profile (transient-local for late-joining subscribers) ------- + self.qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + # -- Publishers ------------------------------------------------------- self.wp_pub = self.create_publisher(String, 'closest_node', qos_profile=self.qos) - self.wd_pub = self.create_publisher(Float32,'closest_node_distance', qos_profile=self.qos) + self.wd_pub = self.create_publisher(Float32, 'closest_node_distance', qos_profile=self.qos) self.cn_pub = self.create_publisher(String, 'current_node', qos_profile=self.qos) self.ce_pub = self.create_publisher(ClosestEdges, 'closest_edges', qos_profile=self.qos) self.tag_pub = self.create_publisher(String, 'current_node/tag', qos_profile=self.qos) - self.robot_navigation_area_pub = self.create_publisher(String, 'robot_navigation_area', qos_profile=self.qos) - self.force_check = True + # -- Localisation state ----------------------------------------------- self.rec_map = False - self.set_nogos = False - self.loc_by_topic = [] - self.persist = {} - - self.current_pose=Pose() - self.previous_pose=Pose() - self.previous_pose.position.x = 1000.0 #just give a random big value so this is tested - - self.service_get_tagged_done_event = Event() - self.callback_group = ReentrantCallbackGroup() - self.callback_localize_pose = ReentrantCallbackGroup() - self.timer_map_group = ReentrantCallbackGroup() - - self.get_tagged_srv = self.create_service(GetTaggedNodes, '/topological_localisation/get_nodes_with_tag' - , self.get_nodes_wtag_cb, callback_group=self.callback_group) - self.loc_pos_srv = self.create_service(LocalisePose, '/topological_localisation/localise_pose' - , self.localise_pose_cb, callback_group=self.callback_localize_pose) - - self.subs_topmap = self.create_subscription(String, '/topological_map_2' - , self.MapCallback, qos_profile=self.qos, callback_group=self.timer_map_group) - self.subs_topmap # prevent unused variable warning - - self.ACTIONS = ActionsType() - - self.get_logger().info("Localisation waiting for the Topological Map...") + self.loc_by_topic: list = [] + self.names_by_topic: list = [] + self.nogos: list = [] + + self.current_pose = Pose() + + # -- Callback groups -------------------------------------------------- + self._cb_group_localise = ReentrantCallbackGroup() + self._cb_group_map = ReentrantCallbackGroup() + + # -- Service ---------------------------------------------------------- + self.loc_pos_srv = self.create_service( + LocalisePose, + '/topological_localisation/localise_pose', + self.localise_pose_cb, + callback_group=self._cb_group_localise, + ) + + # -- Subscription ----------------------------------------------------- + self.create_subscription( + String, + '/topological_map_2', + self._map_callback, + qos_profile=self.qos, + callback_group=self._cb_group_map, + ) + + # -- Wait for the first map ------------------------------------------- + # _map_callback already rebuilds graph, KD-tree, nogos, and + # loc_by_topic, so we only need to wait for rec_map here. + self.get_logger().info("Waiting for the topological map on /topological_map_2 ...") while rclpy.ok(): rclpy.spin_once(self) - if(self.rec_map): - if self.with_tags: - self.nogos = self.get_no_go_nodes() - self.set_nogos = True - else: - self.nogos=[] - self.get_logger().info("NO GO NODES: %s" %self.nogos) - self.get_logger().info("NODES BY TOPIC: %s" %self.names_by_topic) - self.get_logger().info("Listening to the tf transform between {} and {}".format(self.tmap_frame, self.base_frame)) - break - self.get_logger().warning("Wating for the topological map") - + if self.rec_map: + self.get_logger().info(f"No-go nodes: {self.nogos}") + self.get_logger().info(f"Localise-by-topic nodes: {self.names_by_topic}") + self.get_logger().info( + f"Listening for TF: {self.tmap_frame} -> {self.base_frame}" + ) + break + self.get_logger().warning("Still waiting for the topological map ...") + + # -- TF listener & periodic callback ---------------------------------- self.tf_buffer = Buffer() self.listener = TransformListener(self.tf_buffer, self) - self.rate = self.create_rate(20.0) - - self.create_timer(1.0, self.pose_callback) + self.create_timer(1.0, self._pose_callback) - - def get_distances_to_pose(self, pose): - """ - This function returns the distance from each waypoint to a pose in an organised way - """ - distances = [] - for node in self.tmap["nodes"]: - dist = get_distance_node_pose_from_tmap2(node, pose) - a = {} - a["node"] = node - a["dist"] = dist - distances.append(a) - - distances = sorted(distances, key=lambda k: k["dist"]) - return distances - - - def get_edge_distances_to_pose(self, pose): - """ - This function returns the distance from each edge to a pose in an organised way - """ - try: - pnts = np.array(self.vectors_start.shape[0] * [[pose.position.x, pose.position.y, 0]]) - distances = pnt2line(pnts, self.vectors_start, self.vectors_end) - closest_edges = [self.dist_edge_ids[index] for index in np.argsort(distances)] - except Exception as e: - self.get_logger().warning("Cannot get distance to edges: {}".format(e)) - closest_edges = [] - distances = np.array([]) - - return closest_edges, np.sort(distances) - + def get_edge_distances_to_pose(self, pose: Pose): + """Return edge-ID list and distance array for all edges relative to *pose*. - def pose_callback(self): - """ - This function receives the topo_map to base_link tf transform and localises - the robot in topological space + Uses the NetworkX graph for vectorised edge distance calculations. + Thread-safe: takes a snapshot of the graph under the map lock. + + Returns: + Tuple ``(edge_ids, distances)`` - both may be empty if the graph + is unavailable. """ + with self._map_lock: + graph = self._graph + + if graph is None: + self.get_logger().warning( + "Cannot compute edge distances: graph not yet available" + ) + return [], np.array([]) + + return get_edge_distances_nx(graph, pose, logger=self.get_logger()) + + # ----------------------------------------------------------------- + # Periodic TF-based localisation + # ----------------------------------------------------------------- + + def _pose_callback(self): + """Look up the TF transform and localise the robot in the topological map.""" try: - trans = self.tf_buffer.lookup_transform(self.tmap_frame, self.base_frame, rclpy.time.Time()) - msg = Pose() - msg.position.x = trans.transform.translation.x - msg.position.y = trans.transform.translation.y - msg.position.z = trans.transform.translation.z - msg.orientation.x = trans.transform.rotation.x - msg.orientation.y = trans.transform.rotation.y - msg.orientation.z = trans.transform.rotation.z - msg.orientation.w = trans.transform.rotation.w - - self.current_pose = msg - if(self.throttle%self.throttle_val==0): - self.distances = [] - self.distances = self.get_distances_to_pose(msg) - closeststr='none' - currentstr='none' - nodetag = 'Unknown' - - closest_edges, edge_dists = self.get_edge_distances_to_pose(msg) - if len(closest_edges) > 1: - closest_edges = closest_edges[:2] - edge_dists = edge_dists[:2] - - not_loc = True - if self.loc_by_topic: - for i in self.loc_by_topic: - if not_loc: - if not i['localise_anywhere']: #If it should check the influence zone to localise by topic - test_node = get_node_from_tmap2(self.tmap, i['name']) - if self.point_in_poly(test_node, msg): - not_loc=False - closeststr=str(i['name']) - currentstr=str(i['name']) - self.current_closest_node_name = currentstr - self.force_check = False - else: # If not, it is localised!!! - not_loc=False - closeststr=str(i['name']) - currentstr=str(i['name']) - self.current_closest_node_name = currentstr - self.force_check = False - else: - self.force_check = True - - if not_loc: - ind = 0 - while not_loc and ind 0): - edge_ids_list = self.closest_edge_ids[0] - edge_ids = edge_ids_list.split("_") - if(len(edge_ids) == 2): - if((self.ACTIONS.GOAL_ALIGN_INDEX[0] in edge_ids_list) and (self.ACTIONS.GOAL_ALIGN_GOAL[0] in edge_ids_list)): - robot_nav_area = self.ACTIONS.TRANSITION_INTO_POLYTUNNEL - - if(robot_nav_area is None): - robot_nav_area = self.ACTIONS.OUTSIDE_POLYTUNNEL - robot_current_area_info.data = robot_nav_area - self.robot_navigation_area_pub.publish(robot_current_area_info) - + trans = self.tf_buffer.lookup_transform( + self.tmap_frame, self.base_frame, rclpy.time.Time(), + ) except TransformException as ex: - self.get_logger().warn(f'Could not transform {self.tmap_frame} to {self.base_frame}: {ex}') - pass + self.get_logger().warning( + f"TF lookup failed ({self.tmap_frame} -> {self.base_frame}): {ex}" + ) + return + + msg = Pose() + msg.position.x = trans.transform.translation.x + msg.position.y = trans.transform.translation.y + msg.position.z = trans.transform.translation.z + msg.orientation.x = trans.transform.rotation.x + msg.orientation.y = trans.transform.rotation.y + msg.orientation.z = trans.transform.rotation.z + msg.orientation.w = trans.transform.rotation.w + self.current_pose = msg + + if self.throttle % self.throttle_val != 0: + self.throttle += 1 + return + + # Snapshot data structures under the lock so we work with a + # consistent view even if a map update arrives mid-callback. + with self._map_lock: + graph = self._graph + kdtree = self._kdtree + kdtree_names = self._kdtree_node_names + loc_by_topic = self.loc_by_topic + nogos = list(self.nogos) + names_by_topic = list(self.names_by_topic) + + if graph is None or kdtree is None: + self.get_logger().warning( + "Localisation skipped: graph or KD-tree not ready" + ) + self.throttle += 1 + return + + # Current node (inside influence zone) + currentstr = determine_current_node( + graph, kdtree, kdtree_names, + msg, loc_by_topic, nogos, + ) + + # Closest node by distance + closeststr, closest_dist = determine_closest_node( + kdtree, kdtree_names, graph, + currentstr, nogos, names_by_topic, msg, + ) + + # Closest edges (computed from the *current* pose) + closest_edges, edge_dists = get_edge_distances_nx(graph, msg, logger=self.get_logger()) + if len(closest_edges) > 1: + closest_edges = closest_edges[:2] + edge_dists = edge_dists[:2] + + # Resolve node tag + nodetag = self._get_node_tag(closeststr) + + # Publish + closest_dist = float(np.round(closest_dist, 3)) + self._publish_topics( + closeststr, closest_dist, currentstr, + closest_edges, list(np.round(edge_dists, 3)), nodetag, + ) + self.throttle = 1 - def get_string_msgs(self, str): - msg = String() - msg.data = str - return msg + # ----------------------------------------------------------------- + # Tag helper + # ----------------------------------------------------------------- - def get_float32_msgs(self, num): - msg = Float32() - msg.data = num - return msg + def _get_node_tag(self, node_name: str) -> str: + """Return the first tag string for *node_name*, or ``'Unknown'``. - def publishTopics(self, wpstr, closest_dist, cnstr, closest_edge_ids, closest_edge_dists, nodetag = 'Unknown') : - - def pub_closest_edges(closest_edge_ids, closest_edge_dists): + Uses the NetworkX graph ``meta`` attribute instead of a linear + YAML lookup. + """ + if self._graph is None or node_name not in self._graph.nodes: + return 'Unknown' + meta = self._graph.nodes[node_name].get('meta', {}) + try: + return meta['tag'][0] + except (KeyError, IndexError, TypeError): + return 'Unknown' + + # ----------------------------------------------------------------- + # Message construction helpers + # ----------------------------------------------------------------- + + @staticmethod + def _make_string_msg(text: str) -> String: + msg = String() + msg.data = text + return msg + + @staticmethod + def _make_float32_msg(value: float) -> Float32: + msg = Float32() + msg.data = value + return msg + + # ----------------------------------------------------------------- + # Topic publishing + # ----------------------------------------------------------------- + + def _publish_topics( + self, + wpstr: str, + closest_dist: float, + cnstr: str, + closest_edge_ids: list, + closest_edge_dists: list, + nodetag: str = 'Unknown', + ): + """Publish localisation results, optionally in latched mode.""" + + def _pub_edges(edge_ids, edge_dists): msg = ClosestEdges() - msg.edge_ids = closest_edge_ids - msg.distances = closest_edge_dists + msg.edge_ids = edge_ids + msg.distances = edge_dists self.ce_pub.publish(msg) - + if len(set(closest_edge_dists)) == 1: closest_edge_ids.sort() - - if self.only_latched : + + if self.only_latched: if self.wpstr != wpstr: - self.wp_pub.publish(self.get_string_msgs(wpstr)) + self.wp_pub.publish(self._make_string_msg(wpstr)) if self.closest_dist != closest_dist: - self.wd_pub.publish(self.get_float32_msgs(closest_dist)) + self.wd_pub.publish(self._make_float32_msg(closest_dist)) if self.cnstr != cnstr: - self.cn_pub.publish(self.get_string_msgs(cnstr)) + self.cn_pub.publish(self._make_string_msg(cnstr)) if self.nodetag != nodetag: - self.tag_pub.publish(self.get_string_msgs(nodetag)) - if self.closest_edge_ids != closest_edge_ids \ - or self.closest_edge_dists != closest_edge_dists: - pub_closest_edges(closest_edge_ids, closest_edge_dists) + self.tag_pub.publish(self._make_string_msg(nodetag)) + if (self.closest_edge_ids != closest_edge_ids + or self.closest_edge_dists != closest_edge_dists): + _pub_edges(closest_edge_ids, closest_edge_dists) else: - self.wp_pub.publish(self.get_string_msgs(wpstr)) - self.wd_pub.publish(self.get_float32_msgs(closest_dist)) - self.cn_pub.publish(self.get_string_msgs(cnstr)) - self.tag_pub.publish(self.get_string_msgs(nodetag)) - pub_closest_edges(closest_edge_ids, closest_edge_dists) - + self.wp_pub.publish(self._make_string_msg(wpstr)) + self.wd_pub.publish(self._make_float32_msg(closest_dist)) + self.cn_pub.publish(self._make_string_msg(cnstr)) + self.tag_pub.publish(self._make_string_msg(nodetag)) + _pub_edges(closest_edge_ids, closest_edge_dists) + self.wpstr = wpstr self.closest_dist = closest_dist self.cnstr = cnstr self.nodetag = nodetag self.closest_edge_ids = closest_edge_ids self.closest_edge_dists = closest_edge_dists - + # self.get_logger().info( + # f"Published: closest_node='{wpstr}', closest_dist={closest_dist}, " + # f"current_node='{cnstr}', nodetag='{nodetag}', " + # f"closest_edges={closest_edge_ids} (dists: {closest_edge_dists})" + # ) - def MapCallback(self, msg): - """ - This function receives the Topological Map - """ - if(self.rec_map is False): - self.names_by_topic = [] - self.nodes_by_topic = [] - self.nogos = [] - - # self.tmap = json.loads(msg.data) - self.tmap = yaml.load( msg.data, Loader=CustomSafeLoader) - self.tmap_frame = self.tmap["transformation"]["child"] - self.get_logger().info("Localisation received the Topological Map") - - self.get_edge_vectors() - self.update_loc_by_topic() - - self.get_logger().info("Creating localise by topic subscribers...") - - for i in self.subscribers: - del i - self.subscribers = [] - # for j in self.nodes_by_topic: - # # Append to list to keep the instance alive and the subscriber active. - # self.subscribers.append(LocaliseByTopicSubscriber( - # topic=j['topic'], - # callback=self.Callback, - # callback_args=j - # )) - # # Calling instance of class to start subsribing thread. - # self.subscribers[-1]() - self.rec_map = True - - - def get_edge_vectors(self): - - node_poses = {} - for node in self.tmap["nodes"]: - node_poses[node["node"]["name"]] = node["node"]["pose"] - - self.dist_edge_ids = [] - vectors_start = [] - vectors_end = [] - - for node in self.tmap["nodes"]: - orig_pose = node_poses[node["node"]["name"]] - start = [orig_pose["position"]["x"], orig_pose["position"]["y"], 0] - - for edge in node["node"]["edges"]: - dest_pose = node_poses[edge["node"]] - - if node["node"]["name"] != edge["node"]: - self.dist_edge_ids.append(edge["edge_id"]) - end = [dest_pose["position"]["x"], dest_pose["position"]["y"], 0] - - vectors_start.append(start) - vectors_end.append(end) - else: - self.get_logger().error("Cannot get distance to edge {}: Destination is equal to origin".format(edge["edge_id"])) - - self.vectors_start = np.array(vectors_start) - self.vectors_end = np.array(vectors_end) + # ----------------------------------------------------------------- + # Map reception + # ----------------------------------------------------------------- + def _map_callback(self, msg): + """Handle incoming topological map - build graph and KD-tree. - def update_loc_by_topic(self): - """ - This function updates the localisation by topic variables + This callback is safe to invoke repeatedly: on every update the + graph, KD-tree, topic-based localisation config, and no-go nodes + are fully rebuilt so that localisation keeps working after node + positions, edges, or properties change at runtime. """ - for i in self.tmap['nodes']: - if i['node']['localise_by_topic']: - a= json.loads(i['node']['localise_by_topic']) - a['name'] = i['node']['name'] - if not a.has_key('localise_anywhere'): - a['localise_anywhere']=True - if not a.has_key('persistency'): - a['persistency']=10 - self.nodes_by_topic.append(a) - self.names_by_topic.append(a['name']) - - - def Callback(self, msg, item): - #needed for not checking the localise by topic when the robot hasn't moved and making sure it does when the new - #position is close (<10) to the last one it was detected - if self.force_check: - dist = 1.0 + is_update = self.rec_map + label = "Updated" if is_update else "Received" + + self.tmap = yaml.load(msg.data, Loader=CustomSafeLoader) + self.tmap_frame = self.tmap["transformation"]["topo_frame_id"] + self.get_logger().info(f"{label} the topological map") + + # Build new graph and KD-tree in local variables first so the + # live data structures remain consistent until the swap. + new_graph = build_graph_from_tmap(self.tmap, logger=self.get_logger()) + if new_graph is None: + self.get_logger().error("Failed to build the NetworkX graph – aborting map load") + return + self.get_logger().info( + f"Graph built: {new_graph.number_of_nodes()} nodes, " + f"{new_graph.number_of_edges()} edges" + ) + + new_kdtree, new_kdtree_node_names = build_kdtree_from_graph( + new_graph, logger=self.get_logger(), + ) + if new_kdtree is None: + self.get_logger().error("Failed to build KD-tree – aborting map load") + return + self.get_logger().info( + f"KD-tree built with {len(new_kdtree_node_names)} nodes" + ) + + # Topic-based localisation config + new_loc_by_topic, new_names_by_topic = update_loc_by_topic_nx( + new_graph, logger=self.get_logger(), + ) + + # Re-query no-go nodes (may have changed with the map update) + if self.with_tags: + new_nogos = self._get_no_go_nodes() else: - dist = get_distance(self.current_pose, self.previous_pose) - - if dist>0.10: - val = getattr(msg, item['field']) - - if val == item['val']: - if self.persist.has_key(item['name']): - if self.persist[item['name']] < item['persistency']: - self.persist[item['name']]+=1 - else: - self.persist[item['name']]=0 - - if item['name'] not in [x['name'] for x in self.loc_by_topic] and self.persist[item['name']] < item['persistency']: - self.loc_by_topic.append(item) - self.previous_pose = self.current_pose - - else: - if item['name'] in self.persist: - self.persist.pop(item['name']) - - if item['name'] in [x['name'] for x in self.loc_by_topic]: - self.loc_by_topic.remove(item) - self.previous_pose = self.current_pose - - - def get_nodes_wtag_cb(self, req, res): - res.nodes = [] - try: - cli = self.create_client(GetTaggedNodes, '/topological_map_manager2/get_tagged_nodes') - if not cli.wait_for_service(timeout_sec=3.0): - self.get_logger().warning('/topological_map_manager2/get_tagged_nodes service not available') - return res - self.service_get_tagged_done_event.clear() - event = Event() - def done_callback(future): - nonlocal event - event.set() - - cli_req = GetTaggedNodes.Request() - cli_req.tag = req.tag - cli_future = cli.call_async(req) - cli_future.add_done_callback(done_callback) - event.wait() - get_prediction = cli_future.result() - # self.get_logger().info(get_prediction) - tagnodes = get_prediction.nodes - # for x in self.distances: - ldis = [x["node"]["node"]["name"] for x in self.distances] - for i in ldis: - if i in tagnodes: - res.nodes.append(i) - return res - except (Exception) as e: - self.get_logger().error("Service call /topological_map_manager2/get_tagged_nodes failed: %s"%e) - return res - + new_nogos = [] + + # Atomically swap all data structures under the lock so that + # _pose_callback never sees a half-rebuilt state. + with self._map_lock: + self._graph = new_graph + self._kdtree = new_kdtree + self._kdtree_node_names = new_kdtree_node_names + self.loc_by_topic = new_loc_by_topic + self.names_by_topic = new_names_by_topic + self.nogos = new_nogos + + self.rec_map = True + if is_update: + self.get_logger().info( + "Map update applied – graph, KD-tree, no-go nodes, " + "and topic-based localisation refreshed" + ) + + # ----------------------------------------------------------------- + # Localise-pose service + # ----------------------------------------------------------------- def localise_pose_cb(self, req, res): - """ - This function gets the node and closest node for a pose - """ - not_loc = True - distances = [] - distances = self.get_distances_to_pose(req.pose) - closeststr='none' - currentstr='none' - - ind = 0 - while not_loc and ind list: + """Query the map manager for 'no-go' tagged nodes. + + Returns an empty list when the GetTaggedNodes service type is + unavailable (e.g. not defined in topological_navigation_msgs). """ - cli = self.create_client(GetTaggedNodes, '/topological_map_manager2/get_tagged_nodes') - if not cli.wait_for_service(timeout_sec=3.0): - self.get_logger().warning('/topological_map_manager2/get_tagged_nodes service not available') + if not _HAS_GET_TAGGED_NODES: + self.get_logger().info( + "GetTaggedNodes service not available in this build; " + "no-go nodes disabled" + ) return [] - else: - cli_req = GetTaggedNodes.Request() - future = cli.call_async(cli_req) - rclpy.spin_until_future_complete(self, future) - get_prediction = future.result() - return get_prediction.nodes - - def point_in_poly(self,node,pose): - - x=pose.position.x-node["node"]["pose"]["position"]["x"] - y=pose.position.y-node["node"]["pose"]["position"]["y"] - - n = len(node["node"]["verts"]) - inside = False - - p1x = node["node"]["verts"][0]["x"] - p1y = node["node"]["verts"][0]["y"] - for i in range(n+1): - p2x = node["node"]["verts"][i % n]["x"] - p2y = node["node"]["verts"][i % n]["y"] - if y > min(p1y,p2y): - if y <= max(p1y,p2y): - if x <= max(p1x,p2x): - if p1y != p2y: - xints = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x - if p1x == p2x or x <= xints: - inside = not inside - p1x,p1y = p2x,p2y - - return inside -################################################################################################################### - -# class LocaliseByTopicSubscriber(object): -# """ -# Helper class for localise by topic subcription. Callable to start subsriber -# thread. -# """ -# def __init__(self, topic, callback, callback_args): - -# self.topic = rclpy.get_namespace() + topic -# self.callback = callback -# self.callback_args = callback_args -# self.sub = None -# self.t = None - + cli = self.create_client( + GetTaggedNodes, + '/topological_map_manager2/get_tagged_nodes', + ) + if not cli.wait_for_service(timeout_sec=3.0): + self.get_logger().warning( + "Service /topological_map_manager2/get_tagged_nodes unavailable; " + "assuming no no-go nodes" + ) + return [] -# def get_topic_type(self, topic, blocking=False): -# """ -# Get the topic type. -# !!! Overriden from rostopic !!! - -# :param topic: topic name, ``str`` -# :param blocking: (default False) block until topic becomes available, ``bool`` - -# :returns: topic type, real topic name and fn to evaluate the message instance -# if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)`` -# :raises: :exc:`ROSTopicException` If master cannot be contacted -# """ -# topic_type, real_topic, msg_eval = rostopic._get_topic_type(topic) -# if topic_type: -# return topic_type, real_topic, msg_eval -# elif blocking: -# sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n"%topic) -# while not rclpy.is_shutdown(): -# topic_type, real_topic, msg_eval = rostopic._get_topic_type(topic) -# if topic_type: -# return topic_type, real_topic, msg_eval -# else: -# rostopic._sleep(10.) # Change! Waiting for 10 seconds instead of 0.1 to reduce load - -# return None, None, None - - -# def __call__(self): -# """ -# When called start a new thread that waits for the topic type and then -# subscribes. This is therefore non blocking and waits in the background. -# """ -# self.t = Thread(target=self.subscribe) -# self.t.start() - - -# def subscribe(self): -# """ -# Get the topic type and subscribe to topic. Subscriber is kept alive as -# long as the instance of the class is alive. -# """ -# rostopic.get_topic_type = self.get_topic_type # Monkey patch -# topic_type = rostopic.get_topic_class(self.topic, True)[0] -# self.get_logger().info("Subscribing to %s" % self.topic) -# self.sub = rclpy.Subscriber( -# name=self.topic, -# data_class=topic_type, -# callback=self.callback, -# callback_args=self.callback_args -# ) - - -# def close(self): -# self.sub.unregister() - + future = cli.call_async(GetTaggedNodes.Request()) + rclpy.spin_until_future_complete(self, future) + return list(future.result().nodes) -# def __del__(self): -# self.close() -# ################################################################################################################### +# ===================================================================== +# Entry point +# ===================================================================== -################################################################################################################### def main(args=None): rclpy.init(args=args) - wtags = True - node = TopologicalNavLoc('topological_localisation', wtags) + node = TopologicalNavLoc('topological_localisation', with_tags=True) executor = MultiThreadedExecutor() executor.add_node(node) try: executor.spin() except KeyboardInterrupt: - node.get_logger().info('shutting down localisation node\n') + node.get_logger().info("Shutting down localisation node") node.destroy_node() rclpy.shutdown() -if __name__ == '__main__' : +if __name__ == '__main__': main() diff --git a/topological_navigation/topological_navigation/scripts/manual_edge_predictions.py b/topological_navigation/topological_navigation/scripts/manual_edge_predictions.py deleted file mode 100755 index ec98ffe1..00000000 --- a/topological_navigation/topological_navigation/scripts/manual_edge_predictions.py +++ /dev/null @@ -1,80 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import yaml - -import actionlib -from actionlib_msgs.msg import * - -from topological_navigation_msgs.srv import * -from topological_navigation_msgs.msg import * - - -def usage(): - print("Run with a yaml file containing edge prediction information:") - print("\t rosrun topological_navigation manual_edge_predictions.py ") - -# Examaple yaml format: -# - edge_id: WayPoint0_WayPoint1 -# duration: 1.0 -# success_probability: 1.0 -# - edge_id: WayPoint0_WayPoint4 -# duration: 1.0 -# success_probability: 1.0 -# - edge_id: WayPoint1_WayPoint2 -# duration: 1.0 -# success_probability: 1.0 - - -class YamlConfiguredPredictor(object): - - def __init__(self, config) : - - self.response = PredictEdgeStateResponse() - - for edge in config: - self.response.edge_ids.append(edge['edge_id']) - self.response.probs.append(edge['success_probability']) - self.response.durations.append(rospy.Duration(edge['duration'])) - - name= rospy.get_name() - action_name = name+'/build_temporal_model' - - #Creating Action Server - rospy.loginfo("Creating action server.") - self._as = actionlib.SimpleActionServer(action_name, BuildTopPredictionAction, execute_cb = self.build_callback, auto_start = False) - self._as.start() - - - self.predict_srv=rospy.Service('topological_prediction/predict_edges', PredictEdgeState, self.predict_edge_cb) - rospy.loginfo("All Done ...") - rospy.spin() - - - def predict_edge_cb(self, req): - return self.response - - - """ - Build predictions from data. Or not. - - Does nothing for this version. - """ - def build_callback(self, goal): - self._as.set_succeeded() - - -if __name__ == '__main__': - rospy.init_node('topological_prediction') - - if len(sys.argv) != 2: - usage() - sys.exit(1) - config_file = sys.argv[1] - - - with open(config_file, "r") as f: - config = yaml.load(f.read()) - - server = YamlConfiguredPredictor(config) diff --git a/topological_navigation/topological_navigation/scripts/manual_topomapping.py b/topological_navigation/topological_navigation/scripts/manual_topomapping.py index e3858388..8c54ca25 100644 --- a/topological_navigation/topological_navigation/scripts/manual_topomapping.py +++ b/topological_navigation/topological_navigation/scripts/manual_topomapping.py @@ -1,419 +1,593 @@ #! /usr/bin/env python3 -""" - This node is used to manually create a topomap live using the robot driven using - a controller, buttons are pressed to add/remove nodes and to generate - the topomap. - Original Author: Sergi Molina 2022 - ROS1 - Maintainer: Ibrahim Hroob 2024 - ROS2 support +"""Manual topological mapping node for ROS 2. + +Drive a robot with a joystick and press buttons to add / remove nodes. +Edges are created automatically between each new node and its closest +existing neighbour (bidirectional). Node orientations are set to face +along their outgoing edge direction. + +The topological map is published on ``/topological_map_2`` every time a +node is added or removed so that downstream nodes (localisation, +navigation, visualiser) pick up the changes immediately. + +The map follows the ``test_simple_tmap2.yaml`` format with a single +``navigate_to_pose`` action definition. + +Original Author: Sergi Molina 2022 - ROS1 +Maintainer: Ibrahim Hroob 2024 - ROS2 support """ +import math import os import yaml import datetime import numpy as np -from math import sqrt from copy import deepcopy import rclpy import rclpy.duration -from rclpy import Parameter +from rclpy import Parameter from rclpy.node import Node +from rclpy.qos import ( + DurabilityPolicy, + HistoryPolicy, + QoSProfile, + ReliabilityPolicy, +) from nav_msgs.msg import Odometry from geometry_msgs.msg import Pose from sensor_msgs.msg import Joy, Imu +from std_msgs.msg import String from std_srvs.srv import Trigger from visualization_msgs.msg import Marker, MarkerArray from builtin_interfaces.msg import Duration -from tf_transformations import euler_from_quaternion +from tf_transformations import quaternion_from_euler from ament_index_python.packages import get_package_share_directory class RobotTmapping(Node): + """Joystick-driven topological mapping node. + + Nodes are added / removed via joystick buttons. Edges between the + two closest nodes are created automatically (bidirectional). Node + orientations are set to face the direction of their first outgoing + edge. The full map is published to ``/topological_map_2`` on every + change. + """ def __init__(self): super().__init__('manual_tmapping_node') - # Params - self.declare_parameter( 'tmap' , Parameter.Type.STRING ) - self.declare_parameter( 'tmap_dir' , Parameter.Type.STRING ) - self.declare_parameter( 'site_name' , Parameter.Type.STRING ) - self.declare_parameter( "insert_map" , Parameter.Type.BOOL ) - self.declare_parameter( 'inorder_cluster' , Parameter.Type.BOOL ) #use_inorder_clustering - self.declare_parameter( 'dbscan_eps' , Parameter.Type.DOUBLE ) - self.declare_parameter( 'node_thresh' , Parameter.Type.DOUBLE ) #closest_node_threshold - self.declare_parameter( 'min_num_rows' , Parameter.Type.INTEGER ) - # Joystick buttons - self.declare_parameter( 'lock_btn' , Parameter.Type.INTEGER ) #lock_button_index - self.declare_parameter( 'add_btn' , Parameter.Type.INTEGER ) #add_node_button_index - self.declare_parameter( 'remove_btn' , Parameter.Type.INTEGER ) #remove_node_button_index - self.declare_parameter( 'gen_map_btn' , Parameter.Type.INTEGER ) #generate_tmap_button_index - # Topics - self.declare_parameter( 'topic_joy' , Parameter.Type.STRING ) - self.declare_parameter( 'topic_pose' , Parameter.Type.STRING ) - self.declare_parameter( 'topic_imu' , Parameter.Type.STRING ) - - self.pointset = self.get_parameter_or('tmap' , Parameter('str' , Parameter.Type.STRING, '') ).value - self.tmap_dir = self.get_parameter_or('tmap_dir' , Parameter('str' , Parameter.Type.STRING, '') ).value - self.site_name = self.get_parameter_or('site_name' , Parameter('str' , Parameter.Type.STRING, '') ).value - self.insert_map = self.get_parameter_or('insert_map' , Parameter('bool' , Parameter.Type.BOOL, False) ).value - self.inorder_cluster = self.get_parameter_or('inorder_cluster' , Parameter('bool' , Parameter.Type.BOOL, False) ).value - self.dbscan_eps = self.get_parameter_or('dbscan_eps' , Parameter('double', Parameter.Type.DOUBLE, 4.0) ).value - self.node_thresh = self.get_parameter_or('node_thresh' , Parameter('double', Parameter.Type.DOUBLE, 0.5) ).value - self.min_num_rows = self.get_parameter_or('min_num_rows' , Parameter('int' , Parameter.Type.INTEGER, 2) ).value - self.lock_btn = self.get_parameter_or('lock_btn' , Parameter('int' , Parameter.Type.INTEGER, 6) ).value - self.add_btn = self.get_parameter_or('add_btn' , Parameter('int' , Parameter.Type.INTEGER, 1) ).value - self.remove_btn = self.get_parameter_or('remove_btn' , Parameter('int' , Parameter.Type.INTEGER, 2) ).value - self.gen_map_btn = self.get_parameter_or('gen_map_btn' , Parameter('int' , Parameter.Type.INTEGER, 3) ).value - self.topic_joy = self.get_parameter_or('topic_joy' , Parameter('str' , Parameter.Type.STRING, '/joy') ).value - self.topic_pose = self.get_parameter_or('topic_pose' , Parameter('str' , Parameter.Type.STRING, '/gps_base/odometry') ).value - self.topic_imu = self.get_parameter_or('topic_imu' , Parameter('str' , Parameter.Type.STRING, '/gps_base/yaw') ).value - - - - # Disable until toponav 2 supports mongodb storage of new map. - self.insert_map = False - - # Variables + # -- ROS parameters --------------------------------------------------- + self.declare_parameter('tmap', Parameter.Type.STRING) + self.declare_parameter('tmap_dir', Parameter.Type.STRING) + self.declare_parameter('site_name', Parameter.Type.STRING) + self.declare_parameter('node_thresh', Parameter.Type.DOUBLE) + # Joystick buttons + self.declare_parameter('lock_btn', Parameter.Type.INTEGER) + self.declare_parameter('add_btn', Parameter.Type.INTEGER) + self.declare_parameter('remove_btn', Parameter.Type.INTEGER) + self.declare_parameter('gen_map_btn', Parameter.Type.INTEGER) + # Topics + self.declare_parameter('topic_joy', Parameter.Type.STRING) + self.declare_parameter('topic_pose', Parameter.Type.STRING) + self.declare_parameter('topic_imu', Parameter.Type.STRING) + + self.pointset = self.get_parameter_or( + 'tmap', Parameter('str', Parameter.Type.STRING, '')).value + self.tmap_dir = self.get_parameter_or( + 'tmap_dir', Parameter('str', Parameter.Type.STRING, '')).value + self.site_name = self.get_parameter_or( + 'site_name', Parameter('str', Parameter.Type.STRING, '')).value + self.node_thresh = self.get_parameter_or( + 'node_thresh', Parameter('double', Parameter.Type.DOUBLE, 0.5)).value + self.lock_btn = self.get_parameter_or( + 'lock_btn', Parameter('int', Parameter.Type.INTEGER, 6)).value + self.add_btn = self.get_parameter_or( + 'add_btn', Parameter('int', Parameter.Type.INTEGER, 1)).value + self.remove_btn = self.get_parameter_or( + 'remove_btn', Parameter('int', Parameter.Type.INTEGER, 2)).value + self.gen_map_btn = self.get_parameter_or( + 'gen_map_btn', Parameter('int', Parameter.Type.INTEGER, 3)).value + self.topic_joy = self.get_parameter_or( + 'topic_joy', Parameter('str', Parameter.Type.STRING, '/joy')).value + self.topic_pose = self.get_parameter_or( + 'topic_pose', Parameter('str', Parameter.Type.STRING, '/gps_base/odometry')).value + self.topic_imu = self.get_parameter_or( + 'topic_imu', Parameter('str', Parameter.Type.STRING, '/gps_base/yaw')).value + + # -- Internal state ---------------------------------------------------- self.node_id = 0 - self.nodes = [] # A list of node Pose msgs that will be used to generate the tmap + self.nodes = [] # list of [id, Pose] self.previous_button = None - - # Load templates + self.robot_pose_msg = Pose() + self.robot_imu_msg = None + + # -- Load templates from package config -------------------------------- toponav_dir = get_package_share_directory('topological_navigation') config_dir = os.path.join(toponav_dir, 'config') - self.template_node = self.load_yaml(os.path.join(config_dir, 'template_node_2.yaml')) - self.template_edge = self.load_yaml(os.path.join(config_dir, 'template_edge.yaml')) - - # Services - self.create_service(Trigger, '/tmapping_robot/save_waypoints', self.save_waypoints_srv_cb) - self.create_service(Trigger, '/tmapping_robot/save_map', self.generate_tmap_srv_cb) - - # Subscribers - self.create_subscription(Joy , self.topic_joy , self.joy_cb , 10) - - self.create_subscription(Imu , self.topic_pose, self.robot_imu_cb , 10) + self.template_node = self._load_yaml( + os.path.join(config_dir, 'template_node.yaml')) + self.template_edge = self._load_yaml( + os.path.join(config_dir, 'template_edge.yaml')) + self.template_action = self._load_yaml( + os.path.join(config_dir, 'template_action.yaml')) + + # -- QoS (transient-local, same as map_manager2) ----------------------- + self._latched_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + # -- Publishers -------------------------------------------------------- + self.node_vis_pub = self.create_publisher( + MarkerArray, '/tmapping_nodes', 10) + self.map_pub = self.create_publisher( + String, '/topological_map_2', self._latched_qos) + + # -- Services ---------------------------------------------------------- + self.create_service( + Trigger, '/tmapping_robot/save_waypoints', + self.save_waypoints_srv_cb) + self.create_service( + Trigger, '/tmapping_robot/save_map', + self.generate_tmap_srv_cb) + + # -- Subscribers ------------------------------------------------------- + self.create_subscription(Joy, self.topic_joy, self.joy_cb, 10) + self.create_subscription(Imu, self.topic_imu, self.robot_imu_cb, 10) + self.create_subscription( + Odometry, self.topic_pose, self.robot_pose_cb, 10) + + # -- Create save directory --------------------------------------------- + if self.tmap_dir and not os.path.exists(self.tmap_dir): + self.get_logger().info(f"Creating tmap_dir: {self.tmap_dir}") + os.makedirs(self.tmap_dir) - self.create_subscription(Odometry, self.topic_pose, self.robot_pose_cb, 10) + # -- Initialise map and load existing nodes ---------------------------- + self._init_topomap() + self._load_existing_nodes() + self.get_logger().info( + f"There are {len(self.nodes)} nodes in the map.") + + # ================================================================== + # YAML helpers + # ================================================================== + + @staticmethod + def _load_yaml(filename): + """Load a YAML file and return the parsed data.""" + with open(filename, 'r') as fh: + return yaml.safe_load(fh) + + @staticmethod + def _save_yaml(filename, data): + """Dump *data* as YAML to *filename*.""" + with open(filename, 'w') as fh: + yaml.safe_dump(data, fh, default_flow_style=False) + + # ================================================================== + # Map initialisation (test_simple_tmap2.yaml structure) + # ================================================================== + + def _init_topomap(self): + """Create an empty topomap dict using loaded template files.""" + self.topomap = { + "meta": { + "last_updated": self._get_time(), + }, + "metric_map": self.site_name, + "name": self.pointset, + "pointset": self.pointset, + "transformation": { + "topo_frame_id": self.site_name or "map", + "parent": "map", + "rotation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + }, + "definitions": deepcopy( + self.template_action.get("definitions", {}) + ), + "actions": deepcopy( + self.template_action.get("actions", {}) + ), + "nodes": [], + } - # Publishers - self.node_vis_pub = self.create_publisher(MarkerArray, '/tmapping_nodes', 10) + # ================================================================== + # Load existing nodes from disk + # ================================================================== - # Create save folder if is does not exist - if not os.path.exists(self.tmap_dir): - self.get_logger().info("Creating tmap_dir: %s" % (self.tmap_dir)) - os.makedirs(self.tmap_dir) - - # Load existing nodes from tmap_dir if there are any - self.get_tmap_nodes() + def _load_existing_nodes(self): + """Reload node list from tmap file on disk (if it exists).""" + if not self.tmap_dir or not self.pointset: + return + tmap_path = os.path.join(self.tmap_dir, self.pointset) + if not os.path.exists(tmap_path): + return + existing = self._load_yaml(tmap_path) + if existing is None or "nodes" not in existing: + return - self.robot_imu_msg = None + self.get_logger().info(f"Loading existing tmap: {tmap_path}") + for nd in existing["nodes"]: + pose = Pose() + pose.position.x = nd["node"]["pose"]["position"]["x"] + pose.position.y = nd["node"]["pose"]["position"]["y"] + pose.position.z = nd["node"]["pose"]["position"].get("z", 0.0) + pose.orientation.x = nd["node"]["pose"]["orientation"]["x"] + pose.orientation.y = nd["node"]["pose"]["orientation"]["y"] + pose.orientation.z = nd["node"]["pose"]["orientation"]["z"] + pose.orientation.w = nd["node"]["pose"]["orientation"]["w"] + self.nodes.append([self.node_id, pose]) + self.node_id += 1 - def load_yaml(self, filename): - with open(filename, 'r') as f: - return yaml.safe_load(f) - - - def save_yaml(self, filename, data, dfs=True): - with open(filename, 'w') as f: - return yaml.safe_dump(data, f, default_flow_style=dfs) - - - def get_tmap_nodes(self): - tmap = os.path.join(self.tmap_dir, self.pointset) - self.get_logger().info("Getting current nodes from tmap file") - self.get_logger().info(tmap) - - if os.path.exists(tmap): - self.topomap = self.load_yaml(tmap) - self.get_logger().info(f"Existing tmap: {tmap}") - if self.topomap is not None: - for node in self.topomap["nodes"]: - pose = Pose() - pose.position.x = node["node"]["pose"]["position"]["x"] - pose.position.y = node["node"]["pose"]["position"]["y"] - pose.orientation.x = node["node"]["pose"]["orientation"]["x"] - pose.orientation.y = node["node"]["pose"]["orientation"]["y"] - pose.orientation.z = node["node"]["pose"]["orientation"]["z"] - pose.orientation.w = node["node"]["pose"]["orientation"]["w"] - self.nodes.append([self.node_id, pose]) - self.node_id += 1 - self.update_node_markers() - else: - self.init_map() - else: - self.init_map() - self.get_logger().info(f'There are {len(self.nodes)} nodes in the existing map.') + # Rebuild the full topomap with edges and publish + self._rebuild_topomap() + self._update_node_markers() + # ================================================================== + # Subscriber callbacks + # ================================================================== def robot_pose_cb(self, msg): + """Store the latest robot pose from Odometry.""" self.robot_pose_msg = msg.pose.pose - - #Please note, this is an ugly hack, the better way to do it is via topics sync - if self.robot_imu_msg != None: + if self.robot_imu_msg is not None: self.robot_pose_msg.orientation = self.robot_imu_msg def robot_imu_cb(self, msg): + """Store the latest IMU orientation.""" self.robot_imu_msg = msg.orientation - def joy_cb(self, msg): + """Handle joystick button presses.""" buttons = msg.buttons + if not buttons[self.lock_btn]: + self.previous_button = None + return - if buttons[self.lock_btn]: - if buttons[self.add_btn]: - if self.previous_button is None or self.add_btn != self.previous_button: - self.add_node() - self.previous_button = self.add_btn - elif buttons[self.remove_btn]: - if self.remove_btn != self.previous_button: - self.remove_node() - self.previous_button = self.remove_btn - elif buttons[self.gen_map_btn]: - if self.gen_map_btn != self.previous_button: - self.generate_tmap() - self.previous_button = self.gen_map_btn - else: - self.previous_button = None + if buttons[self.add_btn]: + if self.previous_button != self.add_btn: + self.add_node() + self.previous_button = self.add_btn + elif buttons[self.remove_btn]: + if self.previous_button != self.remove_btn: + self.remove_node() + self.previous_button = self.remove_btn + elif buttons[self.gen_map_btn]: + if self.previous_button != self.gen_map_btn: + self.generate_tmap() + self.previous_button = self.gen_map_btn + else: + self.previous_button = None + # ================================================================== + # Add / remove nodes + # ================================================================== def add_node(self): + """Add a node at the robot's current pose if not too close to an existing one.""" self.get_logger().info("Adding node") - pose = self.robot_pose_msg - dist, nearest_id, _ = self.get_nearest_node(pose) - if not dist or dist > self.node_thresh: - self.nodes.append([self.node_id, pose]) - self.get_logger().info(f"New node: {pose}") - self.get_logger().info(f"Num nodes: {len(self.nodes)}") - self.update_node_markers() - self.node_id += 1 - else: + pose = deepcopy(self.robot_pose_msg) + dist, _, _ = self._get_nearest_node(pose) + if dist is not None and dist <= self.node_thresh: self.get_logger().info("Too close to an existing node, won't add one!") + return + + self.nodes.append([self.node_id, pose]) + self.get_logger().info( + f"New node{self.node_id}: x={pose.position.x:.2f}, " + f"y={pose.position.y:.2f} (total: {len(self.nodes)})") + self.node_id += 1 + self._rebuild_topomap() + self._update_node_markers() def remove_node(self): + """Remove the nearest node to the robot (within 5 m).""" self.get_logger().info("Removing nearest node") pose = self.robot_pose_msg - dist, nearest_id, ind = self.get_nearest_node(pose) - if not dist: - self.get_logger().info("List is empty!") - else: - if dist > 5: - self.get_logger().info("Not near any nodes so not removing any!") - else: - self.remove_marker(nearest_id) - self.nodes.pop(ind) - self.get_logger().info(f"Num nodes: {len(self.nodes)}") - - - def update_node_markers(self): - diameter = 0.7 - marker_array = MarkerArray() - - for node in self.nodes: - node_vis = Marker() - node_vis.type = Marker.SPHERE - node_vis.header.frame_id = 'map' - node_vis.id = node[0] - node_vis.pose.position.x = node[1].position.x - node_vis.pose.position.y = node[1].position.y - node_vis.pose.position.z = node[1].position.z + diameter / 2 - node_vis.pose.orientation = node[1].orientation - node_vis.scale.x = diameter - node_vis.scale.y = diameter - node_vis.scale.z = diameter - node_vis.color.a = 1.0 - node_vis.color.r = 1.0 - node_vis.color.g = 0.6 - node_vis.color.b = 0.0 - - node_vis.lifetime = Duration() # Initialize the Duration message - - marker_array.markers.append(node_vis) - - self.node_vis_pub.publish(marker_array) + dist, nearest_id, ind = self._get_nearest_node(pose) + if dist is None: + self.get_logger().info("Node list is empty!") + return + if dist > 5.0: + self.get_logger().info("Not near any nodes – not removing any!") + return + self._remove_marker(nearest_id) + self.nodes.pop(ind) + self.get_logger().info(f"Removed node (total: {len(self.nodes)})") + + self._rebuild_topomap() + self._update_node_markers() + + # ================================================================== + # Nearest-node helpers + # ================================================================== + + def _get_nearest_node(self, pose): + """Return (distance, id, index) of the closest node, or (None, None, None).""" + best_dist = None + best_id = None + best_ind = None + for i, (nid, npose) in enumerate(self.nodes): + d = math.hypot( + pose.position.x - npose.position.x, + pose.position.y - npose.position.y, + ) + if best_dist is None or d < best_dist: + best_dist = d + best_id = nid + best_ind = i + return best_dist, best_id, best_ind + + # ================================================================== + # Edge creation – connect each node to its closest neighbour + # ================================================================== + + @staticmethod + def _yaw_between(src_pose, dst_pose): + """Return yaw angle (radians) from *src_pose* to *dst_pose*.""" + dx = dst_pose.position.x - src_pose.position.x + dy = dst_pose.position.y - src_pose.position.y + return math.atan2(dy, dx) + + @staticmethod + def _orientation_from_yaw(yaw): + """Return a quaternion dict for a given yaw angle.""" + qx, qy, qz, qw = quaternion_from_euler(0.0, 0.0, yaw) + return { + "x": float(qx), "y": float(qy), + "z": float(qz), "w": float(qw), + } - def remove_marker(self, id): - marker_array = MarkerArray() - node_vis = Marker() - node_vis.id = id - node_vis.action = Marker.DELETE - marker_array.markers.append(node_vis) - self.node_vis_pub.publish(marker_array) + def _build_edges(self): + """Build bidirectional edges between closest node pairs. + + For every node, an edge to the closest neighbour is created in + both directions. Duplicate edges are avoided. Returns a dict + mapping ``node_name -> [edge_dict, ...]`` and a dict mapping + ``node_name -> yaw`` (orientation toward first edge target). + """ + n = len(self.nodes) + edges_map = {f"node{nd[0]}": [] for nd in self.nodes} + node_yaws = {} + + if n < 2: + return edges_map, node_yaws + + # Collect positions for vectorised distance calculation + positions = np.array( + [[nd[1].position.x, nd[1].position.y] for nd in self.nodes]) + + for i in range(n): + src_name = f"node{self.nodes[i][0]}" + src_pose = self.nodes[i][1] + + # Distance to every other node + diffs = positions - positions[i] + dists = np.linalg.norm(diffs, axis=1) + dists[i] = np.inf # exclude self + + closest_idx = int(np.argmin(dists)) + dst_name = f"node{self.nodes[closest_idx][0]}" + dst_pose = self.nodes[closest_idx][1] + + # Forward edge (src -> dst) + edge_id_fwd = f"{src_name}_{dst_name}" + existing_ids = {e["edge_id"] for e in edges_map[src_name]} + if edge_id_fwd not in existing_ids: + fwd = deepcopy(self.template_edge) + fwd["edge_id"] = edge_id_fwd + fwd["node"] = dst_name + edges_map[src_name].append(fwd) + + # Reverse edge (dst -> src) + edge_id_rev = f"{dst_name}_{src_name}" + existing_ids_dst = {e["edge_id"] for e in edges_map[dst_name]} + if edge_id_rev not in existing_ids_dst: + rev = deepcopy(self.template_edge) + rev["edge_id"] = edge_id_rev + rev["node"] = src_name + edges_map[dst_name].append(rev) + + # Yaw: point from src toward its closest neighbour + if src_name not in node_yaws: + node_yaws[src_name] = self._yaw_between(src_pose, dst_pose) + + return edges_map, node_yaws + + # ================================================================== + # Topomap rebuild & publish + # ================================================================== + + def _rebuild_topomap(self): + """Rebuild the full topomap dict from self.nodes and publish it.""" + self._init_topomap() + + edges_map, node_yaws = self._build_edges() + + for nid, pose in self.nodes: + name = f"node{nid}" + + # Orientation: face along the edge direction + if name in node_yaws: + orientation = self._orientation_from_yaw(node_yaws[name]) + else: + orientation = { + "x": float(pose.orientation.x), + "y": float(pose.orientation.y), + "z": float(pose.orientation.z), + "w": float(pose.orientation.w), + } + # Build node dict from template + node_dict = deepcopy(self.template_node) + + # Populate meta + node_dict["meta"]["map"] = self.site_name + node_dict["meta"]["node"] = name + node_dict["meta"]["pointset"] = self.pointset + + # Populate node fields + nd = node_dict["node"] + nd["edges"] = edges_map.get(name, []) + nd["name"] = name + nd["pose"]["orientation"] = orientation + nd["pose"]["position"] = { + "x": float(pose.position.x), + "y": float(pose.position.y), + "z": float(pose.position.z), + } - def get_nearest_node(self, pose): - dist = None - id = None - ind = None - for i, node in enumerate(self.nodes): - d = sqrt((pose.position.x - node[1].position.x) ** 2 - + (pose.position.y - node[1].position.y) ** 2) - if not dist or d < dist: - dist = d - id = node[0] - ind = i - self.get_logger().info(f"{dist}, {id}, {ind}") - return dist, id, ind - - - def rotate_influence_zone(self, node, quat): - _, _, theta = euler_from_quaternion([quat.x, quat.y, quat.z, quat.w]) - - R = np.array([[np.cos(theta), -np.sin(theta)], - [np.sin(theta), np.cos(theta)]]) - - verts = deepcopy(node["node"]["verts"]) - new_verts = [] - for vert in verts: - new_vert = np.array([vert["x"], vert["y"]]) - new_vert = np.dot(R, new_vert) - new_verts.append({"x": float(new_vert[0]), "y": float(new_vert[1])}) - - node["node"]["verts"] = new_verts - - return node - - - def init_map(self): - self.topomap = {} - self.topomap["name"] = self.pointset - self.topomap["metric_map"] = self.site_name - self.topomap["pointset"] = self.pointset - - self.topomap["transformation"] = {} - self.topomap["transformation"]["rotation"] = {} - self.topomap["transformation"]["rotation"]["w"] = 1.0 - self.topomap["transformation"]["rotation"]["x"] = 0.0 - self.topomap["transformation"]["rotation"]["y"] = 0.0 - self.topomap["transformation"]["rotation"]["z"] = 0.0 - self.topomap["transformation"]["translation"] = {} - self.topomap["transformation"]["translation"]["x"] = 0.0 - self.topomap["transformation"]["translation"]["y"] = 0.0 - self.topomap["transformation"]["translation"]["z"] = 0.0 - self.topomap["transformation"]["child"] = "topo_map" - self.topomap["transformation"]["parent"] = "map" - - self.topomap["meta"] = {} - self.topomap["meta"]["last_updated"] = self.get_time() - self.topomap["nodes"] = [] - - - def get_time(self): + # Rotate template influence-zone vertices to match orientation + template_verts = nd.get("verts", []) + nd["verts"] = self._rotate_verts( + template_verts, node_yaws.get(name, 0.0)) + + self.topomap["nodes"].append(node_dict) + + self.topomap["meta"]["last_updated"] = self._get_time() + + # Publish to /topological_map_2 + self._publish_topomap() + + def _publish_topomap(self): + """Serialise self.topomap to YAML and publish on /topological_map_2.""" + msg = String() + msg.data = yaml.dump(self.topomap, default_flow_style=False) + self.map_pub.publish(msg) + self.get_logger().info( + f"Published topological map ({len(self.topomap['nodes'])} nodes) " + "to /topological_map_2") + + # ================================================================== + # Influence-zone rotation + # ================================================================== + + @staticmethod + def _rotate_verts(verts, yaw): + """Rotate influence-zone vertices by *yaw* radians.""" + cos_t = math.cos(yaw) + sin_t = math.sin(yaw) + rotated = [] + for v in verts: + rx = v["x"] * cos_t - v["y"] * sin_t + ry = v["x"] * sin_t + v["y"] * cos_t + rotated.append({"x": round(rx, 6), "y": round(ry, 6)}) + return rotated + + # ================================================================== + # Save / generate helpers + # ================================================================== + + @staticmethod + def _get_time(): + """Return a timestamp string.""" return datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S') - - def set_meta(self): - for node in self.topomap["nodes"]: - node["meta"]["map"] = self.site_name - node["meta"]["pointset"] = self.pointset - - def generate_tmap(self): + """Rebuild, save, and publish the topological map.""" if not self.nodes: self.get_logger().warn("No nodes yet, nothing to save!") return self.get_logger().info("Generating tmap from node list") - self.init_map() - self.save_waypoints() - self.set_meta() - - self.get_logger().info("Saving yaml") - tmap_file = os.path.join(self.tmap_dir, self.pointset) - self.save_yaml(tmap_file, self.topomap, False) - self.get_logger().info("Saved yaml") + self._rebuild_topomap() + if self.tmap_dir and self.pointset: + tmap_file = os.path.join(self.tmap_dir, self.pointset) + self._save_yaml(tmap_file, self.topomap) + self.get_logger().info(f"Saved tmap to {tmap_file}") def save_waypoints_srv_cb(self, request, response): + """Service: save raw waypoint poses to a timestamped YAML file.""" self.get_logger().info("Saving waypoints to file") waypoints = [] - for node in self.nodes: - waypoint = { + for nid, pose in self.nodes: + waypoints.append({ "pose": { "position": { - "x": node[1].position.x, - "y": node[1].position.y, - "z": node[1].position.z + "x": float(pose.position.x), + "y": float(pose.position.y), + "z": float(pose.position.z), }, "orientation": { - "x": node[1].orientation.x, - "y": node[1].orientation.y, - "z": node[1].orientation.z, - "w": node[1].orientation.w - } - } - } - waypoints.append(waypoint) - - data = { - "site": self.site_name, - "nodes": waypoints - } + "x": float(pose.orientation.x), + "y": float(pose.orientation.y), + "z": float(pose.orientation.z), + "w": float(pose.orientation.w), + }, + }, + }) - save_file = os.path.join(self.tmap_dir, f'{datetime.datetime.now().strftime("%Y%m%d%H%M%S")}.yml') - self.save_yaml(save_file, data) + data = {"site": self.site_name, "nodes": waypoints} + ts = datetime.datetime.now().strftime("%Y%m%d%H%M%S") + save_file = os.path.join(self.tmap_dir, f'{ts}.yml') + self._save_yaml(save_file, data) response.success = True - response.message = "Waypoints saved successfully" + response.message = f"Waypoints saved to {save_file}" return response - def generate_tmap_srv_cb(self, request, response): - self.get_logger().info("Generating topological map") - self.get_logger().info(f'Generating {len(self.nodes)} point Tmap from current nodes') - - if self.insert_map: - node_id = 0 - for node in self.nodes: - node_to_insert = deepcopy(self.template_node) - node_to_insert["node"]["name"] = f"{self.site_name}_node_{node_id}" - node_to_insert["node"]["pose"]["position"]["x"] = node[1].position.x - node_to_insert["node"]["pose"]["position"]["y"] = node[1].position.y - node_to_insert["node"]["pose"]["position"]["z"] = node[1].position.z - node_to_insert["node"]["pose"]["orientation"]["x"] = node[1].orientation.x - node_to_insert["node"]["pose"]["orientation"]["y"] = node[1].orientation.y - node_to_insert["node"]["pose"]["orientation"]["z"] = node[1].orientation.z - node_to_insert["node"]["pose"]["orientation"]["w"] = node[1].orientation.w - - self.topomap["nodes"].append(node_to_insert) - node_id += 1 - - tmap_file = os.path.join(self.tmap_dir, self.pointset) - self.save_yaml(tmap_file, self.topomap) - + """Service: rebuild, save, and publish the topological map.""" + self.get_logger().info("Generating topological map via service") + self.generate_tmap() response.success = True - response.message = "Topological map generated successfully" + response.message = "Topological map generated and published" return response + # ================================================================== + # Marker visualisation + # ================================================================== - def save_waypoints(self): - self.get_logger().info("Saving tmap waypoints only") - self.init_map() - for i, node_pose in enumerate(self.nodes): - node = deepcopy(self.template_node) - node["meta"]["node"] = 'n' + str(i) - node["node"]["name"] = 'n' + str(i) + def _update_node_markers(self): + """Publish RViz markers for all current nodes.""" + diameter = 0.7 + marker_array = MarkerArray() - node["node"]["pose"]["position"]["x"] = float(node_pose[1].position.x) - node["node"]["pose"]["position"]["y"] = float(node_pose[1].position.y) - node["node"]["pose"]["orientation"]["x"] = float(node_pose[1].orientation.x) - node["node"]["pose"]["orientation"]["y"] = float(node_pose[1].orientation.y) - node["node"]["pose"]["orientation"]["z"] = float(node_pose[1].orientation.z) - node["node"]["pose"]["orientation"]["w"] = float(node_pose[1].orientation.w) - node["node"]["properties"]["xy_goal_tolerance"] = 0.3 - node["node"]["properties"]["yaw_goal_tolerance"] = 0.3 + for nid, pose in self.nodes: + m = Marker() + m.type = Marker.SPHERE + m.header.frame_id = 'map' + m.id = nid + m.pose.position.x = pose.position.x + m.pose.position.y = pose.position.y + m.pose.position.z = pose.position.z + diameter / 2 + m.pose.orientation = pose.orientation + m.scale.x = diameter + m.scale.y = diameter + m.scale.z = diameter + m.color.a = 1.0 + m.color.r = 1.0 + m.color.g = 0.6 + m.color.b = 0.0 + m.lifetime = Duration() + marker_array.markers.append(m) - node = self.rotate_influence_zone(node, node_pose[1].orientation) - self.topomap["nodes"].append(node) + self.node_vis_pub.publish(marker_array) + + def _remove_marker(self, marker_id): + """Delete a single RViz marker by ID.""" + marker_array = MarkerArray() + m = Marker() + m.id = marker_id + m.action = Marker.DELETE + marker_array.markers.append(m) + self.node_vis_pub.publish(marker_array) def main(args=None): @@ -423,5 +597,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/topological_navigation/topological_navigation/scripts/map_manager.py b/topological_navigation/topological_navigation/scripts/map_manager.py deleted file mode 100755 index d74f6e8a..00000000 --- a/topological_navigation/topological_navigation/scripts/map_manager.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python -import math -import rospy -import sys - -import std_msgs.msg -from topological_navigation.topological_node import * -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation_msgs.msg import TopologicalMap - -from mongodb_store.message_store import MessageStoreProxy - -from topological_navigation.manager import map_manager - - -def usage(): - print("\nPublishes Topological Maps:") - print("\nFor loading a map from the mongodb:") - print("\t rosrun topological_navigation map_manager.py map_name") - print("\nFor loading a map from a tmap file:") - print("\t rosrun topological_navigation map_manager.py -f map_filename") - print("\nFor creating a new map:") - print("\t rosrun topological_navigation map_manager.py -n map_name") - print("\n\n") - - - -if __name__ == '__main__' : - load=True - load_from_file = False - if '-h' in sys.argv or '--help' in sys.argv or len(sys.argv) < 2 : - usage() - sys.exit(1) - else: - if '-n' in sys.argv: - ind = sys.argv.index('-n') - _map=sys.argv[ind+1] - print("Creating new Map (%s)" %_map) - load=False - elif '-f' in sys.argv: - ind = sys.argv.index('-f') - _map=sys.argv[ind+1] - print("Loading map from tmap file (%s)" %_map) - load_from_file=True - else: - _map=sys.argv[1] - - rospy.init_node("topological_map_manager") - ps = map_manager() - ps.init_map(_map,load,load_from_file) - rospy.spin() -######################################################################################################### \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/map_manager2.py b/topological_navigation/topological_navigation/scripts/map_manager2.py index ee032ec0..a6bea616 100755 --- a/topological_navigation/topological_navigation/scripts/map_manager2.py +++ b/topological_navigation/topological_navigation/scripts/map_manager2.py @@ -1,49 +1,506 @@ #!/usr/bin/env python3 """ -Created on Thu Nov 5 10:41:24 2020 +Self-contained Topological Map Manager 2 for ROS 2. + +This script manages topological maps: loading, saving, validating against a +YAML schema, and publishing via ROS 2 topics and services. It is fully +self-contained with no dependency on external tmap_model or map_types modules. @author: Adam Binch (abinch@sagarobotics.com) +@maintainer: Ibrahim Hroob (ihroob@lincoln.ac.uk) +Refactored: 2026-02-11 """ +######################################################################################################### +import os import sys +import json +import yaml +import datetime + import rclpy +import rclpy.node +import tf2_ros +import std_msgs.msg +import rosidl_runtime_py +from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy +from ament_index_python.packages import get_package_share_directory +from geometry_msgs.msg import Vector3, Quaternion, TransformStamped -from topological_navigation.manager2 import map_manager_2 +from std_srvs.srv import Trigger +import topological_navigation_msgs.srv as tn_srv +from topological_navigation.tmap_utils import CustomSafeLoader, NoAliasDumper -def usage(): - print("\nPublishes Topological Maps:") - print("\nFor loading a map:") - print("\t rosrun topological_navigation map_manager2.py map_filename") - print("\nFor creating a new map:") - print("\t rosrun topological_navigation map_manager2.py -n map_filename") - print("\n\n") +try: + import jsonschema +except ImportError: + jsonschema = None -def main(args=None): - - load=True - if '-h' in sys.argv or '--help' in sys.argv or len(sys.argv) < 2: - usage() - sys.exit(1) - else: - if '-n' in sys.argv: - ind = sys.argv.index('-n') - _map=sys.argv[ind+1] - print("Creating new Map (%s)" %_map) - load=False +######################################################################################################### +# Exceptions +######################################################################################################### + +class MapValidationError(Exception): + pass + +class NodeNotFoundError(Exception): + pass + +class EdgeNotFoundError(Exception): + pass + + +######################################################################################################### +# map_manager_2 (self-contained) +######################################################################################################### + +class map_manager_2(rclpy.node.Node): + """ + Self-contained topological map manager node. + + All map data (tmap dict), YAML schema validation, loading, saving, and + consistency checking are handled directly in this class -- no external + model module is required. + """ + + # ------------------------------------------------------------------ + # Initialisation + # ------------------------------------------------------------------ + def __init__(self, advertise_srvs=True): + super().__init__('topological_map_manager_2') + + package_path = get_package_share_directory('topological_navigation') + + # Schema path for validation + self.schema_path = str(os.path.join(package_path, 'config', 'tmap-schema.yaml')) + + # Declare parameters + self.declare_parameter('cache_topological_maps', False) + self.declare_parameter('auto_write_topological_maps', False) + self.declare_parameter('topological_map2_name', '') + self.declare_parameter('topological_map2_path', '') + + self.add_on_set_parameters_callback(self._parameters_callback) + + self.cache_maps = self.get_parameter('cache_topological_maps').value + self.auto_write = self.get_parameter('auto_write_topological_maps').value + self.topomap2_name = self.get_parameter('topological_map2_name').value + self.topomap2_path = self.get_parameter('topological_map2_path').value + + # Cache directory + self.cache_dir = os.path.join(os.path.expanduser("~"), ".ros", "topological_maps") + os.makedirs(self.cache_dir, exist_ok=True) + + # Load the schema once + self.schema = self._load_schema(self.schema_path) + + # Initialise an empty tmap dict + self.tmap = self._empty_tmap() + + # Latched QoS for map and schema publishers + latched_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Publisher: topological map (JSON string) + self.map_pub = self.create_publisher( + std_msgs.msg.String, '/topological_map_2', latched_qos + ) + + # Publisher: schema (YAML string, latched) + self.schema_pub = self.create_publisher( + std_msgs.msg.String, '/topological_map_schema', latched_qos + ) + self._publish_schema() + + # Advertise services + if advertise_srvs: + self._advertise() + + # ------------------------------------------------------------------ + # Schema helpers + # ------------------------------------------------------------------ + def _load_schema(self, schema_path): + """Load the YAML schema file. Returns dict or None.""" + try: + with open(schema_path, 'r') as f: + schema = yaml.safe_load(f) + self.get_logger().info(f"Loaded schema from {schema_path}") + return schema + except Exception as e: + self.get_logger().error(f"Failed to load schema: {e}") + return None + + def _publish_schema(self): + """Publish the schema as a latched String message.""" + if self.schema is not None: + msg = std_msgs.msg.String() + msg.data = yaml.safe_dump(self.schema, default_flow_style=False) + self.schema_pub.publish(msg) + self.get_logger().info("Published schema on /topological_map_schema") + + # ------------------------------------------------------------------ + # Empty map template + # ------------------------------------------------------------------ + def _empty_tmap(self): + return { + "meta": {"last_updated": self._get_time()}, + "metric_map": "map_2d", + "name": "new_map", + "pointset": "new_map", + "transformation": { + "rotation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "child": "topo_map", + "parent": "map", + }, + "nodes": [], + } + + @staticmethod + def _get_time(): + return datetime.datetime.now().strftime('%d-%m-%Y_%H-%M-%S') + + # ------------------------------------------------------------------ + # Validation + # ------------------------------------------------------------------ + def validate(self): + """Validate self.tmap against schema + logical consistency.""" + if self.schema is not None: + if jsonschema is None: + self.get_logger().warning( + "jsonschema not installed -- skipping schema validation" + ) + else: + try: + jsonschema.validate(instance=self.tmap, schema=self.schema) + except jsonschema.exceptions.ValidationError as e: + raise MapValidationError( + f"Schema validation failed: {e.message}" + ) + + self._check_consistency() + + def _check_consistency(self): + """Logical checks: duplicate names, broken edges, etc.""" + if "nodes" not in self.tmap: + raise MapValidationError("Map must contain 'nodes' key.") + + pointsets = {n["meta"]["pointset"] for n in self.tmap["nodes"]} + if len(pointsets) > 1: + raise MapValidationError(f"Multiple pointsets found: {pointsets}") + + names = [n["node"]["name"] for n in self.tmap["nodes"]] + if len(names) != len(set(names)): + seen, dupes = set(), [] + for x in names: + if x in seen: + dupes.append(x) + seen.add(x) + raise MapValidationError(f"Duplicate node names: {dupes}") + + name_set = set(names) + for node in self.tmap["nodes"]: + origin = node["node"]["name"] + for edge in node["node"]["edges"]: + dest = edge["node"] + if dest not in name_set: + raise MapValidationError( + f"Edge from {origin} points to non-existent node {dest}" + ) + + # ------------------------------------------------------------------ + # Load / Save + # ------------------------------------------------------------------ + def load_map(self, filename): + """Load a topological map YAML file, validate, and sync state.""" + self.get_logger().info(f"Loading topological map: {filename}") + try: + with open(filename, 'r') as f: + loaded = yaml.load(f, Loader=CustomSafeLoader) + if not isinstance(loaded, dict): + raise MapValidationError(f"Expected dict, got {type(loaded)}") + self.tmap = loaded + self.validate() + self._sync_from_tmap() + self.set_parameters([ + rclpy.parameter.Parameter( + 'topological_map2_name', + rclpy.Parameter.Type.STRING, + self.tmap.get("pointset", ""), + ) + ]) + self.get_logger().info("Map loaded and validated successfully.") + if self.cache_maps: + cache_file = os.path.join( + self.cache_dir, os.path.basename(filename) + ) + self._save_map(cache_file, no_alias=True) + except Exception as e: + self.get_logger().error(f"Failed to load map: {e}") + raise + + def _save_map(self, filename, no_alias=False): + """Write self.tmap to a YAML file.""" + self.get_logger().info(f"Saving map to {filename}") + if "nodes" in self.tmap: + self.tmap["nodes"].sort(key=lambda n: n["node"]["name"]) + dumper = NoAliasDumper if no_alias else yaml.SafeDumper + with open(filename, 'w') as fh: + yaml.dump(self.tmap, fh, default_flow_style=False, Dumper=dumper) + self.get_logger().info("Map saved successfully.") + + def _sync_from_tmap(self): + """Sync convenience attributes from self.tmap.""" + self.name = self.tmap.get("name", "new_map") + self.metric_map = self.tmap.get("metric_map", "map_2d") + self.pointset = self.tmap.get("pointset", "new_map") + self.transformation = self.tmap.get("transformation", {}) + + # ------------------------------------------------------------------ + # Map initialisation + # ------------------------------------------------------------------ + def init_map(self, name="new_map", metric_map="map_2d", pointset="new_map", + transformation="default", filepath=None, load=True): + + if transformation == "default": + self.transformation = { + "rotation": {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}, + "translation": {"x": 0.0, "y": 0.0, "z": 0.0}, + "child": "topo_map", + "parent": "map", + } + else: + self.transformation = transformation + + if load: + self.load_map(filepath) + else: + self.tmap = self._empty_tmap() + self.tmap["name"] = name + self.tmap["metric_map"] = metric_map + self.tmap["pointset"] = pointset + self.tmap["transformation"] = self.transformation + + self.map_pub.publish(std_msgs.msg.String(data=json.dumps(self.tmap))) + self.broadcaster = tf2_ros.StaticTransformBroadcaster(self) + self.broadcast_transform() + + # ------------------------------------------------------------------ + # Update & broadcast + # ------------------------------------------------------------------ + def update(self, update_time=True): + if update_time: + self.tmap["meta"]["last_updated"] = self._get_time() + self.validate() + self.map_pub.publish(std_msgs.msg.String(data=json.dumps(self.tmap))) + + def broadcast_transform(self): + trans, rot = Vector3(), Quaternion() + rosidl_runtime_py.set_message_fields( + trans, self.transformation.get("translation", {}) + ) + rosidl_runtime_py.set_message_fields( + rot, self.transformation.get("rotation", {}) + ) + + # Use topo_frame_id as the child frame so localisation + # can look up the transform to the frame the map is defined in. + child_frame = self.transformation.get( + "topo_frame_id", + self.transformation.get("child", "topo_map"), + ) + + msg = TransformStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.transformation.get("parent", "map") + msg.child_frame_id = child_frame + msg.transform.translation = trans + msg.transform.rotation = rot + self.broadcaster.sendTransform(msg) + self.get_logger().info( + f'Broadcasting static TF: {msg.header.frame_id} -> {child_frame}' + ) + + # ------------------------------------------------------------------ + # Parameter callback + # ------------------------------------------------------------------ + def _parameters_callback(self, params): + from rcl_interfaces.msg import SetParametersResult + for param in params: + if param.name == 'cache_topological_maps': + self.cache_maps = param.value + elif param.name == 'auto_write_topological_maps': + self.auto_write = param.value + elif param.name == 'topological_map2_name': + self.tmap["name"] = param.value + elif param.name == 'topological_map2_path': + pass + self.get_logger().info(f'Param {param.name} -> {param.value}') + return SetParametersResult(successful=True) + + # ------------------------------------------------------------------ + # ROS 2 services + # ------------------------------------------------------------------ + def _advertise(self): + self.get_logger().info("Advertising services...") + self.get_map_srv = self.create_service( + Trigger, + '/topological_map_manager2/get_topological_map', + self.get_topological_map_cb, + ) + self.write_map_srv = self.create_service( + tn_srv.WriteTopologicalMap, + '/topological_map_manager2/write_topological_map', + self.write_topological_map_cb, + ) + self.switch_map_srv = self.create_service( + tn_srv.WriteTopologicalMap, + '/topological_map_manager2/switch_topological_map', + self.switch_topological_map_cb, + ) + + def get_topological_map_cb(self, req, res): + """Service: return the current map as JSON.""" + self.get_logger().info("[SRV] get_topological_map") + res.success = True + res.message = json.dumps(self.tmap) + return res + + def switch_topological_map_cb(self, req, res): + """Service: switch to a different map file.""" + path = self.get_parameter("topological_map2_path").value + if os.path.isabs(req.filename) or path == "": + filename = req.filename else: - _map=sys.argv[1] + filename = os.path.join(path, req.filename) + + self.get_logger().info(f"[SRV] switch_topological_map -> {filename}") + try: + self.load_map(filename) + self.update(False) + self.broadcast_transform() + res.success = True + res.message = json.dumps(self.tmap) + except Exception as e: + res.success = False + res.message = str(e) + return res + + def write_topological_map_cb(self, req, res): + """Service: save the current map to a YAML file.""" + filename = req.filename + if not filename: + path = self.get_parameter("topological_map2_path").value + fname = self.get_parameter("topological_map2_name").value + filename = os.path.join(path, fname) + + self.get_logger().info(f"[SRV] write_topological_map -> {filename}") + try: + self._save_map(filename, req.no_alias) + res.success = True + res.message = f"Map written to {filename}" + except Exception as e: + res.success = False + res.message = str(e) + return res + + +######################################################################################################### +# CLI +######################################################################################################### + +def parse_arguments(): + """Parse CLI arguments.""" + import argparse + + parser = argparse.ArgumentParser( + description='Topological Map Manager 2 -- self-contained ROS 2 node', + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=""" +Examples: + %(prog)s my_map.yaml Load an existing map + %(prog)s -n new_map.yaml Create a new empty map + %(prog)s --test Load the default test map + %(prog)s -v my_map.yaml Load map with verbose logging +""", + ) + parser.add_argument('map_file', nargs='?', default=None, + help='Path to the topological map YAML file') + parser.add_argument('-n', '--new', action='store_true', + help='Create a new empty map') + parser.add_argument('-t', '--test', action='store_true', + help='Load the default test map') + parser.add_argument('-v', '--verbose', action='store_true', + help='Enable verbose logging') + + # Use parse_known_args to ignore ROS2 --ros-args passed by launch files + args, _ = parser.parse_known_args() + + if args.test or (args.map_file is None and not args.new): + try: + pkg = get_package_share_directory('topological_navigation') + map_file = os.path.join(pkg, 'config', 'test_simple_tmap2.yaml') + except Exception as e: + print(f"Error: Could not find default test map: {e}") + sys.exit(1) + elif args.new: + if args.map_file is None: + print("Error: --new requires a map filename") + sys.exit(1) + map_file = args.map_file + else: + map_file = args.map_file + + if not args.new and map_file and not os.path.exists(map_file): + print(f"Error: Map file not found: {map_file}") + sys.exit(1) + + return map_file, not args.new, args.verbose + + +def main(args=None): + try: + map_file, load, verbose = parse_arguments() + rclpy.init(args=args) + + manager = map_manager_2(advertise_srvs=True) + + if verbose: + manager.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) + + manager.get_logger().info("=" * 60) + manager.get_logger().info("Topological Map Manager 2 -- Starting") + manager.get_logger().info("=" * 60) + + manager.init_map(filepath=map_file, load=load) + manager.get_logger().info( + f"Map ready -- name={manager.tmap.get('name')}, " + f"nodes={len(manager.tmap.get('nodes', []))}" + ) + try: + rclpy.spin(manager) + except KeyboardInterrupt: + pass + finally: + manager.destroy_node() + rclpy.shutdown() - rclpy.init(args=args) - node = map_manager_2() - - node.init_map(filename=_map, load=load) + return 0 - rclpy.spin(node) - rclpy.shutdown() + except Exception as e: + print(f"Fatal error: {e}", file=sys.stderr) + import traceback + traceback.print_exc() + return 1 -if __name__ == '__main__' : - main() +if __name__ == '__main__': + sys.exit(main()) +######################################################################################################### diff --git a/topological_navigation/topological_navigation/scripts/map_publisher.py b/topological_navigation/topological_navigation/scripts/map_publisher.py deleted file mode 100755 index 166cff01..00000000 --- a/topological_navigation/topological_navigation/scripts/map_publisher.py +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env python -import math -import rospy -import sys - -import std_msgs.msg -from topological_navigation.topological_node import * -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation_msgs.msg import TopologicalMap - -from mongodb_store.message_store import MessageStoreProxy - -from topological_navigation.publisher import map_publisher - - -if __name__ == '__main__' : - point_set = sys.argv[1] - rospy.init_node("topological_map_publisher") - ps = map_publisher(point_set) - rospy.spin() \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/mean_based_prediction.py b/topological_navigation/topological_navigation/scripts/mean_based_prediction.py deleted file mode 100755 index 51542d02..00000000 --- a/topological_navigation/topological_navigation/scripts/mean_based_prediction.py +++ /dev/null @@ -1,178 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import actionlib -import pymongo -import json -import sys -import math -import time - -from datetime import datetime - -import actionlib -from actionlib_msgs.msg import * -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseStamped -from std_msgs.msg import Header -from std_msgs.msg import String -from nav_msgs.srv import * -import numpy as np - -import topological_navigation_msgs.msg -from topological_navigation_msgs.msg import TopologicalNode -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation_msgs.msg import NavStatistics -from topological_navigation_msgs.msg import TopologicalMap - -from topological_navigation.tmap_utils import * - -from topological_navigation_msgs.srv import * - - -def usage(): - print("For using all the available stats use:") - print("\t rosrun topological_navigation mean_based_prediction.py") - print("For all the stats in a range use:") - print("\t rosrun topological_navigation mean_based_prediction.py from_epoch to_epoch") - print("For all the stats from a date until now use:") - print("\t rosrun topological_navigation mean_based_prediction.py from_epoch -1") - print("For all the stats until one date:") - print("\t rosrun topological_navigation mean_based_prediction.py 0 to_epoch") - - -class TopologicalMeanPred(object): - - def __init__(self, epochs) : - - name= rospy.get_name() - action_name = name+'/build_temporal_model' - self.range = epochs - self.edge_to_duration = {} - self.edge_to_probability = {} - self.map = None - rospy.Subscriber('topological_map', TopologicalMap, self.map_callback) - - rospy.loginfo("Waiting for Topological map and building model ...") - - self.map_received = False - - - while not self.map_received: - rospy.sleep(rospy.Duration(1)) - rospy.loginfo("Waiting for Topological map and building model ...") - - rospy.loginfo("... Done") - - - #Creating Action Server - rospy.loginfo("Creating action server.") - self._as = actionlib.SimpleActionServer(action_name, topological_navigation_msgs.msg.BuildTopPredictionAction, execute_cb = self.build_callback, auto_start = False) - - rospy.loginfo(" ...starting") - self._as.start() - rospy.loginfo(" ...done") - - - self.predict_srv=rospy.Service('topological_prediction/predict_edges', topological_navigation_msgs.srv.PredictEdgeState, self.predict_edge_cb) - rospy.loginfo("All Done ...") - rospy.spin() - - - - def predict_edge_cb(self, req): - # build like this to ensure both lists are in the same order - results = [[edge_id, self.edge_to_probability[edge_id], self.edge_to_duration[edge_id]] for edge_id in self.edge_to_duration.keys()] - return zip(*results) - - """ - map_callback - - This function receives the topological map the computes duration estimates for all edges - """ - def map_callback(self, tmap) : - self.map = tmap - self.build_model() - self.map_received = True - - """ - build_callback - - Does nothing for this version. - """ - def build_callback(self, goal): - # set epoch ranges based on goal - if goal.start_range.secs > 0: - self.range[0] = goal.start_range.secs - if goal.end_range.secs > 0: - self.range[1] = goal.end_range.secs - - rospy.loginfo('Building model for epoch range: %s' % self.range) - - self.build_model() - - - self._as.set_succeeded() - - - def build_model(self): - """ Builds a model of mean duration for each edge using the defined nav stat range - """ - - - msg_store = MessageStoreProxy(collection='nav_stats') - - # reset model - self.edge_to_duration = {} - self.edge_to_probability = {} - - query_meta={} - if len(self.range) == 2: - if self.range[1] < 1: - upperlim = rospy.Time.now().secs - else: - upperlim = self.range[1] - query_meta["epoch"] = {"$gte": self.range[0], "$lt" : upperlim} - - - for i in self.map.nodes : - for j in i.edges: - if j.edge_id not in self.edge_to_duration: - - query = {"topological_map": self.map.name, "edge_id": j.edge_id} - stats = msg_store.query(NavStatistics._type, query, query_meta) - - rospy.loginfo('%s stats for %s' % (len(stats), j.edge_id)) - - # if no stats, use speed the same as speed-based predictor - if len(stats) == 0: - destination = get_node(self.map, j.node) - if j.top_vel >= 0.1: - self.edge_to_duration[j.edge_id] = rospy.Duration(get_distance_to_node(i, destination)/j.top_vel) - else : - self.edge_to_duration[j.edge_id] = rospy.Duration(get_distance_to_node(i, destination)/0.1) - self.edge_to_probability[j.edge_id] = 0.0 - else: - durations = np.array([stat.operation_time for stat, meta in stats]) - self.edge_to_duration[j.edge_id] = rospy.Duration(durations.mean()) - - successes = np.array([1 if stat.status == 'success' else 0 for stat, meta in stats]) - self.edge_to_probability[j.edge_id] = successes.mean() - - -if __name__ == '__main__': - rospy.init_node('topological_prediction') - epochs=[] - if len(sys.argv) < 2: - print("gathering all the stats") - epochs=[0, rospy.get_rostime().to_sec()] - else: - if len(sys.argv) == 3: - epochs.append(int(sys.argv[1])) - epochs.append(int(sys.argv[2])) - print(epochs) - else: - usage() - sys.exit(1) - server = TopologicalMeanPred(epochs) \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/nav_client.py b/topological_navigation/topological_navigation/scripts/nav_client.py deleted file mode 100755 index 2655592c..00000000 --- a/topological_navigation/topological_navigation/scripts/nav_client.py +++ /dev/null @@ -1,48 +0,0 @@ -#! /usr/bin/env python - -import rospy -import sys -# Brings in the SimpleActionClient -import actionlib -import topological_navigation_msgs.msg - - -class topol_nav_client(object): - - def __init__(self, targ) : - - rospy.on_shutdown(self._on_node_shutdown) - self.client = actionlib.SimpleActionClient('topological_navigation', topological_navigation_msgs.msg.GotoNodeAction) - - self.client.wait_for_server() - rospy.loginfo(" ... Init done") - - navgoal = topological_navigation_msgs.msg.GotoNodeGoal() - - print("Requesting Navigation to %s" %targ) - - navgoal.target = targ - #navgoal.origin = orig - - # Sends the goal to the action server. - self.client.send_goal(navgoal)#,self.done_cb, self.active_cb, self.feedback_cb) - - # Waits for the server to finish performing the action. - self.client.wait_for_result() - - # Prints out the result of executing the action - ps = self.client.get_result() # A FibonacciResult - print(ps) - - def _on_node_shutdown(self): - self.client.cancel_all_goals() - #sleep(2) - - -if __name__ == '__main__': - print('Argument List:',str(sys.argv)) - if len(sys.argv) < 2 : - sys.exit(2) - rospy.init_node('topol_nav_test') - ps = topol_nav_client(sys.argv[1]) - diff --git a/topological_navigation/topological_navigation/scripts/navigation.py b/topological_navigation/topological_navigation/scripts/navigation.py deleted file mode 100755 index c6e5730f..00000000 --- a/topological_navigation/topological_navigation/scripts/navigation.py +++ /dev/null @@ -1,1029 +0,0 @@ -#!/usr/bin/env python - -import rospy, actionlib, json -import dynamic_reconfigure.client - -import topological_navigation_msgs.msg -from topological_navigation_msgs.msg import NavStatistics -from topological_navigation_msgs.msg import CurrentEdge -from topological_navigation_msgs.msg import ClosestEdges -from topological_navigation_msgs.srv import EvaluateEdge, EvaluateEdgeRequest, EvaluateNode, EvaluateNodeRequest - -from std_msgs.msg import String -from actionlib_msgs.msg import GoalStatus - -from topological_navigation.route_search2 import RouteChecker, TopologicalRouteSearch2, get_route_distance -from topological_navigation.navigation_stats import nav_stats -from topological_navigation.tmap_utils import * - -from topological_navigation.edge_action_manager import EdgeActionManager -from topological_navigation.edge_reconfigure_manager import EdgeReconfigureManager - -from threading import Lock - -# A list of parameters topo nav is allowed to change and their mapping from dwa speak. -# If not listed then the param is not sent, e.g. TrajectoryPlannerROS doesn't have tolerances. -DYNPARAM_MAPPING = { - "DWAPlannerROS": { - "yaw_goal_tolerance": "yaw_goal_tolerance", - "xy_goal_tolerance": "xy_goal_tolerance", - }, - "TebLocalPlannerROS": { - "yaw_goal_tolerance": "yaw_goal_tolerance", - "xy_goal_tolerance": "xy_goal_tolerance", - }, -} - -status_mapping = {} -status_mapping[0] = "PENDING" -status_mapping[1] = "ACTIVE" -status_mapping[2] = "PREEMPTED" -status_mapping[3] = "SUCCEEDED" -status_mapping[4] = "ABORTED" -status_mapping[5] = "REJECTED" -status_mapping[6] = "PREEMPTING" -status_mapping[7] = "RECALLING" -status_mapping[8] = "RECALLED" -status_mapping[9] = "LOST" -################################################################################################################### - - -################################################################################################################### -class TopologicalNavServer(object): - - _feedback = topological_navigation_msgs.msg.GotoNodeFeedback() - _result = topological_navigation_msgs.msg.GotoNodeResult() - - _feedback_exec_policy = topological_navigation_msgs.msg.ExecutePolicyModeFeedback() - _result_exec_policy = topological_navigation_msgs.msg.ExecutePolicyModeResult() - - def __init__(self, name, mode): - - rospy.on_shutdown(self._on_node_shutdown) - - self.node_by_node = False - self.cancelled = False - self.preempted = False - self.stat = None - self.no_orientation = False - self._target = "None" - self.current_target = "none" - self.current_action = "none" - self.next_action = "none" - self.nav_from_closest_edge = False - self.fluid_navigation = True - self.final_goal = False - - self.current_node = "Unknown" - self.closest_node = "Unknown" - self.closest_edges = ClosestEdges() - self.nfails = 0 - - self.navigation_activated = False - self.navigation_lock = Lock() - - move_base_actions = [ - "move_base", - "human_aware_navigation", - "han_adapt_speed", - "han_vc_corridor", - "han_vc_junction", - ] - - self.move_base_actions = rospy.get_param("~move_base_actions", move_base_actions) - - # what service are we using as move_base? - self.move_base_name = rospy.get_param("~move_base_name", "move_base") - if not self.move_base_name in self.move_base_actions: - self.move_base_actions.append(self.move_base_name) - - self.stats_pub = rospy.Publisher("topological_navigation/Statistics", NavStatistics, queue_size=10) - self.edge_pub = rospy.Publisher("topological_navigation/Edge", CurrentEdge, queue_size=10) - self.route_pub = rospy.Publisher("topological_navigation/Route", topological_navigation_msgs.msg.TopologicalRoute, queue_size=10) - self.cur_edge = rospy.Publisher("current_edge", String, queue_size=10) - self.move_act_pub = rospy.Publisher("topological_navigation/move_action_status", String, latch=True, queue_size=1) - - self._map_received = False - rospy.Subscriber("/topological_map_2", String, self.MapCallback) - rospy.loginfo("Navigation waiting for the Topological Map...") - - while not self._map_received: - rospy.sleep(rospy.Duration.from_sec(0.05)) - rospy.loginfo("Navigation received the Topological Map") - - self.edge_action_manager = EdgeActionManager() - - self.edge_reconfigure = rospy.get_param("~reconfigure_edges", True) - self.srv_edge_reconfigure = rospy.get_param("~reconfigure_edges_srv", False) - if self.edge_reconfigure: - self.edgeReconfigureManager = EdgeReconfigureManager() - else: - rospy.logwarn("Edge Reconfigure Unavailable") - - rospy.loginfo("Subscribing to Localisation Topics...") - rospy.wait_for_message('closest_edges', ClosestEdges, timeout=10) - rospy.Subscriber("closest_node", String, self.closestNodeCallback) - rospy.Subscriber("closest_edges", ClosestEdges, self.closestEdgesCallback) - rospy.Subscriber("current_node", String, self.currentNodeCallback) - rospy.loginfo("...done") - - try: - rospy.loginfo("Waiting for restrictions...") - rospy.wait_for_service('restrictions_manager/evaluate_edge', timeout=3.0) - - self.evaluate_edge_srv = rospy.ServiceProxy( - 'restrictions_manager/evaluate_edge', EvaluateEdge) - self.evaluate_node_srv = rospy.ServiceProxy( - 'restrictions_manager/evaluate_node', EvaluateNode) - - rospy.loginfo("Restrictions Available") - self.using_restrictions = True - except: - rospy.logwarn("Restrictions Unavailable") - self.using_restrictions = False - - # this keeps the runtime state of the fail policies that are currently in execution - self.executing_fail_policy = {} - - # Creating Action Server for navigation - rospy.loginfo("Creating GO-TO-NODE action server...") - self._as = actionlib.SimpleActionServer(name, topological_navigation_msgs.msg.GotoNodeAction, - execute_cb=self.executeCallback, auto_start=False) - self._as.register_preempt_callback(self.preemptCallback) - self._as.start() - rospy.loginfo("...done") - - # Creating Action Server for execute policy - rospy.loginfo("Creating EXECUTE POLICY MODE action server...") - self._as_exec_policy = actionlib.SimpleActionServer("topological_navigation/execute_policy_mode", topological_navigation_msgs.msg.ExecutePolicyModeAction, - execute_cb=self.executeCallbackexecpolicy, auto_start=False) - self._as_exec_policy.register_preempt_callback(self.preemptCallbackexecpolicy) - self._as_exec_policy.start() - rospy.loginfo("...done") - - rospy.loginfo("All Done.") - - - def _on_node_shutdown(self): - with self.navigation_lock: - if self.navigation_activated: - self.preempted = True - self.cancel_current_action(timeout_secs=2) - - - def init_reconfigure(self): - - self.move_base_planner = rospy.get_param("~move_base_planner", "move_base/DWAPlannerROS") - planner = self.move_base_planner.split("/")[-1] - if not planner in DYNPARAM_MAPPING: - DYNPARAM_MAPPING[planner] = {} - - rospy.loginfo("Creating reconfigure client for {}".format(self.move_base_planner)) - self.rcnfclient = dynamic_reconfigure.client.Client(self.move_base_planner) - self.init_dynparams = self.rcnfclient.get_configuration() - - - def reconf_movebase(self, cedg, cnode, intermediate): - - if cnode["node"]["properties"]["xy_goal_tolerance"] <= 0.1: - cxygtol = 0.1 - else: - cxygtol = cnode["node"]["properties"]["xy_goal_tolerance"] - if not intermediate: - if cnode["node"]["properties"]["yaw_goal_tolerance"] <= 0.087266: - cytol = 0.087266 - else: - cytol = cnode["node"]["properties"]["yaw_goal_tolerance"] - else: - cytol = 6.283 - - params = {"yaw_goal_tolerance": cytol, "xy_goal_tolerance": cxygtol} - rospy.loginfo("Reconfiguring %s with %s" % (self.move_base_planner, params)) - print("Intermediate: {}".format(intermediate)) - self.reconfigure_movebase_params(params) - - - def reconfigure_movebase_params(self, params): - - self.init_dynparams = self.rcnfclient.get_configuration() - - key = self.move_base_planner[self.move_base_planner.rfind("/") + 1 :] - translation = DYNPARAM_MAPPING[key] - - translated_params = {} - for k, v in params.items(): - if k in translation: - if rospy.has_param(self.move_base_planner + "/" + translation[k]): - translated_params[translation[k]] = v - else: - rospy.logwarn("%s has no parameter %s" % (self.move_base_planner, translation[k])) - else: - rospy.logwarn("%s has no dynparam translation for %s" % (self.move_base_planner, k)) - - self._do_movebase_reconf(translated_params) - - - def _do_movebase_reconf(self, params): - - try: - self.rcnfclient.update_configuration(params) - except rospy.ServiceException as exc: - rospy.logwarn("Could not reconfigure move_base parameters. Caught service exception: %s. Will continue with previous parameters", exc) - - - def reset_reconf(self): - self._do_movebase_reconf(self.init_dynparams) - - - def MapCallback(self, msg): - """ - This Function updates the Topological Map everytime it is called - """ - self.lnodes = json.loads(msg.data) - self.topol_map = self.lnodes["pointset"] - self.rsearch = TopologicalRouteSearch2(self.lnodes) - self.route_checker = RouteChecker(self.lnodes) - self.make_move_base_edge() - - self._map_received = True - - - def make_move_base_edge(self): - - self.move_base_edge = {} - self.move_base_edge["action"] = self.move_base_name - self.move_base_edge["edge_id"] = "move_base_edge" - - move_base_goal = rospy.get_param("~move_base_goal", {}) - - if not move_base_goal: - for node in self.lnodes["nodes"]: - for edge in node["node"]["edges"]: - if edge["action"] == self.move_base_name: - move_base_goal["action_type"] = edge["action_type"] - move_base_goal["goal"] = edge["goal"] - break - else: - continue - break - - if not move_base_goal: - move_base_goal["action_type"] = "move_base_msgs/MoveBaseGoal" - move_base_goal["goal"] = {} - move_base_goal["goal"]["target_pose"] = {} - move_base_goal["goal"]["target_pose"]["pose"] = "$node.pose" - move_base_goal["goal"]["target_pose"]["header"] = {} - move_base_goal["goal"]["target_pose"]["header"]["frame_id"] = "$node.parent_frame" - - self.move_base_edge["action_type"] = move_base_goal["action_type"] - self.move_base_edge["goal"] = move_base_goal["goal"] - - rospy.loginfo("Move Base Goal set to {}".format(move_base_goal["action_type"])) - - - def executeCallback(self, goal): - """ - This Functions is called when the topo nav Action Server is called - """ - print("\n####################################################################################################") - rospy.loginfo("Processing GO-TO-NODE goal (No Orientation = {})".format(goal.no_orientation)) - can_start = False - - with self.navigation_lock: - if self.cancel_current_action(timeout_secs=10): - # we successfully stopped the previous action, claim the title to activate navigation - self.navigation_activated = True - can_start = True - - if can_start: - - self.cancelled = False - self.preempted = False - self.final_goal = False - self.no_orientation = goal.no_orientation - self.executing_fail_policy = {} - - self._feedback.route = "Starting..." - self._as.publish_feedback(self._feedback) - self.navigate(goal.target) - - else: - rospy.logwarn("Could not cancel current navigation action, GO-TO-NODE goal aborted") - self._as.set_aborted() - - self.navigation_activated = False - - - def executeCallbackexecpolicy(self, goal): - """ - This Function is called when the execute policy Action Server is called - """ - print("\n####################################################################################################") - rospy.loginfo("Processing EXECUTE POLICY MODE goal") - can_start = False - - with self.navigation_lock: - if self.cancel_current_action(timeout_secs=10): - # we successfully stopped the previous action, claim the title to activate navigation - self.navigation_activated = True - can_start = True - - if can_start: - - self.cancelled = False - self.preempted = False - self.final_goal = False - - self.max_dist_to_closest_edge = rospy.get_param("~max_dist_to_closest_edge", 1.0) - - if self.closest_edges.distances[0] > self.max_dist_to_closest_edge or self.current_node != "none": - self.nav_from_closest_edge = False - else: - self.nav_from_closest_edge = True - - route = goal.route - valid_route = self.route_checker.check_route(route) - - if valid_route and (route.source[0] == self.current_node or route.source[0] == self.closest_node): - final_edge = get_edge_from_id_tmap2(self.lnodes, route.source[-1], route.edge_id[-1]) - target = final_edge["node"] - route = self.enforce_navigable_route(route, target) - result = self.execute_policy(route, target) - else: - result = False - self.cancelled = True - rospy.logerr("Invalid route in execute policy mode goal") - - if not self.cancelled and not self.preempted: - self._result_exec_policy.success = result - if result: - self._as_exec_policy.set_succeeded(self._result_exec_policy) - else: - self._as_exec_policy.set_aborted(self._result_exec_policy) - else: - if not self.preempted: - self._result_exec_policy.success = result - self._as_exec_policy.set_aborted(self._result_exec_policy) - else: - self._result_exec_policy.success = False - self._as_exec_policy.set_preempted(self._result_exec_policy) - - else: - rospy.logwarn("Could not cancel current navigation action, EXECUTE POLICY MODE goal aborted.") - self._as_exec_policy.set_aborted() - - self.navigation_activated = False - - - def preemptCallback(self): - rospy.logwarn("Preempting GO-TO-NODE goal") - self.preempted = True - self.cancel_current_action(timeout_secs=2) - - - def preemptCallbackexecpolicy(self): - rospy.logwarn("Preempting EXECUTE POLICY MODE goal") - self.preempted = True - self.cancel_current_action(timeout_secs=2) - - - def closestNodeCallback(self, msg): - self.closest_node = msg.data - - - def closestEdgesCallback(self, msg): - self.closest_edges = msg - - - def currentNodeCallback(self, msg): - - if self.current_node != msg.data: # is there any change on this topic? - self.current_node = msg.data # we are at this new node - if msg.data != "none": # are we at a node? - rospy.loginfo("New node reached: {}".format(self.current_node)) - if self.navigation_activated: # is the action server active? - if self.stat: - self.stat.set_at_node() - # if the robot reached and intermediate node and the next action is move base goal has been reached - if ( - self.current_node == self.current_target - and self._target != self.current_target - and self.next_action in self.move_base_actions - and self.current_action in self.move_base_actions - and self.fluid_navigation - ): - rospy.loginfo("Intermediate node reached: %s", self.current_node) - self.goal_reached = True - - - def navigate(self, target): - """ - This function takes the target node and plans the actions that are required - to reach it. - """ - result = False - if not self.cancelled: - - g_node = self.rsearch.get_node_from_tmap2(target) - - self.max_dist_to_closest_edge = rospy.get_param("~max_dist_to_closest_edge", 1.0) - - if self.closest_edges.distances and (self.closest_edges.distances[0] > self.max_dist_to_closest_edge or self.current_node != "none"): - self.nav_from_closest_edge = False - o_node = self.rsearch.get_node_from_tmap2(self.closest_node) - rospy.loginfo("Planning from the closest NODE: {}".format(self.closest_node)) - else: - self.nav_from_closest_edge = True - o_node, the_edge = self.orig_node_from_closest_edge(g_node) - rospy.loginfo("Planning from the closest EDGE: {}".format(the_edge["edge_id"])) - - rospy.loginfo("Navigating From Origin %s to Target %s", o_node["node"]["name"], target) - - # Everything is Awesome!!! - # Target and Origin are not None - if (g_node is not None) and (o_node is not None): - if g_node["node"]["name"] != o_node["node"]["name"]: - route = self.rsearch.search_route(o_node["node"]["name"], target) - route = self.enforce_navigable_route(route, target) - if route.source: - rospy.loginfo("Navigating Case 1: Following route") - self.publish_route(route, target) - result, inc = self.followRoute(route, target, 0) - rospy.loginfo("Navigating Case 1 -> res: %d", inc) - else: - rospy.logwarn("Navigating Case 1a: There is no route from {} to {}. Check your edges.".format(o_node["node"]["name"], target)) - self.cancelled = True - result = False - inc = 1 - rospy.loginfo("Navigating Case 1a -> res: %d", inc) - else: - if self.nav_from_closest_edge: - result, inc = self.to_goal_node(g_node, the_edge) - else: - result, inc = self.to_goal_node(g_node) - else: - rospy.logwarn("Navigating Case 3: Target or Origin Nodes were not found on Map") - self.cancelled = True - result = False - inc = 1 - rospy.loginfo("Navigating Case 3 -> res: %d", inc) - - if (not self.cancelled) and (not self.preempted): - self._result.success = result - if result: - self._feedback.route = target - self._as.publish_feedback(self._feedback) - self._as.set_succeeded(self._result) - else: - self._feedback.route = self.current_node - self._as.publish_feedback(self._feedback) - self._as.set_aborted(self._result) - else: - if not self.preempted: - self._feedback.route = self.current_node - self._as.publish_feedback(self._feedback) - self._result.success = result - self._as.set_aborted(self._result) - else: - self._result.success = False - self._as.set_preempted(self._result) - - - def execute_policy(self, route, target): - - succeeded, inc = self.followRoute(route, target, 1) - - if succeeded: - rospy.loginfo("Navigation Finished Successfully") - self.publish_feedback_exec_policy(GoalStatus.SUCCEEDED) - else: - if self.cancelled and self.preempted: - rospy.logwarn("Fatal Fail") - self.publish_feedback_exec_policy(GoalStatus.PREEMPTED) - elif self.cancelled: - rospy.logwarn("Navigation Failed") - self.publish_feedback_exec_policy(GoalStatus.ABORTED) - return succeeded - - - def publish_feedback_exec_policy(self, nav_outcome=None): - if self.current_node == "none": # Happens due to lag in fetch system - rospy.sleep(0.5) - if self.current_node == "none": - self._feedback_exec_policy.current_wp = self.closest_node - else: - self._feedback_exec_policy.current_wp = self.current_node - else: - self._feedback_exec_policy.current_wp = self.current_node - if nav_outcome is not None: - self._feedback_exec_policy.status = nav_outcome - self._as_exec_policy.publish_feedback(self._feedback_exec_policy) - - - def orig_node_from_closest_edge(self, g_node): - - name_1, _ = get_node_names_from_edge_id_2(self.lnodes, self.closest_edges.edge_ids[0]) - name_2, _ = get_node_names_from_edge_id_2(self.lnodes, self.closest_edges.edge_ids[1]) - - # Navigate from the closest edge instead of the closest node. First get the closest edges. - edge_1 = get_edge_from_id_tmap2(self.lnodes, name_1, self.closest_edges.edge_ids[0]) - edge_2 = get_edge_from_id_tmap2(self.lnodes, name_2, self.closest_edges.edge_ids[1]) - - # Then get their destination nodes. - o_node_1 = self.rsearch.get_node_from_tmap2(edge_1["node"]) - o_node_2 = self.rsearch.get_node_from_tmap2(edge_2["node"]) - - # If the closest edges are of equal distance (usually a bidirectional edge) - # then use the destination node that results in a shorter route to the goal. - if self.closest_edges.distances[0] == self.closest_edges.distances[1]: - d1 = get_route_distance(self.lnodes, o_node_1, g_node) - d2 = get_route_distance(self.lnodes, o_node_2, g_node) - else: # Use the destination node of the closest edge. - d1 = 0; d2 = 1 - - if d1 <= d2: - return o_node_1, edge_1 - else: - return o_node_2, edge_2 - - - def to_goal_node(self, g_node, the_edge=None): - - rospy.loginfo("Target and Origin Nodes are the same") - self.current_target = g_node["node"]["name"] - - if the_edge is None: - # Check if there is a move_base action in the edges of this node and choose the earliest one in the - # list of move_base actions. If not is dangerous to move. - act_ind = 100 - for i in g_node["node"]["edges"]: - c_action_server = i["action"] - if c_action_server in self.move_base_actions: - c_ind = self.move_base_actions.index(c_action_server) - if c_ind < act_ind: - act_ind = c_ind - the_edge = i - - if the_edge is None: - rospy.logwarn("Navigating Case 2a: Could not find a move base action in the edges of target {}. Unsafe to move".format(g_node["node"]["name"])) - rospy.loginfo("Action not taken, outputting success") - result=True - inc=0 - rospy.loginfo("Navigating Case 2a -> res: %d", inc) - else: - rospy.loginfo("Navigating Case 2: Getting to the exact pose of target {}".format(g_node["node"]["name"])) - self.final_goal = True - self.current_target = g_node["node"]["name"] - origin_name,_ = get_node_names_from_edge_id_2(self.lnodes, the_edge["edge_id"]) - origin_node = self.rsearch.get_node_from_tmap2(origin_name) - - self.edge_reconf_start(the_edge) - result, inc = self.execute_action(the_edge, g_node, origin_node) - self.edge_reconf_end() - - if not result: - rospy.logwarn("Navigation Failed") - inc=1 - else: - rospy.loginfo("Navigation Finished Successfully") - rospy.loginfo("Navigating Case 2 -> res: %d", inc) - - return result, inc - - - def enforce_navigable_route(self, route, target_node): - """ - Enforces the route to always contain the initial edge that leads the robot to the first node in the given route. - In other words, avoid that the route contains an initial edge that is too far from the robot pose. - """ - if self.nav_from_closest_edge and self.closest_edges.edge_ids and len(self.closest_edges.edge_ids) == 2: - if not(self.closest_edges.edge_ids[0] in route.edge_id or self.closest_edges.edge_ids[1] in route.edge_id): - first_node = route.source[0] if len(route.source) > 0 else target_node - - for edge_id in self.closest_edges.edge_ids: - origin, destination = get_node_names_from_edge_id_2(self.lnodes, edge_id) - - if destination == first_node and edge_id not in route.edge_id: - route.source.insert(0, origin) - route.edge_id.insert(0, edge_id) - break - return route - - - def followRoute(self, route, target, exec_policy): - """ - This function follows the chosen route to reach the goal. - """ - self.navigation_activated = True - - nnodes = len(route.source) - Orig = route.source[0] - Targ = target - self._target = Targ - - self.init_reconfigure() - - rospy.loginfo("%d Nodes on route" % nnodes) - - inc = 1 - rindex = 0 - nav_ok = True - recovering = False - replanned = False - self.fluid_navigation = True - - o_node = self.rsearch.get_node_from_tmap2(Orig) - edge_from_id = get_edge_from_id_tmap2(self.lnodes, route.source[0], route.edge_id[0]) - if edge_from_id: - a = edge_from_id["action"] - rospy.loginfo("First action: %s" % a) - else: - rospy.logerr("Failed to get edge from id {}. Invalid route".format(route.edge_id[0])) - return False, inc - - if not self.nav_from_closest_edge: - # If the robot is not on a node or the first action is not move base type - # navigate to closest node waypoint (only when first action is not move base) - if a not in self.move_base_actions: - rospy.loginfo("The action of the first edge in the route is a navigate to pose action") - rospy.loginfo("Current node is {}".format(self.current_node)) - - if self.current_node == "none" and a not in self.move_base_actions: - self.next_action = a - rospy.loginfo("Do %s to origin %s" % (self.move_base_name, o_node["node"]["name"])) - - # 5 degrees tolerance - params = {"yaw_goal_tolerance": 0.087266} - self.reconfigure_movebase_params(params) - - self.current_target = Orig - nav_ok, inc = self.execute_action(self.move_base_edge, o_node) - rospy.loginfo("Navigation Finished Successfully") if nav_ok else rospy.logwarn("Navigation Failed") - - elif a not in self.move_base_actions: - move_base_act = False - for i in o_node["node"]["edges"]: - # Check if there is a move_base action in the edages of this node - # if not is dangerous to move - if i["action"] in self.move_base_actions: - move_base_act = True - - if not move_base_act: - rospy.logwarn("Could not find a move base action in the edges of origin {}. Unsafe to move".format(o_node["node"]["name"])) - rospy.loginfo("Action not taken, outputing success") - nav_ok = True - inc = 0 - else: - rospy.loginfo("Getting to the exact pose of origin {}".format(o_node["node"]["name"])) - self.current_target = Orig - nav_ok, inc = self.execute_action(self.move_base_edge, o_node) - rospy.loginfo("Navigation Finished Successfully") if nav_ok else rospy.logwarn("Navigation Failed") - - - while rindex < (len(route.edge_id)) and not self.cancelled and (nav_ok or recovering): - - cedg = get_edge_from_id_tmap2(self.lnodes, route.source[rindex], route.edge_id[rindex]) - a = cedg["action"] - - if rindex < (len(route.edge_id) - 1): - nedge = get_edge_from_id_tmap2(self.lnodes, route.source[rindex + 1], route.edge_id[rindex + 1]) - a1 = nedge["action"] - self.fluid_navigation = nedge["fluid_navigation"] - else: - nedge = None - a1 = "none" - self.fluid_navigation = False - self.final_goal = True - - self.current_action = a - self.next_action = a1 - - rospy.loginfo("From %s do (%s) to %s" % (route.source[rindex], a, cedg["node"])) - - current_edge = "%s--%s" % (cedg["edge_id"], self.topol_map) - rospy.loginfo("Current edge: %s" % current_edge) - self.cur_edge.publish(current_edge) - - if not exec_policy: - self._feedback.route = "%s to %s using %s" % (route.source[rindex], cedg["node"], a) - self._as.publish_feedback(self._feedback) - else: - self.publish_feedback_exec_policy() - - cnode = self.rsearch.get_node_from_tmap2(cedg["node"]) - onode = self.rsearch.get_node_from_tmap2(route.source[rindex]) - - # do not care for the orientation of the waypoint if is not the last waypoint AND - # the current and following action are move_base or human_aware_navigation - # and when the fuild_navigation is true - if rindex < len(route.edge_id) - 1 and a1 in self.move_base_actions and a in self.move_base_actions and self.fluid_navigation: - self.reconf_movebase(cedg, cnode, True) - else: - if self.no_orientation: - self.reconf_movebase(cedg, cnode, True) - else: - self.reconf_movebase(cedg, cnode, False) - - self.current_target = cedg["node"] - - self.stat = nav_stats(route.source[rindex], cedg["node"], self.topol_map, cedg["edge_id"]) - dt_text = self.stat.get_start_time_str() - - self.edge_reconf_start(cedg) - if exec_policy: - nav_ok, inc = self.execute_action(cedg, cnode, onode) - else: - nav_ok, inc, recovering, route, replanned = self.execute_action_fail_recovery(cedg, cnode, route, rindex, onode, target) - self.edge_reconf_end() - - params = {"yaw_goal_tolerance": 0.087266, "xy_goal_tolerance": 0.1} - self.reconfigure_movebase_params(params) - - not_fatal = nav_ok - if self.cancelled: - nav_ok = True - if self.preempted: - not_fatal = False - nav_ok = False - - self.stat.set_ended(self.current_node) - dt_text=self.stat.get_finish_time_str() - operation_time = self.stat.operation_time - time_to_wp = self.stat.time_to_wp - - if nav_ok: - self.stat.status = "success" - rospy.loginfo("Navigation Finished on %s (%d/%d)" % (dt_text, operation_time, time_to_wp)) - else: - if not_fatal: - rospy.logwarn("Navigation Failed on %s (%d/%d)" % (dt_text, operation_time, time_to_wp)) - self.stat.status = "failed" - else: - rospy.logwarn("Fatal Fail on %s (%d/%d)" % (dt_text, operation_time, time_to_wp)) - self.stat.status = "fatal" - - self.publish_stats() - - current_edge = "none" - self.cur_edge.publish(current_edge) - - self.current_action = "none" - self.next_action = "none" - - if not replanned: - rindex = rindex + 1 - - self.reset_reconf() - - self.navigation_activated = False - - result = nav_ok - return result, inc - - - def edge_reconf_start(self, edge): - - if self.edge_reconfigure: - if not self.srv_edge_reconfigure: - self.edgeReconfigureManager.register_edge(edge) - self.edgeReconfigureManager.initialise() - self.edgeReconfigureManager.reconfigure() - else: - self.edgeReconfigureManager.srv_reconfigure(edge["edge_id"]) - - - def edge_reconf_end(self): - - if self.edge_reconfigure and not self.srv_edge_reconfigure and self.edgeReconfigureManager.active: - self.edgeReconfigureManager._reset() - rospy.sleep(rospy.Duration.from_sec(0.3)) - - - def cancel_current_action(self, timeout_secs=-1): - """ - Cancels the action currently in execution. Returns True if the current goal is correctly ended. - """ - rospy.loginfo("Cancelling current navigation goal, timeout_secs = {}...".format(timeout_secs)) - - self.edge_action_manager.preempt() - self.cancelled = True - - if timeout_secs > 0: - stime = rospy.get_rostime() - timeout = rospy.Duration().from_sec(timeout_secs) - while self.navigation_activated: - if (rospy.get_rostime() - stime) > timeout: - rospy.loginfo("\t[timeout called]") - break - rospy.sleep(0.2) - - rospy.loginfo("Navigation active: " + str(self.navigation_activated)) - return not self.navigation_activated - - - def publish_route(self, route, target): - stroute = topological_navigation_msgs.msg.TopologicalRoute() - for i in route.source: - stroute.nodes.append(i) - stroute.nodes.append(target) - self.route_pub.publish(stroute) - - - def publish_stats(self): - pubst = NavStatistics() - pubst.edge_id = self.stat.edge_id - pubst.status = self.stat.status - pubst.origin = self.stat.origin - pubst.target = self.stat.target - pubst.topological_map = self.stat.topological_map - pubst.final_node = self.stat.final_node - pubst.time_to_waypoint = self.stat.time_to_wp - pubst.operation_time = self.stat.operation_time - pubst.date_started = self.stat.get_start_time_str() - pubst.date_at_node = self.stat.date_at_node.strftime("%A, %B %d %Y, at %H:%M:%S hours") - - pubst.date_finished = self.stat.get_finish_time_str() - self.stats_pub.publish(pubst) - self.stat = None - - - def get_fail_policy_state(self, edge): - policy = None - state = -1 - if len(self.executing_fail_policy) == 0: - _policy = [action.strip().split("_") for action in edge["fail_policy"].split(",")] - policy = [] - # repeat the actions that can be repeated more than once - for action in _policy: - if len(action) > 1 and isinstance(eval(action[-1]), int): - for _ in range(eval(action[-1])): - policy.append(action[:-1]) - else: - policy.append(action) - state = 0 - self.executing_fail_policy = { - "policy": policy, # the policy we want to execute - "state": state, # at which point of the policy we are - "edge": edge["edge_id"] - } - - else: - policy = self.executing_fail_policy["policy"] - # increment the state because if we are here it means that the previous policy action failed - self.executing_fail_policy["state"] += 1 - state = self.executing_fail_policy["state"] - - return policy, state - - - def execute_action_fail_recovery(self, edge, destination_node, route, idx, origin_node, target): - """ - This function wraps `execute_action` by executing the fail_policy in case of ABORTED action - The fail policy sometimes modifies the current route by including the recovery action - """ - nav_ok, inc = self.execute_action(edge, destination_node, origin_node) - - new_route = route - recovering = False - replanned = False - - # this means the action is aborted -> execute the fail policy - # make sure that if the goal is cancelled by the client we don't enter here - if not nav_ok and not self.preempted: - rospy.loginfo("\t>> route: {}".format(route)) - route_updated = False - while not route_updated: - policy, state = self.get_fail_policy_state(edge) - if state < len(policy): - rec_action = policy[state] - rospy.loginfo(">> EXECUTING FAIL POLICY ACTION: {}.".format(rec_action)) - - if rec_action[0] == "retry": - new_route.source.insert(idx+1, route.source[idx]) - new_route.edge_id.insert(idx+1, route.edge_id[idx]) - route_updated = True - recovering = True - replanned = False - elif rec_action[0] == "fail": - route_updated = True - recovering = False - replanned = False - elif rec_action[0] == "wait": - secs = 1 - if len(rec_action) > 1: - secs = int(rec_action[-1]) - rospy.sleep(secs) - recovering = True - replanned = False - elif rec_action[0] == "replan": - _route = self.rsearch.search_route(origin_node["node"]["name"], target, avoid_edges=[edge["edge_id"]]) - _route = self.enforce_navigable_route(_route, target) - - # build the new route - new_route.source = route.source[:idx] + _route.source - new_route.edge_id = route.edge_id[:idx] + _route.edge_id - route_updated = True - recovering = True - replanned = True - else: - # clean the current fail policy data - self.executing_fail_policy = {} - recovering = False - replanned = False - - rospy.loginfo("\t>> new route: {}".format(new_route)) - return nav_ok, inc, recovering, new_route, replanned - - - def execute_action(self, edge, destination_node, origin_node=None): - - inc = 0 - result = True - self.goal_reached = False - self.prev_status = None - - if self.using_restrictions and edge["edge_id"] != "move_base_edge": - rospy.loginfo("Evaluating restrictions on edge {}".format(edge["edge_id"])) - ev_edge_msg = EvaluateEdgeRequest() - ev_edge_msg.edge = edge["edge_id"] - ev_edge_msg.runtime = True - resp = self.evaluate_edge_srv.call(ev_edge_msg) - if resp.success and resp.evaluation: - rospy.logwarn("The edge is restricted, stopping navigation") - result = False - inc = 1 - return result, inc - - rospy.loginfo("Evaluating restrictions on node {}".format(destination_node["node"]["name"])) - ev_node_msg = EvaluateNodeRequest() - ev_node_msg.node = destination_node["node"]["name"] - ev_node_msg.runtime = True - resp = self.evaluate_node_srv.call(ev_node_msg) - if resp.success and resp.evaluation: - rospy.logwarn("The node is restricted, stopping navigation") - result = False - inc = 1 - return result, inc - - - self.edge_action_manager.initialise(edge, destination_node, origin_node) - self.edge_action_manager.execute() - - status = self.edge_action_manager.client.get_state() - self.pub_status(status) - while ( - (status == GoalStatus.ACTIVE or status == GoalStatus.PENDING) - and not self.cancelled - and not self.goal_reached - ): - status = self.edge_action_manager.client.get_state() - self.pub_status(status) - rospy.sleep(rospy.Duration.from_sec(0.01)) - - res = self.edge_action_manager.client.get_result() - - if status != GoalStatus.SUCCEEDED: - if not self.goal_reached: - result = False - if status is GoalStatus.PREEMPTED: - self.preempted = True - else: - result = True - - if not res: - if not result: - inc = 1 - else: - inc = 0 - - rospy.sleep(rospy.Duration.from_sec(0.5)) - status = self.edge_action_manager.client.get_state() - self.pub_status(status) - - rospy.loginfo("move action status: {}, goal reached: {}, inc: {}".format(status_mapping[status], result, inc)) - return result, inc - - - def pub_status(self, status): - - if status != self.prev_status: - d = {} - d["goal"] = self.edge_action_manager.destination_node["node"]["name"] - d["final_goal"] = self.final_goal - d["action"] = self.edge_action_manager.current_action.upper() - d["status"] = status_mapping[status] - - self.move_act_pub.publish(String(json.dumps(d))) - self.prev_status = status -################################################################################################################### - - -################################################################################################################### -if __name__ == "__main__": - rospy.init_node("topological_navigation") - mode = "normal" - server = TopologicalNavServer(rospy.get_name(), mode) - rospy.spin() - - rospy.loginfo("Exiting.") -################################################################################################################### \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/navigation2.py b/topological_navigation/topological_navigation/scripts/navigation2.py old mode 100755 new mode 100644 index 0bef2339..f9989054 --- a/topological_navigation/topological_navigation/scripts/navigation2.py +++ b/topological_navigation/topological_navigation/scripts/navigation2.py @@ -1,1330 +1,1844 @@ #!/usr/bin/env python +"""Map-Driven Topological Navigation Server. + +All navigation behaviour is defined by the topological map YAML file: + +- **definitions**: inline BT XML blobs, written to temp files for Nav2. +- **actions**: maps edge action names to Nav2 action types, servers, + goal templates, and composability flags. +- **edges**: reference action names in the ``actions`` section. + +The map file is the single source of truth. No BT file paths or +action-type mappings are hard-coded in this script. + +Architecture +~~~~~~~~~~~~ +1. Map arrives on ``/topological_map_2`` -> parsed into a NetworkX + ``DiGraph`` and map-level ``definitions`` / ``actions`` dicts. +2. For every unique ``(action_type, action_server)`` pair an + ``ActionClient`` is created. +3. Route planning uses NetworkX shortest-path algorithms whose + parameters (algorithm, weight attribute) are exposed as ROS 2 + parameters so they can be changed at launch time. +4. Consecutive composable edges are merged into multi-waypoint + segments; non-composable edges are dispatched individually. +5. Boundary polygons are published for any segment whose edges + carry ``boundary_left`` / ``boundary_right`` properties. + +ROS 2 interfaces + Action servers: + / (GotoNode) + /topological_navigation/execute_policy_mode (ExecutePolicyMode) + Subscriptions: + /topological_map_2, closest_node, closest_edges, current_node + Publishers: + topological_navigation/Statistics, topological_navigation/Route, + current_edge, /boundary_checker (PolygonStamped), + /robot_operation_current_status (String), + topological_navigation/move_action_status (String) + Parameters: + max_dist_to_closest_edge (double) -- origin heuristic + default_boundary_left (double) -- row corridor left + default_boundary_right (double) -- row corridor right + route_algorithm (string) -- 'astar' | 'dijkstra' + route_weight_attr (string) -- edge attribute for cost + +Last Updated: 2026-02-25 """ -Created on Tue Nov 5 22:02:24 2023 -@author: Geesara Kulathunga (ggeesara@gmail.com) -""" +import importlib +import json +import math +import os +import tempfile +import time +import threading +from datetime import datetime -import rclpy, json, yaml +import yaml -import math -from topological_navigation_msgs.msg import NavStatistics, CurrentEdge, ClosestEdges, TopologicalRoute, GotoNodeFeedback, ExecutePolicyModeFeedback -from topological_navigation_msgs.srv import EvaluateEdge, EvaluateNode -from topological_navigation_msgs.action import GotoNode, ExecutePolicyMode -from topological_navigation_msgs.action import ExecutePolicyMode -from std_msgs.msg import String -import os +import rclpy +import rclpy.node from action_msgs.msg import GoalStatus -from topological_navigation.route_search2 import RouteChecker, TopologicalRouteSearch2, get_route_distance -from topological_navigation.navigation_stats import nav_stats -from topological_navigation.scripts.param_processing import ParameterUpdaterNode -from topological_navigation.tmap_utils import * -from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy -from rclpy.action import ActionServer -from rclpy import Parameter -from topological_navigation.edge_action_manager2 import EdgeActionManager -from topological_navigation.edge_reconfigure_manager2 import EdgeReconfigureManager -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup -from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor -from threading import Lock - -from ament_index_python.packages import get_package_share_directory -from topological_navigation.scripts.actions_bt import ActionsType -# A list of parameters topo nav is allowed to change and their mapping from dwa speak. -# If not listed then the param is not sent, e.g. TrajectoryPlannerROS doesn't have tolerances. +from geometry_msgs.msg import Point32, PolygonStamped, PoseStamped +from rcl_interfaces.msg import Parameter as RclParameter +from rcl_interfaces.msg import ParameterType, ParameterValue +from rcl_interfaces.srv import SetParameters +from rclpy import Parameter +from rclpy.action import ActionClient, ActionServer +from rclpy.callback_groups import ( + MutuallyExclusiveCallbackGroup, + ReentrantCallbackGroup, +) +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import ( + DurabilityPolicy, + HistoryPolicy, + QoSHistoryPolicy, + QoSProfile, + ReliabilityPolicy, +) +from std_msgs.msg import String - -# this ensures that all the poses and translates -# are float-type and not int-type as there is an -# assertion in ros2 messages (vector3, pose etc.) -# for float-type [x,y,z,w] keys. -class CustomSafeLoader(yaml.SafeLoader): +from topological_navigation.navigation_graph import ( + ACTION_TO_STATE, + NavState, + NavStateMachine, + compute_boundary_polygon, + get_route_distance, + get_route_edges, + merge_action_segments, + plan_route, +) +from topological_navigation.networkx_utils import build_graph_from_tmap +from topological_navigation.tmap_utils import ( + get_edge_from_id_tmap2, + get_node_from_tmap2, +) +from topological_navigation_msgs.action import ExecutePolicyMode, GotoNode +from topological_navigation_msgs.msg import ( + ClosestEdges, + ExecutePolicyModeFeedback, + GotoNodeFeedback, + NavStatistics, + TopologicalRoute, +) +# ===================================================================== +# GoalStatus helpers +# ===================================================================== + +_STATUS_MAP = { + GoalStatus.STATUS_UNKNOWN: "STATUS_UNKNOWN", + GoalStatus.STATUS_ACCEPTED: "STATUS_ACCEPTED", + GoalStatus.STATUS_EXECUTING: "STATUS_EXECUTING", + GoalStatus.STATUS_CANCELING: "STATUS_CANCELING", + GoalStatus.STATUS_SUCCEEDED: "STATUS_SUCCEEDED", + GoalStatus.STATUS_CANCELED: "STATUS_CANCELED", + GoalStatus.STATUS_ABORTED: "STATUS_ABORTED", +} + + +def _status_str(code: int) -> str: + return _STATUS_MAP.get(code, "STATUS_UNKNOWN(%d)" % code) + + +# ===================================================================== +# YAML loader -- coerce pose ints to float +# ===================================================================== + +class _FloatSafeLoader(yaml.SafeLoader): def construct_mapping(self, node, deep=False): - mapping = super().construct_mapping(node, deep=deep) + m = super().construct_mapping(node, deep=deep) + for k in ('x', 'y', 'z', 'w'): + if k in m and isinstance(m[k], int): + m[k] = float(m[k]) + return m + + +# ===================================================================== +# Navigation statistics +# ===================================================================== + +class nav_stats: + """Timing statistics for a single edge traversal.""" + + def __init__(self, origin, target, topol_map, edge_id): + self.status = "active" + self.origin = origin + self.target = target + self.topological_map = topol_map + self.edge_id = edge_id + self.set_start() + + def set_start(self): + """Record navigation start time.""" + self.date_started = datetime.now() + self.date_at_node = self.date_started + + def set_ended(self, node): + """Record navigation end time and compute durations.""" + self.final_node = node + self.date_finished = datetime.now() + self.get_operation_time() + self.get_time_to_wp() + + def set_at_node(self): + """Record time of arrival at intermediate node.""" + self.date_at_node = datetime.now() + + def get_operation_time(self): + """Total seconds from start to finish.""" + delta = self.date_finished - self.date_started + self.operation_time = delta.total_seconds() + return self.operation_time + + def get_time_to_wp(self): + """Seconds from last intermediate node to finish.""" + if self.date_at_node != self.date_started: + delta = self.date_finished - self.date_at_node + self.time_to_wp = delta.total_seconds() + else: + self.time_to_wp = 0 + return self.time_to_wp - # this can be extended to test the validity of the tmap2 - # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w']: - if key in mapping and isinstance(mapping[key], int): - mapping[key] = float(mapping[key]) - - return mapping + def get_start_time_str(self): + """Human-readable start timestamp.""" + return self.date_started.strftime( + '%A, %B %d %Y, at %H:%M:%S hours', + ) + def get_finish_time_str(self): + """Human-readable finish timestamp.""" + return self.date_finished.strftime( + '%A, %B %d %Y, at %H:%M:%S hours', + ) -class TopologicalNavServer(rclpy.node.Node): - - _feedback = GotoNodeFeedback() - _result = GotoNode.Result() - _feedback_exec_policy = ExecutePolicyModeFeedback() - _result_exec_policy = ExecutePolicyMode.Result() +# ===================================================================== +# Topological Navigation Server +# ===================================================================== - def __init__(self, name, update_params_control_server, edge_action_manager_server): - super().__init__(name) - rclpy.get_default_context().on_shutdown(self._on_node_shutdown) - self.node_by_node = False - self.cancelled = False - self.preempted = False - self.stat = None - self.no_orientation = False - self._target = "None" - self.current_target = "none" - self.current_action = "none" - self.next_action = "none" - self.nav_from_closest_edge = False - self.fluid_navigation = True - self.final_goal = False - self.update_params_control_server = update_params_control_server - - self.route = None - self.target = None +class TopologicalNavServer(rclpy.node.Node): + """Map-driven topological navigation server. - self.current_node = "Unknown" - self.closest_node = "Unknown" - self.closest_edges = ClosestEdges() - self.nfails = 0 - - self.navigation_activated = False - self.navigation_lock = Lock() - self.ACTIONS = ActionsType() - - self.declare_parameter('navigation_action_name', Parameter.Type.STRING) - self.declare_parameter('navigation_actions', Parameter.Type.STRING_ARRAY) - self.declare_parameter('navigation_action_goal', Parameter.Type.STRING_ARRAY) - self.declare_parameter("max_dist_to_closest_edge", Parameter.Type.DOUBLE) - self.declare_parameter('reconfigure_edges', Parameter.Type.BOOL) - self.declare_parameter('reconfigure_edges_srv', Parameter.Type.BOOL) - - self.declare_parameter("row_traversal_planner", Parameter.Type.STRING) - self.declare_parameter("default_planner", Parameter.Type.STRING) - self.declare_parameter("goal_align_planner", Parameter.Type.STRING) - - self.declare_parameter("row_traversal_planner_xy_goal_tolerance", Parameter.Type.DOUBLE) - self.declare_parameter("default_planner_xy_goal_tolerance", Parameter.Type.DOUBLE) - self.declare_parameter("goal_align_planner_xy_goal_tolerance", Parameter.Type.DOUBLE) + All action clients, BT selection, and composability rules are + derived from the ``definitions`` and ``actions`` sections of the + topological map YAML. NetworkX path-planning parameters are + exposed as ROS 2 parameters. + """ - self.declare_parameter("row_traversal_planner_yaw_goal_tolerance", Parameter.Type.DOUBLE) - self.declare_parameter("default_planner_xy_yaw_goal_tolerance", Parameter.Type.DOUBLE) - self.declare_parameter("goal_align_planner_xy_yaw_goal_tolerance", Parameter.Type.DOUBLE) - - self.declare_parameter('use_nav2_follow_route', Parameter.Type.BOOL) - self.declare_parameter('use_in_row_operation', Parameter.Type.BOOL) - self.declare_parameter('inrow_step_size', Parameter.Type.DOUBLE) - self.declare_parameter('inrow_step_intermediate_dis', Parameter.Type.DOUBLE) - self.declare_parameter(self.ACTIONS.BT_DEFAULT, Parameter.Type.STRING) - self.declare_parameter(self.ACTIONS.BT_IN_ROW, Parameter.Type.STRING) - self.declare_parameter(self.ACTIONS.BT_GOAL_ALIGN, Parameter.Type.STRING) - self.declare_parameter(self.ACTIONS.BT_IN_ROW_OPERATION, Parameter.Type.STRING) - self.declare_parameter(self.ACTIONS.BT_IN_ROW_RECOVERY, Parameter.Type.STRING) - - self.declare_parameter("allow_intermediate_orientation_override", Parameter.Type.BOOL) - self.allow_intermediate_orientation_override = self.get_parameter_or("allow_intermediate_orientation_override", Parameter('bool', Parameter.Type.BOOL, False)).value - - self.navigation_action_name = self.get_parameter_or("navigation_action_name", Parameter('str', Parameter.Type.STRING, self.ACTIONS.NAVIGATE_TO_POSE)).value - self.navigation_actions = self.get_parameter_or("navigation_actions", Parameter('str', Parameter.Type.STRING_ARRAY, self.ACTIONS.navigation_actions)).value - self.use_nav2_follow_route = self.get_parameter_or("use_nav2_follow_route", Parameter('bool', Parameter.Type.BOOL, False)).value - self.use_in_row_operation = self.get_parameter_or("use_in_row_operation", Parameter('bool', Parameter.Type.BOOL, False)).value - self.inrow_step_size = self.get_parameter_or("inrow_step_size", Parameter('double', Parameter.Type.DOUBLE, 2.0)).value - self.inrow_step_intermediate_dis = self.get_parameter_or("inrow_step_intermediate_dis", Parameter('double', Parameter.Type.DOUBLE, -1.0)).value - - row_traversal_planner = self.get_parameter_or("row_traversal_planner", Parameter('str', Parameter.Type.STRING, "dwb_core::DWBLocalPlanner")).value - default_planner = self.get_parameter_or("default_planner", Parameter('str', Parameter.Type.STRING, "dwb_core::DWBLocalPlanner")).value - goal_align_planner = self.get_parameter_or("goal_align_planner",Parameter('str', Parameter.Type.STRING, "dwb_core::DWBLocalPlanner")).value - - self.ACTIONS.setPlanner(row_traversal_planner, self.ACTIONS.ROW_TRAVERSAL) - self.ACTIONS.setPlanner(default_planner, self.ACTIONS.NAVIGATE_TO_POSE) - self.ACTIONS.setPlanner(default_planner, self.ACTIONS.NAVIGATE_THROUGH_POSES) - self.ACTIONS.setPlanner(goal_align_planner, self.ACTIONS.GOAL_ALIGN) - - row_traversal_planner_xy_goal_tolerance = self.get_parameter_or("row_traversal_planner_xy_goal_tolerance", Parameter('double', Parameter.Type.DOUBLE, 0.1)).value - row_traversal_planner_yaw_goal_tolerance = self.get_parameter_or("row_traversal_planner_yaw_goal_tolerance", Parameter('double', Parameter.Type.DOUBLE, 0.1)).value - - default_planner_xy_goal_tolerance = self.get_parameter_or("default_planner_xy_goal_tolerance", Parameter('double', Parameter.Type.DOUBLE, 0.5)).value - default_planner_xy_yaw_goal_tolerance = self.get_parameter_or("default_planner_xy_yaw_goal_tolerance", Parameter('double', Parameter.Type.DOUBLE, 0.3)).value - - goal_align_planner_xy_goal_tolerance = self.get_parameter_or("goal_align_planner_xy_goal_tolerance", Parameter('double', Parameter.Type.DOUBLE, 0.2)).value - goal_align_planner_xy_yaw_goal_tolerance = self.get_parameter_or("goal_align_planner_xy_yaw_goal_tolerance", Parameter('double', Parameter.Type.DOUBLE, 0.1)).value - - - self.ACTIONS.setPlannerParams(row_traversal_planner, row_traversal_planner_xy_goal_tolerance, row_traversal_planner_yaw_goal_tolerance) - self.ACTIONS.setPlannerParams(default_planner, default_planner_xy_goal_tolerance, default_planner_xy_yaw_goal_tolerance) - self.ACTIONS.setPlannerParams(goal_align_planner, goal_align_planner_xy_goal_tolerance, goal_align_planner_xy_yaw_goal_tolerance) - - bt_tree_default = os.path.join(get_package_share_directory('topological_navigation'), 'config', 'bt_tree_default.xml') - bt_tree_goal_align = os.path.join(get_package_share_directory('topological_navigation'), 'config', 'bt_tree_goal_align.xml') - bt_tree_in_row = os.path.join(get_package_share_directory('topological_navigation'), 'config', 'bt_tree_in_row.xml') - - self.bt_trees = {} - self.bt_trees[self.ACTIONS.NAVIGATE_TO_POSE] = self.get_parameter_or(self.ACTIONS.BT_DEFAULT, Parameter('str' - , Parameter.Type.STRING, bt_tree_default)).value - self.bt_trees[self.ACTIONS.ROW_TRAVERSAL] = self.get_parameter_or(self.ACTIONS.BT_IN_ROW - , Parameter('str', Parameter.Type.STRING, bt_tree_in_row)).value - self.bt_trees[self.ACTIONS.GOAL_ALIGN] = self.get_parameter_or(self.ACTIONS.BT_GOAL_ALIGN - , Parameter('str', Parameter.Type.STRING, bt_tree_goal_align)).value - - if self.use_in_row_operation: - bt_tree_in_row_operation = os.path.join(get_package_share_directory('topological_navigation'), 'config', 'bt_tree_in_row_operation.xml') - self.bt_trees[self.ACTIONS.ROW_OPERATION] = self.get_parameter_or(self.ACTIONS.BT_IN_ROW_OPERATION - , Parameter('str', Parameter.Type.STRING, bt_tree_in_row_operation)).value - bt_tree_in_row_recovery = os.path.join(get_package_share_directory('topological_navigation'), 'config', 'bt_tree_in_row_recovery.xml') - self.bt_trees[self.ACTIONS.ROW_RECOVERY] = self.get_parameter_or(self.ACTIONS.BT_IN_ROW_RECOVERY - , Parameter('str', Parameter.Type.STRING, bt_tree_in_row_recovery)).value - - if not self.navigation_action_name in self.navigation_actions: - self.navigation_actions.append(self.navigation_action_name) + # ----------------------------------------------------------------- + # Construction + # ----------------------------------------------------------------- + + def __init__(self, name): + super().__init__(name) + rclpy.get_default_context().on_shutdown(self._on_shutdown) + + # -- State machine ------------------------------------------- + self._sm = NavStateMachine(logger=self.get_logger()) + self._sm.transition(NavState.WAITING_FOR_MAP) + + # -- Runtime flags ------------------------------------------- + self._cancelled = False + self._preempted = False + self._goal_reached = False + self._navigation_activated = False + self._no_orientation = False + self._target = "none" + self._current_target = "none" + + # -- Map data ------------------------------------------------ + self._tmap = None + self._graph = None + self._topol_map = "" + + # -- Deferred map update (thread-safe buffering) ------------- + self._pending_map_msg = None + self._map_lock = threading.RLock() + self._map_updated_during_nav = False + + # -- Map-driven config (populated by _load_map_config) ------- + self._map_definitions = {} + self._map_actions = {} + self._bt_files = {} + self._action_clients = {} # name -> {client, action_class, config} + + # -- Localisation -------------------------------------------- + self._current_node = "Unknown" + self._closest_node = "Unknown" + self._closest_edges = ClosestEdges() + + # -- Nav2 infra ---------------------------------------------- + self._nav2_cb_group = MutuallyExclusiveCallbackGroup() + self._goal_handle = None + self._action_status = GoalStatus.STATUS_UNKNOWN + + # -- Stats --------------------------------------------------- + self._stat = None + + # -- Parameters ---------------------------------------------- + self._declare_parameters() + self._load_parameters() + + # -- QoS ----------------------------------------------------- + self._latch = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) - self.latching_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.keep_history_qos = QoSProfile(reliability = ReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=10, ) - self.stat = None - self.stats_pub = self.create_publisher(NavStatistics, "topological_navigation/Statistics", qos_profile=self.latching_qos) - self.route_pub = self.create_publisher(TopologicalRoute, "topological_navigation/Route", qos_profile= self.keep_history_qos) - self.route_pub_timer = self.create_timer( 2.0 , self.router_pub_timer_callback) - self.stroute = None - self.cur_edge = self.create_publisher(String, "current_edge", qos_profile=self.latching_qos) - self.move_act_pub = self.create_publisher(String, "topological_navigation/move_action_status", qos_profile=self.latching_qos) + self._best = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + ) + + # -- Publishers ---------------------------------------------- + self._stats_pub = self.create_publisher( + NavStatistics, "topological_navigation/Statistics", + qos_profile=self._latch, + ) + self._route_pub = self.create_publisher( + TopologicalRoute, "topological_navigation/Route", + qos_profile=self._best, + ) + self._route_timer = self.create_timer(2.0, self._route_pub_cb) + self._stroute = None + self._cur_edge_pub = self.create_publisher( + String, "current_edge", qos_profile=self._latch, + ) + self._move_status_pub = self.create_publisher( + String, "topological_navigation/move_action_status", + qos_profile=self._latch, + ) + self._boundary_pub = self.create_publisher( + PolygonStamped, "/boundary_checker", + qos_profile=self._latch, + ) + self._status_pub = self.create_publisher( + String, "/robot_operation_current_status", + qos_profile=self._latch, + ) + + # -- Map subscription (blocking wait) ------------------------ self._map_received = False - self._localization_received = False - - self.callback_group_map = ReentrantCallbackGroup() - self.subs_topmap = self.create_subscription(String, '/topological_map_2', self.MapCallback, callback_group=self.callback_group_map, qos_profile=self.latching_qos) - self.get_logger().info("Navigation waiting for the Topological Map...") - - while rclpy.ok(): + self._loc_received = False + self._cb_map = ReentrantCallbackGroup() + self.create_subscription( + String, '/topological_map_2', self._map_cb, + callback_group=self._cb_map, qos_profile=self._latch, + ) + + self.get_logger().info( + "[INIT] Waiting for topological map on /topological_map_2 ...", + ) + self._publish_status("WAITING_FOR_MAP") + while rclpy.ok() and not self._map_received: rclpy.spin_once(self) - if self._map_received: - self.get_logger().info("Navigation received the Topological Map") - break - - self.edge_action_manager = edge_action_manager_server - self.edge_action_manager.init(self.ACTIONS, self.rsearch, self.update_params_control_server, - self.inrow_step_size, self.inrow_step_intermediate_dis) - - self.edge_reconfigure = self.get_parameter_or("reconfigure_edges", Parameter('bool', Parameter.Type.BOOL, True)).value - self.srv_edge_reconfigure = self.get_parameter_or("reconfigure_edges_srv", Parameter('bool', Parameter.Type.BOOL, False)).value - if self.edge_reconfigure: - self.edgeReconfigureManager = EdgeReconfigureManager() - else: - self.get_logger().warn("Edge Reconfigure Unavailable") - self.get_logger().info("Subscribing to Localisation Topics...") - self.subs_closest_node = self.create_subscription(String, 'closest_node', self.closestNodeCallback, qos_profile=self.latching_qos) - while rclpy.ok(): + # Parse map-driven config (definitions, actions, clients) + self._load_map_config() + + self.get_logger().info( + "[INIT] Map '%s' -- %d nodes, %d edges" + % ( + self._topol_map, + self._graph.number_of_nodes(), + self._graph.number_of_edges(), + ), + ) + + # -- Localisation subscription (blocking wait) --------------- + self._sm.transition(NavState.WAITING_FOR_LOCALISATION) + self._publish_status("WAITING_FOR_LOCALISATION") + self.create_subscription( + String, 'closest_node', self._closest_node_cb, + qos_profile=self._latch, + ) + self.get_logger().info("[INIT] Waiting for localisation ...") + while rclpy.ok() and not self._loc_received: rclpy.spin_once(self) - if self._localization_received: - self.get_logger().info("Navigation received the localisation info") - break - - self.subs_closest_edges = self.create_subscription(ClosestEdges, 'closest_edges', self.closestEdgesCallback, qos_profile=self.latching_qos) - self.subs_current_node = self.create_subscription(String, 'current_node', self.currentNodeCallback, qos_profile=self.latching_qos) - - self.get_logger().info("...done") - - try: - self.get_logger().info("Waiting for restrictions...") - self.evaluate_edge_srv = self.create_client(EvaluateEdge, '/restrictions_manager/evaluate_edge') - if not self.evaluate_edge_srv.wait_for_service(timeout_sec=3.0): - self.get_logger().warning('/restrictions_manager/evaluate_edge service not available') - self.get_logger().warning("Restrictions Unavailable") - self.using_restrictions = False - else: - self.evaluate_node_srv = self.create_client(EvaluateNode, '/restrictions_manager/evaluate_node') - self.get_logger().info("Restrictions Available") - self.using_restrictions = True - except Exception as e: - self.get_logger().error("Error while calling servie /restrictions_manager/evaluate_edge {}".format(e)) - - - # this keeps the runtime state of the fail policies that are currently in execution - self.executing_fail_policy = {} - self.callback_group_gotonode = ReentrantCallbackGroup() - self.callback_group_policy = ReentrantCallbackGroup() - - # Creating Action Server for navigation - self.get_logger().info("Creating GO-TO-NODE action server...") - self._as = ActionServer(self, GotoNode, "/" + name, - execute_callback=self.executeCallback, - cancel_callback=self.preemptCallback, - callback_group=self.callback_group_gotonode) - self._as_action_feedback_pub = self.create_publisher(GotoNodeFeedback, - "/" + name + '/feedback', - qos_profile=self.latching_qos) - self.get_logger().info("...done") - - # Creating Action Server for execute policy - self.get_logger().info("Creating EXECUTE POLICY MODE action server...") - self._as_exec_policy = ActionServer(self, ExecutePolicyMode, "/topological_navigation/execute_policy_mode", - execute_callback=self.executeCallbackexecpolicy, - cancel_callback=self.preemptCallbackexecpolicy, - callback_group=self.callback_group_policy) - self._as_exec_policy_action_feedback_pub = self.create_publisher(ExecutePolicyModeFeedback, - 'topological_navigation/execute_policy_mode/feedback', - qos_profile=self.latching_qos) - - - self.get_logger().info("...done") - self.get_logger().info("All Done.") - - - def _on_node_shutdown(self): - if self.navigation_activated: - self.preempted = True - self.cancel_current_action(timeout_secs=2) - - def init_reconfigure(self): - self.nav_planner = self.get_parameter_or("nav_planner", Parameter('str', Parameter.Type.STRING, "dwb_core::DWBLocalPlanner")).value - planner = self.nav_planner.split("/")[-1] - if not planner in self.ACTIONS.planner_with_goal_checker_config: - self.ACTIONS.planner_with_goal_checker_config[planner] = {} - self.get_logger().info("Creating reconfigure client for {}".format(self.nav_planner)) - self.init_dynparams = self.update_params_control_server.get_params() - - - def reconf_movebase(self, cedg, cnode, intermediate): - if cnode["node"]["properties"]["xy_goal_tolerance"] <= 0.1: - cxygtol = 0.1 - else: - cxygtol = cnode["node"]["properties"]["xy_goal_tolerance"] - if not intermediate: - if cnode["node"]["properties"]["yaw_goal_tolerance"] <= 0.087266: - cytol = 0.087266 - else: - cytol = cnode["node"]["properties"]["yaw_goal_tolerance"] - else: - cytol = 6.283 - - params = {"FollowPath.yaw_goal_tolerance": cytol, "FollowPath.xy_goal_tolerance": cxygtol} - self.get_logger().info("Reconfiguring %s with %s" % (self.nav_planner, params)) - print("Intermediate: {}".format(intermediate)) - self.update_params_control_server.set_params(params) + self.get_logger().info( + "[INIT] Localisation OK. Closest: %s" % self._closest_node, + ) + + self.create_subscription( + ClosestEdges, 'closest_edges', self._closest_edges_cb, + qos_profile=self._latch, + ) + self.create_subscription( + String, 'current_node', self._current_node_cb, + qos_profile=self._latch, + ) + + # -- Action servers ------------------------------------------ + self._sm.transition(NavState.READY) + self._publish_status("READY") + + cb_goto = ReentrantCallbackGroup() + cb_policy = ReentrantCallbackGroup() + + self._as_goto = ActionServer( + self, GotoNode, "/" + name, + execute_callback=self._exec_goto_cb, + cancel_callback=self._cancel_goto_cb, + callback_group=cb_goto, + ) + self._goto_fb_pub = self.create_publisher( + GotoNodeFeedback, "/" + name + "/feedback", + qos_profile=self._latch, + ) + + self._as_policy = ActionServer( + self, ExecutePolicyMode, + "/topological_navigation/execute_policy_mode", + execute_callback=self._exec_policy_cb, + cancel_callback=self._cancel_policy_cb, + callback_group=cb_policy, + ) + self._policy_fb_pub = self.create_publisher( + ExecutePolicyModeFeedback, + "topological_navigation/execute_policy_mode/feedback", + qos_profile=self._latch, + ) + + # -- Goal checker service client --------------------------- + gc_node = self._goal_checker_node + self._set_params_client = self.create_client( + SetParameters, + '/%s/set_parameters' % gc_node, + callback_group=ReentrantCallbackGroup(), + ) + self._last_xy_tol = None + self._last_yaw_tol = None + + self.get_logger().info( + "[INIT] Navigation server READY ('%s', algo=%s, weight=%s)" + % (self._topol_map, self._route_algorithm, self._route_weight), + ) + + # ================================================================= + # Parameters + # ================================================================= + + def _declare_parameters(self): + """Declare all ROS 2 parameters.""" + for name, ptype in [ + ('max_dist_to_closest_edge', Parameter.Type.DOUBLE), + ('default_boundary_left', Parameter.Type.DOUBLE), + ('default_boundary_right', Parameter.Type.DOUBLE), + # NetworkX path optimisation + ('route_algorithm', Parameter.Type.STRING), + ('route_weight_attr', Parameter.Type.STRING), + # Nav2 goal checker + ('goal_checker_node', Parameter.Type.STRING), + ('xy_tolerance_param', Parameter.Type.STRING), + ('yaw_tolerance_param', Parameter.Type.STRING), + ]: + self.declare_parameter(name, ptype) + + def _load_parameters(self): + """Read parameter values with sensible defaults.""" + + def _p(name, ptype, default): + return self.get_parameter_or( + name, Parameter('_', ptype, default), + ).value + + self._max_dist_to_closest_edge = _p( + 'max_dist_to_closest_edge', Parameter.Type.DOUBLE, 1.0, + ) + self._default_boundary_left = _p( + 'default_boundary_left', Parameter.Type.DOUBLE, 0.5, + ) + self._default_boundary_right = _p( + 'default_boundary_right', Parameter.Type.DOUBLE, 0.5, + ) + # 'astar' (with Euclidean heuristic) or 'dijkstra' + self._route_algorithm = _p( + 'route_algorithm', Parameter.Type.STRING, 'astar', + ) + # The edge attribute used as the cost for path planning + self._route_weight = _p( + 'route_weight_attr', Parameter.Type.STRING, 'weight', + ) + # Nav2 goal checker node and parameter names + self._goal_checker_node = _p( + 'goal_checker_node', Parameter.Type.STRING, + 'controller_server', + ) + self._xy_tolerance_param = _p( + 'xy_tolerance_param', Parameter.Type.STRING, + 'goal_checker.xy_goal_tolerance', + ) + self._yaw_tolerance_param = _p( + 'yaw_tolerance_param', Parameter.Type.STRING, + 'goal_checker.yaw_goal_tolerance', + ) + + # ================================================================= + # Map-driven configuration + # ================================================================= + + def _load_action_type(self, type_str): + # Example: nav2_msgs.action.NavigateThroughPoses + module_name, class_name = type_str.rsplit('.', 1) + module = importlib.import_module(module_name) + return getattr(module, class_name) + - def reset_reconf(self): - self.update_params_control_server.set_params(self.init_dynparams) + def _load_map_config(self): + """Extract ``definitions`` and ``actions`` from the topomap. - def MapCallback(self, msg): - """ - This Function updates the Topological Map everytime it is called + 1. Writes each BT definition to a temp file. + 2. Resolves ``${definitions.}`` template refs to paths. + 3. Creates ``ActionClient`` instances for unique servers. """ - # self.lnodes = json.loads(msg.data) - self.lnodes = yaml.load( msg.data, Loader=CustomSafeLoader ) - self.topol_map = self.lnodes["pointset"] - self.rsearch = TopologicalRouteSearch2(self.lnodes) - self.route_checker = RouteChecker(self.lnodes) - self.make_navigation_edge() - self._map_received = True - - - def make_navigation_edge(self): - - self.navigation_action_edge = {} - self.navigation_action_edge["action"] = self.navigation_action_name - self.navigation_action_edge["edge_id"] = "navigation_action_edge" - navigation_action_goal = {} - if not navigation_action_goal: - for node in self.lnodes["nodes"]: - for edge in node["node"]["edges"]: - if edge["action"] == self.navigation_action_name: - navigation_action_goal["action_type"] = edge["action_type"] - navigation_action_goal["goal"] = edge["goal"] - break + self._map_definitions = self._tmap.get('definitions', {}) + self._map_actions = self._tmap.get('actions', {}) + + if not self._map_actions: + self.get_logger().warning( + "[MAP] No 'actions' section -- behaviour undefined", + ) + return + + # -- Write BT definitions to temp files ---------------------- + bt_dir = os.path.join(tempfile.gettempdir(), 'topo_nav_bt') + os.makedirs(bt_dir, exist_ok=True) + + self._bt_files = {} + for name, content in self._map_definitions.items(): + path = os.path.join(bt_dir, '%s.xml' % name) + with open(path, 'w') as fh: + fh.write(content) + self._bt_files[name] = path + self.get_logger().info( + "[MAP] BT '%s' -> %s" % (name, path), + ) + + # -- Resolve ${definitions.} refs ----------------------- + for act_name, act_cfg in self._map_actions.items(): + tpl = act_cfg.get('action_goal_template', {}) + bt_ref = tpl.get('behavior_tree', '') + if ( + isinstance(bt_ref, str) + and bt_ref.startswith('${definitions.') + and bt_ref.endswith('}') + ): + key = bt_ref[len('${definitions.'):-1] + if key in self._bt_files: + tpl['behavior_tree'] = self._bt_files[key] else: - continue - break - if not navigation_action_goal: - navigation_action_goal["action_type"] = "geometry_msgs/PoseStamped" - navigation_action_goal["goal"] = {} - navigation_action_goal["goal"]["target_pose"] = {} - navigation_action_goal["goal"]["target_pose"]["pose"] = "$node.pose" - navigation_action_goal["goal"]["target_pose"]["header"] = {} - navigation_action_goal["goal"]["target_pose"]["header"]["frame_id"] = "$node.parent_frame" - - self.navigation_action_edge["action_type"] = navigation_action_goal["action_type"] - self.navigation_action_edge["goal"] = navigation_action_goal["goal"] - self.get_logger().info("Move Base Goal set to {}".format(navigation_action_goal["action_type"])) - - - def executeCallback(self, goal): - """ - This Functions is called when the topo nav Action Server is called + self.get_logger().warning( + "[MAP] '%s' ref not found for action '%s'" + % (key, act_name), + ) + + self.get_logger().info( + "[MAP] Action '%s': type=%s server=%s composable=%s" + % ( + act_name, + act_cfg.get('action_type', '?'), + act_cfg.get('action_server', '?'), + act_cfg.get('composable', True), + ), + ) + + # -- Create action clients ----------------------------------- + self._create_action_clients() + + def _create_action_clients(self): + """Create ``ActionClient`` instances from the map ``actions``. + + Clients are shared when multiple actions target the same + ``action_server``. """ - self.get_logger().info("\n####################################################################################################") - self.get_logger().info("Processing GO-TO-NODE goal (No Orientation = {})".format(goal.request.no_orientation)) - - status = self.edge_action_manager.get_state() - - can_start = False - if self.cancel_current_action(timeout_secs=10): - # we successfully stopped the previous action, claim the title to activate navigation - self.navigation_activated = True - can_start = True - if can_start: - self.cancelled = False - self.preempted = False - self.final_goal = False - self.no_orientation = goal.request.no_orientation - self.executing_fail_policy = {} - self._feedback.route = "Starting..." - self._as_action_feedback_pub.publish(self._feedback) - self.navigate(goal.request.target) - else: - self.get_logger().warning("Could not cancel current navigation action, GO-TO-NODE goal aborted") + created = {} # action_server -> (client, action_class) + self._action_clients = {} - self.navigation_activated = False - nav_current_state = self.edge_action_manager.get_state() - if (nav_current_state == GoalStatus.STATUS_SUCCEEDED): - goal.succeed() - else: - goal.abort() - - self.get_logger().warning("Done processing the nav action GO-TO-NODE....") - result = GotoNode.Result() - result.success = self._result.success - - return result - - def executeCallbackexecpolicy(self, goal): - """ - This Function is called when the execute policy Action Server is called - """ - self.get_logger().info("\n####################################################################################################") - self.get_logger().info("Processing EXECUTE-POLICY-MODE goal") - can_start = False - if self.cancel_current_action(timeout_secs=10): - # we successfully stopped the previous action, claim the title to activate navigation - self.navigation_activated = True - can_start = True - - if can_start: - - self.cancelled = False - self.preempted = False - self.final_goal = False - - self.max_dist_to_closest_edge = self.get_parameter_or("max_dist_to_closest_edge", Parameter('double', Parameter.Type.DOUBLE, 1.0)).value - self.nav_from_closest_edge = False if self.closest_edges.distances[0] > self.max_dist_to_closest_edge or self.current_node != "none" else True - - route = goal.request.route - valid_route = self.route_checker.check_route(route) - - if valid_route and (route.source[0] == self.current_node or route.source[0] == self.closest_node): - final_edge = get_edge_from_id_tmap2(self.lnodes, route.source[-1], route.edge_id[-1]) - target = final_edge["node"] - route = self.enforce_navigable_route(route, target) - self.get_logger().warn("[executeCallbackexecpolicy] - publishing route") - self.publish_route(route, target) - result = self.execute_policy(route, target) - else: - result = False - self.cancelled = True - self.get_logger().error("Invalid route in execute policy mode goal") + for act_name, act_cfg in self._map_actions.items(): + type_str = act_cfg.get('action_type', '') + server = act_cfg.get('action_server', '') - if not self.cancelled and not self.preempted: - self._result_exec_policy.success = result + # Dynamically import the action type (e.g. nav2_msgs.action.NavigateToPose) + try: + action_class = self._load_action_type(type_str) + except Exception as exc: + self.get_logger().warning( + "[MAP] Cannot load action_type '%s' for '%s': %s" + % (type_str, act_name, exc), + ) + continue + + if server in created: + client, existing = created[server] + if existing != action_class: + self.get_logger().error( + "[MAP] Conflicting types on '%s': %s vs %s" + % (server, existing.__name__, action_class.__name__), + ) + continue else: - if not self.preempted: - self._result_exec_policy.success = result - else: - self._result_exec_policy.success = False - else: - self.get_logger().warning("Could not cancel current navigation action, EXECUTE POLICY MODE goal aborted.") - - self.navigation_activated = False - goal.succeed() - self.get_logger().warning("Done processing the nav action EXECUTE POLICY MODE....") - result = ExecutePolicyMode.Result() - result.success = self._result_exec_policy.success - return result - - - def preemptCallback(self, goal_handle): - self.get_logger().warning("Preempting GO-TO-NODE goal") - self.preempted = True - self.cancel_current_action(timeout_secs=2) - - - def preemptCallbackexecpolicy(self, goal_handle): - self.get_logger().warning("Preempting EXECUTE POLICY MODE goal") - self.preempted = True - self.cancel_current_action(timeout_secs=2) - - - def closestNodeCallback(self, msg): - self._localization_received = True - self.closest_node = msg.data + client = ActionClient( + self, action_class, server, + callback_group=self._nav2_cb_group, + ) + created[server] = (client, action_class) + self.get_logger().info( + "[MAP] Client: %s -> %s" + % (action_class.__name__, server), + ) + + self._action_clients[act_name] = { + 'client': client, + 'action_class': action_class, + 'config': act_cfg, + } + # ================================================================= + # Lifecycle + # ================================================================= - def closestEdgesCallback(self, msg): - self.closest_edges = msg + def _on_shutdown(self): + self.get_logger().info("[SHUTDOWN] Tearing down") + if self._navigation_activated: + self._preempted = True + self._cancel_nav2_goal(timeout_sec=2.0) + # ================================================================= + # Topic callbacks + # ================================================================= - def currentNodeCallback(self, msg): - if self.current_node != msg.data: # is there any change on this topic? - self.current_node = msg.data # we are at this new node - if msg.data != "none": # are we at a node? - self.get_logger().info("New node reached: {}".format(self.current_node)) - if self.navigation_activated: # is the action server active? - if self.stat: - self.stat.set_at_node() - # if the robot reached and intermediate node and the next action is move base goal has been reached - if ( - self.current_node == self.current_target - and self._target != self.current_target - and self.next_action in self.navigation_actions - and self.current_action in self.navigation_actions - and self.fluid_navigation - ): - self.get_logger().info("Intermediate node reached: {}".format(self.current_node)) - self.goal_reached = True + def _map_cb(self, msg): + """``/topological_map_2`` callback -- buffer map update. - def followRoute(self, route, target, exec_policy): - """ - This function follows the chosen route to reach the goal. + The first map is applied immediately (needed for init). All + subsequent maps received during active navigation are buffered + and applied at safe points between route segments. """ - self.navigation_activated = True - - nnodes = len(route.source) - Orig = route.source[0] - Targ = target - self._target = Targ - - self.init_reconfigure() - self.get_logger().info(" {} Nodes on route".format(nnodes)) - - inc = 1 - rindex = 0 - nav_ok = True - recovering = False - replanned = False - self.fluid_navigation = True - - o_node = self.rsearch.get_node_from_tmap2(Orig) - edge_from_id = get_edge_from_id_tmap2(self.lnodes, route.source[0], route.edge_id[0]) - if edge_from_id: - a = edge_from_id["action"] - self.get_logger().info("First action: {}".format(a)) + with self._map_lock: + self._pending_map_msg = msg + if not self._map_received: + # First map: apply immediately so __init__ can proceed. + self._apply_pending_map() + return + + if self._navigation_activated: + self.get_logger().info( + "[MAP] Update buffered (navigation active)", + ) else: - self.get_logger().error("Failed to get edge from id {}. Invalid route".format(route.edge_id[0])) - return False, inc - - if not self.nav_from_closest_edge: - # If the robot is not on a node or the first action is not move base type - # navigate to closest node waypoint (only when first action is not move base) - if a not in self.navigation_actions: - self.get_logger().info("The action of the first edge in the route is not a navigate to pose action") - self.get_logger().info("Current node is {}".format(self.current_node)) - - if self.current_node == "none" and a not in self.navigation_actions: - self.next_action = a - self.get_logger().info("Do {} to origin {}".format(self.navigation_action_name, o_node["node"]["name"])) - - # 5 degrees tolerance - params = {"yaw_goal_tolerance": 0.087266} - self.update_params_control_server.set_params(params) - - self.current_target = Orig - nav_ok, inc = self.execute_action(self.navigation_action_edge, o_node) - self.get_logger().info("Navigation Finished Successfully") if nav_ok else self.get_logger().warning("Navigation Failed") - - elif a not in self.navigation_actions: - navigation_action_act = False - for i in o_node["node"]["edges"]: - # Check if there is a navigation action in the edages of this node - # if not is dangerous to move - if i["action"] in self.navigation_actions: - navigation_action_act = True - - if not navigation_action_act: - self.get_logger().warning("Could not find a move base action in the edges of origin {}. Unsafe to move".format(o_node["node"]["name"])) - self.get_logger().info("Action not taken, outputing success") - nav_ok = True - inc = 0 - else: - self.get_logger().info("Getting to the exact pose of origin {}".format(o_node["node"]["name"])) - self.current_target = Orig - nav_ok, inc = self.execute_action(self.navigation_action_edge, o_node) - self.get_logger().info("Navigation Finished Successfully") if nav_ok else self.get_logger().warning("Navigation Failed") - - while rindex < (len(route.edge_id)) and not self.cancelled and (nav_ok or recovering): - cedg = get_edge_from_id_tmap2(self.lnodes, route.source[rindex], route.edge_id[rindex]) - a = cedg["action"] - - if rindex < (len(route.edge_id) - 1): - nedge = get_edge_from_id_tmap2(self.lnodes, route.source[rindex + 1], route.edge_id[rindex + 1]) - a1 = nedge["action"] - self.fluid_navigation = nedge["fluid_navigation"] - else: - nedge = None - a1 = "none" - self.fluid_navigation = False - self.final_goal = True - - self.current_action = a - self.next_action = a1 - - self.get_logger().info("From {} do ({}) to {}".format(route.source[rindex], a, cedg["node"])) - - current_edge = "%s--%s" % (cedg["edge_id"], self.topol_map) - self.get_logger().info("Current edge: {}".format(current_edge)) - msg = String() - msg.data = current_edge - self.cur_edge.publish(msg) - - if not exec_policy: - self._feedback.route = "%s to %s using %s" % (route.source[rindex], cedg["node"], a) - self._as_action_feedback_pub.publish(self._feedback) - else: - self.publish_feedback_exec_policy() - - cnode = self.rsearch.get_node_from_tmap2(cedg["node"]) - onode = self.rsearch.get_node_from_tmap2(route.source[rindex]) - - # do not care for the orientation of the waypoint if is not the last waypoint AND - # the current and following action are navigation or human_aware_navigation - # and when the fuild_navigation is true - if rindex < len(route.edge_id) - 1 and a1 in self.navigation_actions and a in self.navigation_actions and self.fluid_navigation: - self.reconf_movebase(cedg, cnode, True) - else: - if self.no_orientation: - self.reconf_movebase(cedg, cnode, True) - else: - self.reconf_movebase(cedg, cnode, False) - - self.current_target = cedg["node"] - - self.stat = nav_stats(route.source[rindex], cedg["node"], self.topol_map, cedg["edge_id"]) - dt_text = self.stat.get_start_time_str() - - self.edge_reconf_start(cedg) - if exec_policy: - nav_ok, inc = self.execute_action(cedg, cnode, onode) - else: - nav_ok, inc, recovering, route, replanned = self.execute_action_fail_recovery(cedg, cnode, route, rindex, onode, target) - self.edge_reconf_end() - - params = {"yaw_goal_tolerance": 0.087266, "xy_goal_tolerance": 0.1} - self.update_params_control_server.set_params(params) - - not_fatal = nav_ok - if self.cancelled: - nav_ok = True - if self.preempted: - not_fatal = False - nav_ok = False - - - self.stat.set_ended(self.current_node) - dt_text=self.stat.get_finish_time_str() - operation_time = self.stat.operation_time - time_to_wp = self.stat.time_to_wp - - if nav_ok: - self.stat.status = "success" - self.get_logger().info("Navigation Finished on {} ({}/{})".format(dt_text, operation_time, time_to_wp)) - else: - if not_fatal: - self.get_logger().warning("Navigation Failed on {} ({}/{})".format(dt_text, operation_time, time_to_wp)) - self.stat.status = "failed" - else: - self.get_logger().warning("Fatal Fail on {} ({}/{})".format(dt_text, operation_time, time_to_wp)) - self.stat.status = "fatal" - - self.publish_stats() + # Not navigating -- safe to apply immediately. + self._apply_pending_map() - current_edge = "none" - msg = String() - msg.data = current_edge - self.cur_edge.publish(msg) + def _apply_pending_map(self): + """Apply a buffered map update. Thread-safe. - self.current_action = "none" - self.next_action = "none" - - if not replanned: - rindex = rindex + 1 - - self.reset_reconf() - - self.navigation_activated = False - - result = nav_ok - return result, inc - - def navigate_to_poses(self, route, target, exec_policy): - """ - This function follows the chosen route to reach the goal. + Returns ``True`` if a new map was applied, ``False`` if there + was nothing pending or the update failed. """ - self.navigation_activated = True - - nnodes = len(route.source) - Orig = route.source[0] - Targ = target - self._target = Targ - - self.init_reconfigure() - self.get_logger().info(" {} Nodes on route".format(nnodes)) - - inc = 1 - rindex = 0 - nav_ok = True - recovering = False - replanned = False - self.fluid_navigation = True - - o_node = self.rsearch.get_node_from_tmap2(Orig) - edge_from_id = get_edge_from_id_tmap2(self.lnodes, route.source[0], route.edge_id[0]) - if edge_from_id: - a = edge_from_id["action"] - self.get_logger().info("First action: {}".format(a)) - else: - self.get_logger().error("Failed to get edge from id {}. Invalid route".format(route.edge_id[0])) - return False, inc - - if not self.nav_from_closest_edge: - # If the robot is not on a node or the first action is not move base type - # navigate to closest node waypoint (only when first action is not move base) - if a not in self.navigation_actions: - self.get_logger().info("The action of the first edge in the route is not a navigate to poses action") - self.get_logger().info("Current node is {}".format(self.current_node)) - - if self.current_node == "none" and a not in self.navigation_actions: - self.next_action = a - self.get_logger().info("Do {} to origin {}".format(self.navigation_action_name, o_node["node"]["name"])) - - # 5 degrees tolerance - params = {"yaw_goal_tolerance": 0.087266} - self.update_params_control_server.set_params(params) - - self.current_target = Orig - nav_ok, inc = self.execute_action(self.navigation_action_edge, o_node) - self.get_logger().info("Navigation Finished Successfully") if nav_ok else self.get_logger().warning("Navigation Failed") - - elif a not in self.navigation_actions: - navigation_action_act = False - for i in o_node["node"]["edges"]: - # Check if there is a navigation action in the edages of this node - # if not is dangerous to move - if i["action"] in self.navigation_actions: - navigation_action_act = True - - if not navigation_action_act: - self.get_logger().warning("Could not find a nav action in the edges of origin {}. Unsafe to move".format(o_node["node"]["name"])) - self.get_logger().info("Action not taken, outputing success") - nav_ok = True - inc = 0 - else: - self.get_logger().info("Getting to the exact pose of origin {}".format(o_node["node"]["name"])) - self.current_target = Orig - nav_ok, inc = self.execute_action(self.navigation_action_edge, o_node) - self.get_logger().info("Navigation Finished Successfully") if nav_ok else self.get_logger().warning("Navigation Failed") - - - route_edges = [] - route_dests = [] - route_origins = [] - route_actions_list = [] - cedg = get_edge_from_id_tmap2(self.lnodes, route.source[rindex], route.edge_id[rindex]) - self.stat = nav_stats(route.source[rindex], cedg["node"], self.topol_map, cedg["edge_id"]) - dt_text = self.stat.get_start_time_str() - - while rindex < (len(route.edge_id)): - cedg = get_edge_from_id_tmap2(self.lnodes, route.source[rindex], route.edge_id[rindex]) - a = cedg["action"] - route_actions_list.append(a) - - if(rindex == (len(route.edge_id)-1)): - self.stat.target = cedg["edge_id"] - - if rindex < (len(route.edge_id) - 1): - nedge = get_edge_from_id_tmap2(self.lnodes, route.source[rindex + 1], route.edge_id[rindex + 1]) - a1 = nedge["action"] - self.fluid_navigation = nedge["fluid_navigation"] - else: - nedge = None - a1 = "none" - self.fluid_navigation = False - self.final_goal = True - - self.current_action = a - self.next_action = a1 - current_edge = "%s--%s" % (cedg["edge_id"], self.topol_map) - self.get_logger().info("From {} do ({}) to {} from edge {}".format(route.source[rindex], a, cedg["node"], current_edge)) - cnode = self.rsearch.get_node_from_tmap2(cedg["node"]) - onode = self.rsearch.get_node_from_tmap2(route.source[rindex]) - self.current_target = cedg["node"] - route_edges.append(cedg) - route_dests.append(cnode) - route_origins.append(onode) - rindex = rindex + 1 - - self.get_logger().info(" ========== Action list {} ".format(route_actions_list)) - - if self.allow_intermediate_orientation_override: - # ====================================================================== - # ## NEW LOGIC START: Realign Orientations for Continuous Flow - # ====================================================================== - # We iterate through all destinations except the very last one. - # We point each node to look at the NEXT node. - - for i in range(len(route_dests) - 1): - curr_node = route_dests[i] - next_node = route_dests[i+1] - - # 1. Get positions - # Note: Adjust keys ["pose"]["position"] if your dictionary structure is different - cx = curr_node["node"]["pose"]["position"]["x"] - cy = curr_node["node"]["pose"]["position"]["y"] - nx = next_node["node"]["pose"]["position"]["x"] - ny = next_node["node"]["pose"]["position"]["y"] - - # 2. Calculate the specific angle (Yaw) to the next node - dx = nx - cx - dy = ny - cy - yaw = math.atan2(dy, dx) - - # 3. Convert Yaw to Quaternion manually (to avoid extra dependencies) - # Formula for Z-axis rotation - qz = math.sin(yaw * 0.5) - qw = math.cos(yaw * 0.5) - - # 4. Overwrite the orientation of the intermediate node - # Now the planner thinks this node is "facing" the path, so it won't stop to turn. - curr_node["node"]["pose"]["orientation"]["x"] = 0.0 - curr_node["node"]["pose"]["orientation"]["y"] = 0.0 - curr_node["node"]["pose"]["orientation"]["z"] = qz - curr_node["node"]["pose"]["orientation"]["w"] = qw - - self.get_logger().info(f"Realigned node {i} to yaw: {yaw:.2f}") - - # The LAST node (route_dests[-1]) is left untouched so it keeps - # the final desired docking/goal orientation. - # ====================================================================== - # ## NEW LOGIC END - # ====================================================================== - - nav_ok, inc, status = self.execute_actions(route_edges, route_dests, route_origins, - action_name=self.ACTIONS.NAVIGATE_THROUGH_POSES, is_execpolicy=exec_policy) - - if(self.stat is None): - self.stat = nav_stats(route.source[0], cedg["node"], self.topol_map, cedg["edge_id"]) - - self.stat.set_ended(self.current_node) - dt_text = self.stat.get_finish_time_str() - operation_time = self.stat.operation_time - time_to_wp = self.stat.time_to_wp - not_fatal = nav_ok - if self.cancelled: - nav_ok = True - if self.preempted: - not_fatal = False - nav_ok = False - - if nav_ok: - self.stat.status = "success" - self.get_logger().info("Navigation Finished on {} ({}/{})".format(dt_text, operation_time, time_to_wp)) - else: - if not_fatal: - self.get_logger().warning("Navigation Failed on {} ({}/{})".format(dt_text, operation_time, time_to_wp)) - self.stat.status = "failed" - else: - self.get_logger().warning("Fatal Fail on {} ({}/{})".format(dt_text, operation_time, time_to_wp)) - self.stat.status = "fatal" - + with self._map_lock: + if self._pending_map_msg is None: + return False + msg = self._pending_map_msg + self._pending_map_msg = None - self.publish_stats() - self.reset_reconf() - self.navigation_activated = False - result = nav_ok - return result, inc, status - - def navigate(self, target): + try: + tmap = yaml.load(msg.data, Loader=_FloatSafeLoader) + graph = build_graph_from_tmap( + tmap, logger=self.get_logger(), + ) + if graph is None: + self.get_logger().error("[MAP] Graph build failed") + return False + + self._tmap = tmap + self._graph = graph + self._topol_map = tmap.get("pointset", "unknown") + self._map_received = True + self._load_map_config() + + self.get_logger().info( + "[MAP] Applied update '%s' -- %d nodes, %d edges" + % ( + self._topol_map, + graph.number_of_nodes(), + graph.number_of_edges(), + ), + ) + return True + except Exception as exc: + self.get_logger().error("[MAP] Apply error: %s" % exc) + return False + + def _validate_remaining_route(self, route_nodes, start_idx): + """Check that remaining nodes and edges still exist in the graph. + + Parameters + ---------- + route_nodes : list[str] + Full ordered list of node names in the route. + start_idx : int + Index into *route_nodes* of the segment about to execute + (i.e. remaining = route_nodes[start_idx:]). + + Returns + ------- + bool + ``True`` if every remaining node exists in ``self._graph`` + and every consecutive pair is connected by an edge. """ - This function takes the target node and plans the actions that are required - to reach it. + remaining = route_nodes[start_idx:] + for node_name in remaining: + if node_name not in self._graph: + self.get_logger().warning( + "[MAP-CHECK] Node '%s' no longer in map" + % node_name, + ) + return False + + for i in range(len(remaining) - 1): + src, tgt = remaining[i], remaining[i + 1] + if not self._graph.has_edge(src, tgt): + self.get_logger().warning( + "[MAP-CHECK] Edge '%s' -> '%s' no longer in map" + % (src, tgt), + ) + return False + + return True + + def _closest_node_cb(self, msg): + self._loc_received = True + self._closest_node = msg.data + + def _closest_edges_cb(self, msg): + self._closest_edges = msg + + def _current_node_cb(self, msg): + if self._current_node != msg.data: + self._current_node = msg.data + if msg.data != "none": + self.get_logger().info( + "[LOC] Entered node: %s" % self._current_node, + ) + if ( + self._navigation_activated + and self._current_node == self._current_target + ): + self.get_logger().info( + "[LOC] Reached target: %s" % self._current_target, + ) + self._goal_reached = True + + def _route_pub_cb(self): + if self._stroute and self._stroute.nodes: + self._route_pub.publish(self._stroute) + + # ================================================================= + # Status publishers + # ================================================================= + + def _publish_status(self, state_str): + msg = String() + msg.data = state_str + self._status_pub.publish(msg) + + def _publish_boundary(self, polygon_pts, frame_id="map"): + msg = PolygonStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = frame_id + for x, y in polygon_pts: + pt = Point32() + pt.x = float(x) + pt.y = float(y) + pt.z = 0.0 + msg.polygon.points.append(pt) + self._boundary_pub.publish(msg) + self.get_logger().info( + "[BOUNDARY] %d-point polygon (frame=%s)" + % (len(polygon_pts), frame_id), + ) + + def _publish_empty_boundary(self, frame_id="map"): + msg = PolygonStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = frame_id + self._boundary_pub.publish(msg) + + def _publish_route(self, route_nodes): + stroute = TopologicalRoute() + for n in route_nodes: + stroute.nodes.append(n) + self._stroute = stroute + self._route_pub.publish(stroute) + + def _publish_stats(self): + if self._stat is None: + return + s = self._stat + msg = NavStatistics() + msg.edge_id = s.edge_id + msg.status = s.status + msg.origin = s.origin + msg.target = s.target + msg.topological_map = s.topological_map + msg.time_to_waypoint = float(s.time_to_wp) + msg.operation_time = s.operation_time + msg.date_started = s.get_start_time_str() + msg.date_at_node = s.date_at_node.strftime( + "%A, %B %d %Y, at %H:%M:%S hours", + ) + msg.date_finished = s.get_finish_time_str() + self._stats_pub.publish(msg) + + def _publish_current_edge(self, edge_id): + msg = String() + msg.data = ( + "%s--%s" % (edge_id, self._topol_map) + if edge_id != "none" else "none" + ) + self._cur_edge_pub.publish(msg) + + def _publish_move_status(self, goal_node, action, status_str): + d = { + "goal": goal_node, + "final_goal": self._target, + "action": action.upper(), + "status": status_str, + } + msg = String() + msg.data = json.dumps(d) + self._move_status_pub.publish(msg) + + # ================================================================= + # Pose construction + # ================================================================= + + def _nav_frame(self): + """Default navigation frame from ``transformation.topo_frame_id``. + + Falls back to ``transformation.parent``, then ``'map'``. """ - result = False - if not self.cancelled: - g_node = self.rsearch.get_node_from_tmap2(target) - - self.max_dist_to_closest_edge = self.get_parameter_or("max_dist_to_closest_edge", Parameter('double', Parameter.Type.DOUBLE, 1.0)).value - - # if we are nowhere near an edge or not at a node, then do a node plan - if self.closest_edges.distances and (self.closest_edges.distances[0] > self.max_dist_to_closest_edge or self.current_node != "none"): - self.nav_from_closest_edge = False - o_node = self.rsearch.get_node_from_tmap2(self.closest_node) - self.get_logger().info("Planning from the closest NODE: {}".format(self.closest_node)) - else: - self.nav_from_closest_edge = True - o_node, the_edge = self.orig_node_from_closest_edge(g_node) - # This creates essentially a fake "previous node" to address the edge case when navigating over a single edge and the closest node is on the edge but the current node isnt. (otherwise it will mark as complete without navigation). - if o_node == target: - o_node, the_edge = self.orig_node_from_closest_edge(g_node, flip=True) - self.get_logger().info("Planning from the closest EDGE: {}".format(the_edge["edge_id"])) - - self.get_logger().info("Navigating From Origin {} to Target {} ".format(o_node["node"]["name"], target)) - - # Everything is Awesome!!! - # Target and Origin are not None - if (g_node is not None) and (o_node is not None): - if g_node["node"]["name"] != o_node["node"]["name"]: - route = self.rsearch.search_route(o_node["node"]["name"], target) #! <---- HERE YOU RETRIEVE THE SHORTEST ROUTE - route = self.enforce_navigable_route(route, target) - if route.source: - self.get_logger().info("Navigating Case 1: Following route") - self.route = route - self.target = target - self.publish_route(route, target) - if(self.use_nav2_follow_route): - result, inc, status = self.navigate_to_poses(route, target, 0) - else: - result, inc = self.followRoute(route, target, 0) - self.get_logger().info("Navigating Case 1 -> res: {}".format(inc)) - else: - self.get_logger().warning("Navigating Case 1a: There is no route from {} to {}. Check your edges.".format(o_node["node"]["name"], target)) - self.cancelled = True - result = False - inc = 1 - self.get_logger().info("Navigating Case 1a -> res: {}".format(inc)) - else: - if self.nav_from_closest_edge: - result, inc = self.to_goal_node(g_node, the_edge) - else: - result, inc = self.to_goal_node(g_node) - else: - self.get_logger().warning("Navigating Case 3: Target or Origin Nodes were not found on Map") - self.cancelled = True - result = False - inc = 1 - self.get_logger().info("Navigating Case 3 -> res: {}".format(inc)) - - if (not self.cancelled) and (not self.preempted): - self._result.success = result - if result: - self._feedback.route = target - self._as_action_feedback_pub.publish(self._feedback) - # self._as.set_succeeded(self._result) - else: - self._feedback.route = self.current_node - self._as_action_feedback_pub.publish(self._feedback) - # self._as.set_aborted(self._result) - else: - if not self.preempted: - self._feedback.route = self.current_node - self._as_action_feedback_pub.publish(self._feedback) - self._result.success = result - # self._as.set_aborted(self._result) - else: - self._result.success = False - # self._as.set_preempted(self._result) - - - - def execute_policy(self, route, target): - # if(False): - if(self.use_nav2_follow_route): - result, inc, status = self.navigate_to_poses(route, target, 1) - else: - result, inc = self.followRoute(route, target, 1) # <-- IT DOESNT WORK [LUCA] - if result: - self.get_logger().info("Navigation Finished Successfully") - self.publish_feedback_exec_policy(GoalStatus.STATUS_SUCCEEDED) - else: - if self.cancelled and self.preempted: - self.get_logger().warning("Fatal Fail") - self.publish_feedback_exec_policy(GoalStatus.STATUS_CANCELED) - elif self.cancelled: - self.get_logger().warning("Navigation Failed") - self.publish_feedback_exec_policy(GoalStatus.STATUS_ABORTED) - return result - + tx = self._tmap.get('transformation', {}) + return tx.get('topo_frame_id', tx.get('parent', 'map')) + def _node_nav_frame(self, node_name): + """Resolve the navigation frame for a specific node. - def publish_feedback_exec_policy(self, nav_outcome=None): - if self.current_node == "none": # Happens due to lag in fetch system - if self.current_node == "none": - self._feedback_exec_policy.current_wp = self.closest_node - else: - self._feedback_exec_policy.current_wp = self.current_node - else: - self._feedback_exec_policy.current_wp = self.current_node - if nav_outcome is not None: - self._feedback_exec_policy.status = nav_outcome - self._as_exec_policy_action_feedback_pub.publish(self._feedback_exec_policy) - - - def orig_node_from_closest_edge(self, g_node, flip=False): - - name_1, _ = get_node_names_from_edge_id_2(self.lnodes, self.closest_edges.edge_ids[0]) - name_2, _ = get_node_names_from_edge_id_2(self.lnodes, self.closest_edges.edge_ids[1]) - - # Navigate from the closest edge instead of the closest node. First get the closest edges. - edge_1 = get_edge_from_id_tmap2(self.lnodes, name_1, self.closest_edges.edge_ids[0]) - edge_2 = get_edge_from_id_tmap2(self.lnodes, name_2, self.closest_edges.edge_ids[1]) - - # Then get their destination nodes. - o_node_1 = self.rsearch.get_node_from_tmap2(edge_1["node"]) - o_node_2 = self.rsearch.get_node_from_tmap2(edge_2["node"]) - - # If the closest edges are of equal distance (usually a bidirectional edge) - # then use the destination node that results in a shorter route to the goal. - if self.closest_edges.distances[0] == self.closest_edges.distances[1]: - d1 = get_route_distance(self.lnodes, o_node_1, g_node) - d2 = get_route_distance(self.lnodes, o_node_2, g_node) - else: # Use the destination node of the closest edge. - d1 = 0; d2 = 1 - if flip: - if d1 <= d2: - return o_node_1, edge_1 - else: - return o_node_2, edge_2 + Lookup order: + 1. ``nav_frame`` attribute on the graph node (per-node override) + 2. Map-level default via :meth:`_nav_frame` + """ + if node_name and node_name in self._graph: + nf = self._graph.nodes[node_name].get('nav_frame', '') + if nf: + return nf + return self._nav_frame() + + def _build_pose_stamped(self, node_dict, ignore_orientation=False): + """Build ``PoseStamped`` from a raw topomap node dict.""" + nd = node_dict["node"] + pose = nd["pose"] + ps = PoseStamped() + ps.header.stamp = self.get_clock().now().to_msg() + raw_frame = nd.get("nav_frame", '') + if raw_frame and raw_frame.startswith('${'): + raw_frame = self._resolve_tmap_ref(raw_frame) + ps.header.frame_id = raw_frame or self._nav_frame() + ps.pose.position.x = float(pose["position"]["x"]) + ps.pose.position.y = float(pose["position"]["y"]) + ps.pose.position.z = float(pose["position"]["z"]) + if ignore_orientation: + ps.pose.orientation.w = 1.0 else: - o_node_1b = self.rsearch.get_node_from_tmap2(name_1) - o_node_2b = self.rsearch.get_node_from_tmap2(name_2) - if d1 <= d2: - return o_node_1b, edge_1 - else: - return o_node_2b, edge_2 - - def to_goal_node(self, g_node, the_edge=None): - self.get_logger().info("Target and Origin Nodes are the same") - self.current_target = g_node["node"]["name"] - if the_edge is None: - # Check if there is a navigation action in the edges of this node and choose the earliest one in the - # list of navigation actions. If not is dangerous to move. - act_ind = 100 - for i in g_node["node"]["edges"]: - c_action_server = i["action"] - if c_action_server in self.navigation_actions: - c_ind = self.navigation_actions.index(c_action_server) - if c_ind < act_ind: - act_ind = c_ind - the_edge = i - if the_edge is None: - self.get_logger().warning("Navigating Case 2a: Could not find a move base action in the edges of target {}. Unsafe to move".format(g_node["node"]["name"])) - self.get_logger().info("Action not taken, outputting success") - result=True - inc=0 - self.get_logger().info("Navigating Case 2a -> res: {}".format(inc)) + ps.pose.orientation.x = float(pose["orientation"]["x"]) + ps.pose.orientation.y = float(pose["orientation"]["y"]) + ps.pose.orientation.z = float(pose["orientation"]["z"]) + ps.pose.orientation.w = float(pose["orientation"]["w"]) + return ps + + def _build_pose_from_graph(self, node_name, ignore_orientation=False): + """Build ``PoseStamped`` from NetworkX graph attributes.""" + if node_name not in self._graph: + return None + attrs = self._graph.nodes[node_name] + ps = PoseStamped() + ps.header.stamp = self.get_clock().now().to_msg() + ps.header.frame_id = self._node_nav_frame(node_name) + ps.pose.position.x = float(attrs.get('x', 0.0)) + ps.pose.position.y = float(attrs.get('y', 0.0)) + ps.pose.position.z = float(attrs.get('z', 0.0)) + if ignore_orientation: + ps.pose.orientation.w = 1.0 else: - self.get_logger().info("Navigating Case 2: Getting to the exact pose of target {}".format(g_node["node"]["name"])) - self.final_goal = True - self.current_target = g_node["node"]["name"] - origin_name,_ = get_node_names_from_edge_id_2(self.lnodes, the_edge["edge_id"]) - origin_node = self.rsearch.get_node_from_tmap2(origin_name) - - if(self.edge_reconf_start(the_edge)): - result, inc = self.execute_action(the_edge, g_node, origin_node) - self.edge_reconf_end() - else: - result = False - if not result: - self.get_logger().warning("Navigation Failed") - inc=1 - else: - self.get_logger().info("Navigation Finished Successfully") - self.get_logger().info("Navigating Case 2 -> res: {} ".format(inc)) - return result, inc - - - def enforce_navigable_route(self, route, target_node): + ori = attrs.get( + 'orientation', + {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}, + ) + ps.pose.orientation.x = float(ori.get('x', 0.0)) + ps.pose.orientation.y = float(ori.get('y', 0.0)) + ps.pose.orientation.z = float(ori.get('z', 0.0)) + ps.pose.orientation.w = float(ori.get('w', 1.0)) + return ps + + def _pose_or_fallback(self, node_name, ignore_orientation=False): + """Build pose from graph, falling back to raw YAML.""" + ps = self._build_pose_from_graph(node_name, ignore_orientation) + if ps is None: + nd = get_node_from_tmap2(self._tmap, node_name) + if nd: + ps = self._build_pose_stamped(nd, ignore_orientation) + return ps + + def _build_edge_oriented_pose(self, node_name, next_node_name): + """Build ``PoseStamped`` at *node_name* oriented toward *next_node*. + + Used for intermediate waypoints in composable segments so the + robot faces the direction of the next edge instead of using + the stored node orientation. + + Args: + node_name: Node whose position is used. + next_node_name: Node toward which the orientation points. + + Returns: + ``PoseStamped`` with yaw facing *next_node_name*, + or ``None`` if either node is missing from the graph. """ - Enforces the route to always contain the initial edge that leads the robot to the first node in the given route. - In other words, avoid that the route contains an initial edge that is too far from the robot pose. + if ( + node_name not in self._graph + or next_node_name not in self._graph + ): + return None + + attrs = self._graph.nodes[node_name] + next_attrs = self._graph.nodes[next_node_name] + + dx = float(next_attrs.get('x', 0.0)) - float(attrs.get('x', 0.0)) + dy = float(next_attrs.get('y', 0.0)) - float(attrs.get('y', 0.0)) + yaw = math.atan2(dy, dx) + + ps = PoseStamped() + ps.header.stamp = self.get_clock().now().to_msg() + ps.header.frame_id = self._node_nav_frame(node_name) + ps.pose.position.x = float(attrs.get('x', 0.0)) + ps.pose.position.y = float(attrs.get('y', 0.0)) + ps.pose.position.z = float(attrs.get('z', 0.0)) + ps.pose.orientation.z = math.sin(yaw / 2.0) + ps.pose.orientation.w = math.cos(yaw / 2.0) + return ps + + # ================================================================= + # Nav2 goal tolerances + # ================================================================= + + def _set_goal_tolerances(self, node_name): + """Set Nav2 goal checker tolerances from node properties. + + Reads ``xy_goal_tolerance`` and ``yaw_goal_tolerance`` from + the target node's properties and sets them on the Nav2 + controller_server via the ``SetParameters`` service. + + This is best-effort: if the service is unavailable the + goal proceeds with the previously configured tolerances. """ - if self.nav_from_closest_edge and self.closest_edges.edge_ids and len(self.closest_edges.edge_ids) == 2: - if not(self.closest_edges.edge_ids[0] in route.edge_id or self.closest_edges.edge_ids[1] in route.edge_id): - first_node = route.source[0] if len(route.source) > 0 else target_node - - for edge_id in self.closest_edges.edge_ids: - origin, destination = get_node_names_from_edge_id_2(self.lnodes, edge_id) - - if destination == first_node and edge_id not in route.edge_id: - route.source.insert(0, origin) - route.edge_id.insert(0, edge_id) - break - return route - - def edge_reconf_start(self, edge): - if self.edge_reconfigure: - if not self.srv_edge_reconfigure: - self.edgeReconfigureManager.register_edge(edge) - if (not self.edgeReconfigureManager.initialise()): - return False - self.edgeReconfigureManager.reconfigure() - else: - self.edgeReconfigureManager.srv_reconfigure(edge["edge_id"]) - return True - - - def edge_reconf_end(self): - if self.edge_reconfigure and not self.srv_edge_reconfigure and self.edgeReconfigureManager.active: - self.edgeReconfigureManager._reset() - - - def cancel_current_action(self, timeout_secs=-1): + if node_name not in self._graph: + return + + props = self._graph.nodes[node_name].get('properties', {}) + xy_tol = props.get('xy_goal_tolerance') + yaw_tol = props.get('yaw_goal_tolerance') + + if xy_tol is None and yaw_tol is None: + return + + # Skip if unchanged from last set + if xy_tol == self._last_xy_tol and yaw_tol == self._last_yaw_tol: + return + + params = [] + if xy_tol is not None: + p = RclParameter() + p.name = self._xy_tolerance_param + p.value = ParameterValue( + type=ParameterType.PARAMETER_DOUBLE, + double_value=float(xy_tol), + ) + params.append(p) + + if yaw_tol is not None: + p = RclParameter() + p.name = self._yaw_tolerance_param + p.value = ParameterValue( + type=ParameterType.PARAMETER_DOUBLE, + double_value=float(yaw_tol), + ) + params.append(p) + + if not self._set_params_client.service_is_ready(): + self.get_logger().debug( + "[TOL] SetParameters service not available, skipping", + ) + return + + req = SetParameters.Request() + req.parameters = params + future = self._set_params_client.call_async(req) + future.add_done_callback(self._tolerance_set_cb) + + self._last_xy_tol = xy_tol + self._last_yaw_tol = yaw_tol + + self.get_logger().info( + "[TOL] Setting tolerances for '%s': xy=%.3f yaw=%.3f" + % ( + node_name, + float(xy_tol) if xy_tol is not None else -1, + float(yaw_tol) if yaw_tol is not None else -1, + ), + ) + + def _tolerance_set_cb(self, future): + """Log result of tolerance parameter update.""" + try: + result = future.result() + failed = [ + r for r in result.results if not r.successful + ] + if failed: + self.get_logger().warning( + "[TOL] Some parameters failed to set", + ) + except Exception as exc: + self.get_logger().debug( + "[TOL] SetParameters call failed: %s" % exc, + ) + + # ================================================================= + # Goal construction (map-driven) + # ================================================================= + + def _resolve_tmap_ref(self, value): + """Resolve ``${transformation.}`` against the map.""" + if not isinstance(value, str): + return value + if not ( + value.startswith('${transformation.') + and value.endswith('}') + ): + return value + key = value[len('${transformation.'):-1] + tx = self._tmap.get('transformation', {}) + return tx.get(key, value) + + def _resolve_node_ref(self, value, node_name): + """Resolve ``${node.}`` and ``${transformation.}``. + + Looks up *attr* on the NetworkX graph node for *node_name*, + or resolves transformation-level references. + Returns the resolved value, or the original string if the + pattern does not match or the attribute is missing. """ - Cancels the action currently in execution. Returns True if the current goal is correctly ended. + if not isinstance(value, str): + return value + if value.startswith('${transformation.'): + return self._resolve_tmap_ref(value) + if not (value.startswith('${node.') and value.endswith('}')): + return value + attr = value[len('${node.'):-1] + if node_name and node_name in self._graph: + resolved = self._graph.nodes[node_name].get(attr, '') + if resolved: + return resolved + return value + + def _resolve_template_frame(self, template, node_name): + """Return the resolved ``header.frame_id`` from *template*. + + Supports the nested format where ``header`` lives inside the + goal field (``pose.header`` or ``poses[0].header``) as well as + the legacy flat format (``header`` at template root). + + Returns ``None`` when the template does not declare a header. """ - self.get_logger().info("Cancelling current navigation goal, timeout_secs = {}...".format(timeout_secs)) - self.edge_action_manager.preempt(timeout_secs=timeout_secs) - self.cancelled = True - self.navigation_activated = False - self.get_logger().info("Navigation active: " + str(self.navigation_activated)) - return not self.navigation_activated - - - def publish_route(self, route, target): - stroute = TopologicalRoute() - for i in route.source: - stroute.nodes.append(i) - stroute.nodes.append(target) - self.stroute = stroute - self.route_pub.publish(stroute) - - def router_pub_timer_callback(self): - - if (self.stroute is not None and self.stroute.nodes): - self.route_pub.publish(self.stroute) - - def publish_stats(self): - pubst = NavStatistics() - pubst.edge_id = self.stat.edge_id - pubst.status = self.stat.status - pubst.origin = self.stat.origin - pubst.target = self.stat.target - pubst.topological_map = self.stat.topological_map - # pubst.final_node = self.stat.final_node # FIXME: not - self.get_logger().info(" {}".format(self.stat.time_to_wp)) - pubst.time_to_waypoint = float(self.stat.time_to_wp) - pubst.operation_time = self.stat.operation_time - pubst.date_started = self.stat.get_start_time_str() - pubst.date_at_node = self.stat.date_at_node.strftime("%A, %B %d %Y, at %H:%M:%S hours") - - pubst.date_finished = self.stat.get_finish_time_str() - self.stats_pub.publish(pubst) - # self.stat = None - - - def get_fail_policy_state(self, edge): - policy = None - state = -1 - if len(self.executing_fail_policy) == 0: - _policy = [action.strip().split("_") for action in edge["fail_policy"].split(",")] - policy = [] - # repeat the actions that can be repeated more than once - for action in _policy: - if len(action) > 1 and isinstance(eval(action[-1]), int): - for _ in range(eval(action[-1])): - policy.append(action[:-1]) + header = None + + # New nested format: pose.header or poses[0].header + pose_tpl = template.get('pose') + if isinstance(pose_tpl, dict): + header = pose_tpl.get('header', {}) + if not header: + poses_tpl = template.get('poses') + if isinstance(poses_tpl, list) and poses_tpl: + first = poses_tpl[0] + if isinstance(first, dict): + header = first.get('header', {}) + + # Legacy flat format: header at template root + if not header: + header = template.get('header', {}) + + if not header: + return None + raw = header.get('frame_id', '') + if not raw: + return None + return self._resolve_node_ref(raw, node_name) + + def _build_segment_goal(self, segment, is_final_segment): + """Build a Nav2 goal dynamically from map ``actions`` config. + + The action class is resolved at map-load time via + ``_load_action_type`` and stored in ``_action_clients``. + Single-pose vs multi-pose dispatch is detected by checking + whether the goal object has a ``poses`` (list) or ``pose`` + (single) attribute -- no hardcoded action imports needed. + + The ``action_goal_template`` mirrors the ROS 2 goal structure:: + + action_goal_template: + pose: # PoseStamped wrapper + header: + frame_id: '${node.nav_frame}' + pose: '${node.pose}' + behavior_tree: '${definitions.default_bt}' + + If the template contains a ``header.frame_id`` (nested under + ``pose`` or ``poses[0]``), the resolved value overrides the + frame on every built ``PoseStamped``. + """ + action = segment.action_type + info = self._action_clients.get(action) + + if not info: + self.get_logger().error( + "[GOAL] No action config for '%s'" % action, + ) + return None + + action_class = info['action_class'] + tpl = info['config'].get('action_goal_template', {}) + bt = tpl.get('behavior_tree', '') + if isinstance(bt, str) and bt.startswith('${'): + bt = '' + + goal = action_class.Goal() + + # Multi-waypoint goal (e.g. NavigateThroughPoses) + if hasattr(goal, 'poses'): + n = segment.num_edges + for i, ed in enumerate(segment.edge_data): + is_last = (i == n - 1) + tgt = ed['target'] + + if not is_last: + # Intermediate waypoint: orient toward the next + # edge's target so the robot faces its travel + # direction instead of using the node's stored + # orientation. + next_tgt = segment.edge_data[i + 1]['target'] + ps = self._build_edge_oriented_pose(tgt, next_tgt) + if ps is None: + ps = self._pose_or_fallback( + tgt, ignore_orientation=True, + ) + elif self._no_orientation and is_final_segment: + # Final waypoint with no-orientation flag. + ps = self._pose_or_fallback( + tgt, ignore_orientation=True, + ) else: - policy.append(action) - state = 0 - self.executing_fail_policy = { - "policy": policy, # the policy we want to execute - "state": state, # at which point of the policy we are - "edge": edge["edge_id"] - } - + # Final waypoint: keep the node's orientation. + ps = self._pose_or_fallback(tgt) + + if ps is not None: + frame = self._resolve_template_frame( + tpl, tgt, + ) + if frame: + ps.header.frame_id = frame + goal.poses.append(ps) + self.get_logger().info( + "[GOAL] %s %d poses, BT=%s" + % (action_class.__name__, len(goal.poses), + bt or 'default'), + ) + # Single-pose goal (e.g. NavigateToPose) + elif hasattr(goal, 'pose'): + tgt = segment.last_target + ignore = self._no_orientation and is_final_segment + ps = self._pose_or_fallback( + tgt, ignore_orientation=ignore, + ) + if ps is not None: + frame = self._resolve_template_frame(tpl, tgt) + if frame: + ps.header.frame_id = frame + goal.pose = ps + self.get_logger().info( + "[GOAL] %s -> '%s', BT=%s" + % (action_class.__name__, tgt, bt or 'default'), + ) else: - policy = self.executing_fail_policy["policy"] - # increment the state because if we are here it means that the previous policy action failed - self.executing_fail_policy["state"] += 1 - state = self.executing_fail_policy["state"] + self.get_logger().warning( + "[GOAL] %s goal has no 'pose'/'poses' attr" + % action_class.__name__, + ) - return policy, state + if bt and hasattr(goal, 'behavior_tree'): + goal.behavior_tree = bt + return goal - def execute_action_fail_recovery(self, edge, destination_node, route, idx, origin_node, target): - """ - This function wraps `execute_action` by executing the fail_policy in case of ABORTED action - The fail policy sometimes modifies the current route by including the recovery action - """ - nav_ok, inc = self.execute_action(edge, destination_node, origin_node) - new_route = route - recovering = False - replanned = False - # this means the action is aborted -> execute the fail policy - # make sure that if the goal is cancelled by the client we don't enter here - if not nav_ok and not self.preempted: - self.get_logger().info("\t>> route: {}".format(route)) - route_updated = False - while not route_updated: - policy, state = self.get_fail_policy_state(edge) - if state < len(policy): - rec_action = policy[state] - self.get_logger().info(">> EXECUTING FAIL POLICY ACTION: {}.".format(rec_action)) - if rec_action[0] == "retry": - new_route.source.insert(idx+1, route.source[idx]) - new_route.edge_id.insert(idx+1, route.edge_id[idx]) - route_updated = True - recovering = True - replanned = False - elif rec_action[0] == "fail": - route_updated = True - recovering = False - replanned = False - elif rec_action[0] == "wait": - recovering = True - replanned = False - elif rec_action[0] == "replan": - _route = self.rsearch.search_route(origin_node["node"]["name"], target, avoid_edges=[edge["edge_id"]]) - _route = self.enforce_navigable_route(_route, target) - new_route.source = route.source[:idx] + _route.source - new_route.edge_id = route.edge_id[:idx] + _route.edge_id - route_updated = True - recovering = True - replanned = True - else: - # clean the current fail policy data - self.executing_fail_policy = {} - recovering = False - replanned = False + # ================================================================= + # Nav2 dispatch + # ================================================================= - self.get_logger().info("\t>> new route: {}".format(new_route)) - return nav_ok, inc, recovering, new_route, replanned - - def execute_actions(self, edges, destination_nodes, origin_nodes=None, action_name=None, is_execpolicy=False): - inc = 0 - result = True - self.goal_reached = False - self.prev_status = None - - self.edge_action_manager.initialise(self.bt_trees, edges, destination_nodes, origin_nodes, - action_name=action_name, in_row_operation=self.use_in_row_operation, is_execpolicy=is_execpolicy) - self.edge_action_manager.execute() - status = self.edge_action_manager.get_state() - self.pub_status(status) - - if ((status == GoalStatus.STATUS_EXECUTING or status == GoalStatus.STATUS_UNKNOWN) and not self.cancelled and not self.goal_reached): - try: - self.get_logger().info(" Current status {} ".format(self.edge_action_manager.get_status_msg(status))) - self.pub_status(status) - except Exception as e: - pass - - res = self.edge_action_manager.get_result() - if status != GoalStatus.STATUS_SUCCEEDED: - if not self.goal_reached: - result = False - if status is GoalStatus.STATUS_CANCELED: - self.preempted = True - else: - result = True - else: - self.preempted = False + def _send_nav2_goal(self, goal, action_client=None): + """Send goal to Nav2. Blocks until result. - if not res: - if not result: - inc = 1 - else: - inc = 0 + The main ``MultiThreadedExecutor`` processes action-client + callbacks, so this method polls futures with ``time.sleep`` + instead of calling ``rclpy.spin_once`` (which would conflict + with the executor that already owns the node). - status = self.edge_action_manager.get_state() - self.pub_status(status) - - self.get_logger().info("Navigation action status: {}, goal reached: {}, inc: {}".format(self.edge_action_manager.get_status_msg(status), result, inc)) - return result, inc, status - - def execute_action(self, edge, destination_node, origin_node=None): - inc = 0 - result = True - self.goal_reached = False - self.prev_status = None - - if self.using_restrictions and edge["edge_id"] != "navigation_action_edge": - self.get_logger().info("Evaluating restrictions on edge {}".format(edge["edge_id"])) - ev_edge_msg = EvaluateEdge() - ev_edge_msg.edge = edge["edge_id"] - ev_edge_msg.runtime = True - future = self.evaluate_edge_srv.call_async(ev_edge_msg) - while rclpy.ok(): - rclpy.spin_once(self,) - if future.done(): - resp = future.result() - break - if resp.success and resp.evaluation: - self.get_logger().warning("The edge is restricted, stopping navigation") - result = False - inc = 1 - return result, inc - - self.get_logger().info("Evaluating restrictions on node {}".format(destination_node["node"]["name"])) - ev_node_msg = EvaluateNode() - ev_node_msg.node = destination_node["node"]["name"] - ev_node_msg.runtime = True - future = self.evaluate_node_srv.call_async(ev_node_msg) - while rclpy.ok(): - rclpy.spin_once(self,) - if future.done(): - resp = future.result() - break - if resp.success and resp.evaluation: - self.get_logger().warning("The node is restricted, stopping navigation") - result = False - inc = 1 - return result, inc - - self.edge_action_manager.initialise(self.bt_trees, edge, destination_node, origin_node) - self.edge_action_manager.execute() - status = self.edge_action_manager.get_state() - self.pub_status(status) - - if ((status == GoalStatus.STATUS_EXECUTING or status == GoalStatus.STATUS_UNKNOWN) and not self.cancelled and not self.goal_reached): - try: - self.get_logger().info(" Current status {} ".format(self.edge_action_manager.get_status_msg(status))) - self.pub_status(status) - except Exception as e: - pass - - res = self.edge_action_manager.get_result() - if status != GoalStatus.STATUS_SUCCEEDED: - if not self.goal_reached: - result = False - if status is GoalStatus.STATUS_CANCELED: - self.preempted = True - else: - result = True - - if not res: - if not result: - inc = 1 - else: - inc = 0 + Returns ``GoalStatus`` integer. + """ + if action_client is None: + for info in self._action_clients.values(): + action_client = info['client'] + break + if action_client is None: + self.get_logger().error("[NAV2] No action client available") + return GoalStatus.STATUS_ABORTED + + if not action_client.server_is_ready(): + self.get_logger().info("[NAV2] Waiting for server ...") + if not action_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error("[NAV2] Server unavailable") + return GoalStatus.STATUS_ABORTED + + future = action_client.send_goal_async( + goal, feedback_callback=self._nav2_fb_cb, + ) + + # Wait for goal acceptance + while rclpy.ok() and not future.done(): + if self._preempted or self._cancelled: + return GoalStatus.STATUS_CANCELED + time.sleep(0.1) + + if not future.done(): + self.get_logger().error("[NAV2] send_goal did not complete") + return GoalStatus.STATUS_ABORTED + + self._goal_handle = future.result() + if not self._goal_handle.accepted: + self.get_logger().error("[NAV2] Goal REJECTED") + return GoalStatus.STATUS_ABORTED + self.get_logger().info("[NAV2] Goal ACCEPTED") + + # Wait for result + result_future = self._goal_handle.get_result_async() + while rclpy.ok(): + if self._preempted or self._cancelled: + self._cancel_nav2_goal() + return GoalStatus.STATUS_CANCELED + if result_future.done(): + res = result_future.result() + self._action_status = res.status + self.get_logger().info( + "[NAV2] Finished: %s" % _status_str(res.status), + ) + return res.status + time.sleep(0.1) + return GoalStatus.STATUS_ABORTED + + def _nav2_fb_cb(self, _feedback_msg): + self._action_status = GoalStatus.STATUS_EXECUTING + + def _cancel_nav2_goal(self, timeout_sec=None): + """Cancel the active Nav2 goal (fire-and-forget).""" + if self._goal_handle is None: + return + try: + self._goal_handle.cancel_goal_async() + self.get_logger().info("[NAV2] Cancel requested") + except Exception as exc: + self.get_logger().error("[NAV2] Cancel error: %s" % exc) + finally: + self._goal_handle = None + + # ================================================================= + # Action-server callbacks + # ================================================================= + + def _exec_goto_cb(self, goal_handle): + """``GotoNode`` action callback.""" + target = goal_handle.request.target + self.get_logger().info( + "=" * 60 + "\n[GOTO] target='%s', no_ori=%s" + % (target, goal_handle.request.no_orientation), + ) + # Preempt any active navigation and wait for it to stop + if self._navigation_activated: + self.get_logger().info("[GOTO] Preempting active navigation") + self._preempted = True + self._cancel_nav2_goal() + deadline = time.time() + 5.0 + while self._navigation_activated and time.time() < deadline: + time.sleep(0.05) + if self._navigation_activated: + self.get_logger().warning( + "[GOTO] Timeout waiting for old navigation", + ) + self._navigation_activated = False + if self._sm.is_terminal(): + self._sm.reset() + elif self._sm.state != NavState.READY: + self._sm.transition(NavState.CANCELLED) + self._sm.reset() + + self._navigation_activated = True + self._cancelled = False + self._preempted = False + self._no_orientation = goal_handle.request.no_orientation + + fb = GotoNodeFeedback() + fb.route = "Planning..." + self._goto_fb_pub.publish(fb) + + success = self._navigate(target) + self._navigation_activated = False - status = self.edge_action_manager.get_state() - self.pub_status(status) + result = GotoNode.Result() + result.success = success + if success: + goal_handle.succeed() + self.get_logger().info("[GOTO] SUCCEEDED -> '%s'" % target) + else: + goal_handle.abort() + self.get_logger().warning( + "[GOTO] %s -> '%s'" + % ("CANCELLED" if self._preempted else "FAILED", target), + ) + if self._sm.is_terminal(): + self._sm.reset() + return result - self.get_logger().info("Navigation action status: {}, goal reached: {}, inc: {}".format(self.edge_action_manager.get_status_msg(status), result, inc)) - return result, inc - - - def pub_status(self, status): - if status != self.prev_status: - d = {} - d["goal"] = self.edge_action_manager.destination_node["node"]["name"] - d["final_goal"] = self.final_goal - d["action"] = self.edge_action_manager.current_action.upper() - d["status"] = self.edge_action_manager.get_status_msg(status) - msg = String() - msg.data = json.dumps(d) - self.move_act_pub.publish(msg) - self.prev_status = status -################################################################################################################### + def _exec_policy_cb(self, goal_handle): + """``ExecutePolicyMode`` action callback.""" + self.get_logger().info("=" * 60 + "\n[POLICY] Goal received") + + # Preempt any active navigation and wait for it to stop + if self._navigation_activated: + self.get_logger().info("[POLICY] Preempting active navigation") + self._preempted = True + self._cancel_nav2_goal() + deadline = time.time() + 5.0 + while self._navigation_activated and time.time() < deadline: + time.sleep(0.05) + if self._navigation_activated: + self.get_logger().warning( + "[POLICY] Timeout waiting for old navigation", + ) + self._navigation_activated = False + if self._sm.is_terminal(): + self._sm.reset() + elif self._sm.state != NavState.READY: + self._sm.transition(NavState.CANCELLED) + self._sm.reset() + + self._navigation_activated = True + self._cancelled = False + self._preempted = False + + route = goal_handle.request.route + if ( + len(route.source) < 1 + or len(route.source) != len(route.edge_id) + ): + self.get_logger().error("[POLICY] Invalid route data") + self._navigation_activated = False + goal_handle.succeed() + return ExecutePolicyMode.Result(success=False) + + if ( + route.source[0] != self._current_node + and route.source[0] != self._closest_node + ): + self.get_logger().error( + "[POLICY] Route starts at '%s' but robot at '%s'" + % (route.source[0], self._current_node), + ) + self._navigation_activated = False + goal_handle.succeed() + return ExecutePolicyMode.Result(success=False) + + route_nodes = list(route.source) + if route.edge_id: + last_e = get_edge_from_id_tmap2( + self._tmap, route.source[-1], route.edge_id[-1], + ) + if last_e: + ft = last_e["node"] + if not route_nodes or route_nodes[-1] != ft: + route_nodes.append(ft) + + target = route_nodes[-1] + self.get_logger().info( + "[POLICY] Route: %s" % " -> ".join(route_nodes), + ) + self._publish_route(route_nodes) + success = self._execute_route(route_nodes, target) + + self._navigation_activated = False + if self._sm.is_terminal(): + self._sm.reset() + goal_handle.succeed() + self.get_logger().info( + "[POLICY] %s" % ("SUCCEEDED" if success else "FAILED"), + ) + return ExecutePolicyMode.Result(success=success) + + def _cancel_goto_cb(self, _gh): + self.get_logger().warning("[GOTO] Cancel requested") + self._preempted = True + self._cancel_nav2_goal() + + def _cancel_policy_cb(self, _gh): + self.get_logger().warning("[POLICY] Cancel requested") + self._preempted = True + self._cancel_nav2_goal() + + # ================================================================= + # Core navigation + # ================================================================= + + def _navigate(self, target): + """Plan route and execute. Returns ``True`` on success.""" + if self._cancelled: + return False + self._sm.transition(NavState.PLANNING) + self._publish_status("PLANNING") + self._target = target + + if target not in self._graph: + self.get_logger().error("[NAV] '%s' not in map" % target) + self._sm.transition(NavState.FAILED) + self._publish_status("FAILED") + return False + + origin = self._determine_origin(target) + if origin is None: + self.get_logger().error("[NAV] Cannot determine origin") + self._sm.transition(NavState.FAILED) + self._publish_status("FAILED") + return False + + self.get_logger().info( + "[NAV] Planning '%s' -> '%s'" % (origin, target), + ) + + if origin == target: + self.get_logger().info("[NAV] Already at target") + self._sm.transition(NavState.SUCCEEDED) + self._publish_status("SUCCEEDED") + return True + + route_nodes = plan_route( + self._graph, origin, target, + logger=self.get_logger(), + algorithm=self._route_algorithm, + weight=self._route_weight, + ) + if not route_nodes or len(route_nodes) < 2: + self.get_logger().error( + "[NAV] No route '%s' -> '%s'" % (origin, target), + ) + self._sm.transition(NavState.FAILED) + self._publish_status("FAILED") + return False + + self.get_logger().info( + "[NAV] Route: %s (%d nodes)" + % (" -> ".join(route_nodes), len(route_nodes)), + ) + self._publish_route(route_nodes) + success = self._execute_route(route_nodes, target) + + # If the map was updated mid-execution, replan from scratch. + if not success and self._map_updated_during_nav: + self._map_updated_during_nav = False + self.get_logger().info( + "[NAV] Replanning after mid-navigation map update", + ) + return self._navigate(target) + + if not success and not self._cancelled and not self._preempted: + self._sm.transition(NavState.FAILED) + self._publish_status("FAILED") + return success + + def _determine_origin(self, target): + """Best origin: current_node > closest-edge > closest_node.""" + if self._current_node not in ("none", "Unknown"): + return self._current_node + + if ( + self._closest_edges.distances + and self._closest_edges.distances[0] + <= self._max_dist_to_closest_edge + ): + origin = self._origin_from_closest_edge(target) + if origin: + return origin + + if self._closest_node != "Unknown": + return self._closest_node + return None + + def _origin_from_closest_edge(self, target): + eids = self._closest_edges.edge_ids + + def _src(eid): + if not eid: + return None + for u, _v, d in self._graph.edges(data=True): + if d.get('edge_id') == eid: + return u + return None + + src1 = _src(eids[0] if eids else None) + if src1 is None: + return self._closest_node + + if len(eids) > 1 and len(self._closest_edges.distances) > 1: + src2 = _src(eids[1]) + if ( + src2 + and self._closest_edges.distances[0] + == self._closest_edges.distances[1] + ): + r1 = plan_route( + self._graph, src1, target, + algorithm=self._route_algorithm, + weight=self._route_weight, + ) + r2 = plan_route( + self._graph, src2, target, + algorithm=self._route_algorithm, + weight=self._route_weight, + ) + d1 = get_route_distance(self._graph, r1) if r1 else float('inf') + d2 = get_route_distance(self._graph, r2) if r2 else float('inf') + return src1 if d1 <= d2 else src2 + return src1 + + # ================================================================= + # Route execution + # ================================================================= + + def _execute_route(self, route_nodes, target): + """Execute a planned route segment by segment. + + Between segments the method checks for buffered map updates. + If the map changed, the remaining route is validated; if any + node or edge is now missing the method signals a replan by + setting ``_map_updated_during_nav`` and returning ``False``. + """ + self._navigation_activated = True + self._map_updated_during_nav = False + + route_edges = get_route_edges(self._graph, route_nodes) + if not route_edges: + self.get_logger().error("[EXEC] No edges in route") + return False + + segments = merge_action_segments( + route_edges, map_actions=self._map_actions or None, + ) + + self.get_logger().info( + "[EXEC] %d edges -> %d segment(s):" + % (len(route_edges), len(segments)), + ) + for i, seg in enumerate(segments): + self.get_logger().info( + " [%d] %s x%d: %s -> %s" % ( + i, seg.action_type, seg.num_edges, + seg.first_source, seg.last_target, + ), + ) + + # Track which node index the current segment starts at so we + # can validate the *remaining* route after a map update. + node_idx = 0 + + for si, seg in enumerate(segments): + if self._cancelled or self._preempted: + self._sm.transition(NavState.CANCELLED) + self._publish_status("CANCELLED") + return False + + # -- Safe point: apply any buffered map update ----------- + if self._apply_pending_map(): + self.get_logger().info( + "[EXEC] Map updated between segments", + ) + if not self._validate_remaining_route( + route_nodes, node_idx, + ): + self.get_logger().warning( + "[EXEC] Remaining route invalid after map " + "update -- requesting replan", + ) + self._map_updated_during_nav = True + return False + # Route still valid on new graph; re-derive segments + # from the remaining nodes so edge lookups use the + # updated ``self._tmap``. + remaining_nodes = route_nodes[node_idx:] + route_edges = get_route_edges( + self._graph, remaining_nodes, + ) + if not route_edges: + self.get_logger().warning( + "[EXEC] No edges after re-derive -- replan", + ) + self._map_updated_during_nav = True + return False + segments = merge_action_segments( + route_edges, + map_actions=self._map_actions or None, + ) + # Restart the segment loop with updated segments + return self._execute_remaining_segments( + segments, route_nodes[node_idx:], target, + ) + + is_final = si == len(segments) - 1 + ok = self._execute_segment(seg, is_final, si, len(segments)) + if not ok: + if self._cancelled or self._preempted: + self._sm.transition(NavState.CANCELLED) + self._publish_status("CANCELLED") + return False + + # Advance node_idx past the nodes consumed by this segment + node_idx += seg.num_edges + + self._sm.transition(NavState.SUCCEEDED) + self._publish_status("SUCCEEDED") + return True + + def _execute_remaining_segments( + self, segments, route_nodes, target, + ): + """Continue segment execution after a mid-route map update. + + This is factored out of ``_execute_route`` to avoid resetting + the segment loop counter. The logic is identical. + """ + node_idx = 0 + + for si, seg in enumerate(segments): + if self._cancelled or self._preempted: + self._sm.transition(NavState.CANCELLED) + self._publish_status("CANCELLED") + return False + + # Check for *another* map update + if self._apply_pending_map(): + self.get_logger().info( + "[EXEC] Map updated again between segments", + ) + if not self._validate_remaining_route( + route_nodes, node_idx, + ): + self._map_updated_during_nav = True + return False + remaining_nodes = route_nodes[node_idx:] + route_edges = get_route_edges( + self._graph, remaining_nodes, + ) + if not route_edges: + self._map_updated_during_nav = True + return False + segments = merge_action_segments( + route_edges, + map_actions=self._map_actions or None, + ) + return self._execute_remaining_segments( + segments, route_nodes[node_idx:], target, + ) + + is_final = si == len(segments) - 1 + ok = self._execute_segment(seg, is_final, si, len(segments)) + if not ok: + if self._cancelled or self._preempted: + self._sm.transition(NavState.CANCELLED) + self._publish_status("CANCELLED") + return False + + node_idx += seg.num_edges + + self._sm.transition(NavState.SUCCEEDED) + self._publish_status("SUCCEEDED") + return True + + def _execute_segment(self, segment, is_final, seg_idx, total): + """Execute one action segment.""" + action = segment.action_type + exec_state = ACTION_TO_STATE.get( + action, NavState.EXECUTING_NAVIGATE_TO_POSE, + ) + self._sm.transition(exec_state) + self._publish_status(exec_state.value) + + self.get_logger().info( + "[SEG %d/%d] %s x%d: %s -> %s" % ( + seg_idx + 1, total, action, segment.num_edges, + segment.first_source, segment.last_target, + ), + ) + + # Publish boundary polygon if edges carry boundary props + self._publish_segment_boundary(segment) + + # Pre-flight: validate all edges + edge_dicts = [] + for edata in segment.edge_data: + if self._cancelled or self._preempted: + return False + + src = edata['source'] + tgt = edata['target'] + eid = edata.get('edge_id', '') + + ed = get_edge_from_id_tmap2(self._tmap, src, eid) + if not ed: + self.get_logger().error( + " Edge '%s' (%s->%s): lookup failed" + % (eid, src, tgt), + ) + self._stat = nav_stats(src, tgt, self._topol_map, eid) + self._stat.set_ended(self._current_node) + self._stat.status = "failed" + self._publish_stats() + return False + + edge_dicts.append(ed) + + # Build and send goal + self._current_target = segment.last_target + self._publish_current_edge( + segment.edge_ids[0] if segment.edge_ids else "none", + ) + + # Set Nav2 goal checker tolerances from target node properties + self._set_goal_tolerances(segment.last_target) + + self.get_logger().info( + " Sending %d-wp %s goal" % (segment.num_edges, action), + ) + + goal = self._build_segment_goal(segment, is_final) + self._stat = nav_stats( + segment.first_source or "?", + segment.last_target or "?", + self._topol_map, + segment.edge_ids[0] if segment.edge_ids else "", + ) + + info = self._action_clients.get(action) + client = info['client'] if info else None + status = self._send_nav2_goal(goal, action_client=client) + + self._publish_move_status( + segment.last_target or "?", action, _status_str(status), + ) + + # Evaluate + self._stat.set_ended(self._current_node) + if status == GoalStatus.STATUS_SUCCEEDED or self._goal_reached: + self._stat.status = "success" + self._publish_stats() + self.get_logger().info( + " Segment OK: %s -> %s (%.1fs)" % ( + segment.first_source, segment.last_target, + self._stat.operation_time, + ), + ) + self._goal_reached = False + else: + self._stat.status = "failed" + self._publish_stats() + if status == GoalStatus.STATUS_CANCELED: + self._preempted = True + self.get_logger().warning( + " Segment FAILED: %s -> %s (%s)" % ( + segment.first_source, segment.last_target, + _status_str(status), + ), + ) + return False + + self._publish_current_edge("none") + return True + + # ================================================================= + # Boundary publishing + # ================================================================= + + def _publish_segment_boundary(self, segment): + """Publish a boundary polygon if edges have boundary properties. + + Checks whether any edge in the segment carries + ``boundary_left`` or ``boundary_right`` properties. If so, + a corridor polygon is computed and published. Otherwise an + empty polygon is published to clear any previous boundary. + """ + has_boundary = any( + 'boundary_left' in ed.get('properties', {}) + or 'boundary_right' in ed.get('properties', {}) + for ed in segment.edge_data + ) + if not has_boundary: + self._publish_empty_boundary() + return + + frame_id = self._node_nav_frame(segment.first_source) + poly = compute_boundary_polygon( + self._graph, segment, + default_left=self._default_boundary_left, + default_right=self._default_boundary_right, + ) + if poly: + self._publish_boundary(poly, frame_id) + else: + self.get_logger().warning( + "[BOUNDARY] Cannot compute boundary polygon", + ) + self._publish_empty_boundary(frame_id) + + # ================================================================= + # Helpers + # ================================================================= + + def _cancel_current_nav(self): + if self._navigation_activated: + self.get_logger().info("[CANCEL] Stopping current nav") + self._cancel_nav2_goal(timeout_sec=2.0) + self._cancelled = True + self._navigation_activated = False + # Reset state machine so next goal can transition to PLANNING + if not self._sm.is_terminal() and self._sm.state != NavState.READY: + self._sm.transition(NavState.CANCELLED) + if self._sm.is_terminal(): + self._sm.reset() + + +# ===================================================================== +# Entry point +# ===================================================================== def main(): rclpy.init(args=None) - update_params_control_server = ParameterUpdaterNode("controller_server") - edge_action_manager_server = EdgeActionManager("edge_action_manager") - node = TopologicalNavServer('topological_navigation', update_params_control_server, edge_action_manager_server) - + node = TopologicalNavServer('topological_navigation') executor = MultiThreadedExecutor() - - executor.add_node(update_params_control_server) - executor.add_node(edge_action_manager_server) executor.add_node(node) try: executor.spin() except KeyboardInterrupt: - node.get_logger().info('shutting down localisation node\n') + node.get_logger().info("[SHUTDOWN] Keyboard interrupt") node.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/topological_navigation/topological_navigation/scripts/navstats_logger.py b/topological_navigation/topological_navigation/scripts/navstats_logger.py deleted file mode 100755 index 3ad75ead..00000000 --- a/topological_navigation/topological_navigation/scripts/navstats_logger.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python2 - - -import calendar -from datetime import datetime - -import rospy - -from topological_navigation_msgs.msg import NavStatistics -from mongodb_store.message_store import MessageStoreProxy - -class TopologicalNavStatsSaver(object): - """ - Class for Topological Navigation stats logging - - """ - def __init__(self) : - rospy.Subscriber('topological_navigation/Statistics', NavStatistics, self.statsCallback) - - rospy.spin() - - def statsCallback(self, msg): - #print "---------------------------------------------------" - meta = {} - meta["type"] = "Topological Navigation Stat" - epoch = datetime.strptime(msg.date_at_node, '%A, %B %d %Y, at %H:%M:%S hours').timetuple()# .time() - #print calendar.timegm(epoch) - meta["epoch"] = calendar.timegm(epoch)#calendar.timegm(self.stat.date_at_node.timetuple()) - meta["date"] = msg.date_at_node - meta["pointset"] = msg.topological_map - - #print meta - - msg_store = MessageStoreProxy(collection='nav_stats') - msg_store.insert(msg,meta) - - -if __name__ == '__main__': - mode="normal" - rospy.init_node('topological_navstats_logger') - server = TopologicalNavStatsSaver() \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/occupancy_checker.py b/topological_navigation/topological_navigation/scripts/occupancy_checker.py deleted file mode 100755 index 57ec01eb..00000000 --- a/topological_navigation/topological_navigation/scripts/occupancy_checker.py +++ /dev/null @@ -1,315 +0,0 @@ -import rclpy -from rclpy.node import Node -from rclpy.executors import MultiThreadedExecutor -from geometry_msgs.msg import PoseArray, PoseStamped -import math -import tf2_ros -import tf2_geometry_msgs -import yaml -import numpy as np -import tf_transformations -from std_msgs.msg import String -from visualization_msgs.msg import Marker, MarkerArray -from rclpy.qos import QoSProfile, DurabilityPolicy -from topological_navigation_msgs.msg import TopologicalOccupiedNode - -# =============================================================== -# UTILS -# =============================================================== -def load_waypoints_from_tmap(file_path): - """ - Loads and parses a tmap file, and pre-transforms the local vertices of each - waypoint into the global 'map' frame. - """ - try: - with open(file_path, 'r') as file: - tmap_data = yaml.safe_load(file) - - waypoints_data = {} - for node_entry in tmap_data.get('nodes', []): - try: - node_name = node_entry['node']['name'] - pos = node_entry['node']['pose']['position'] - orient = node_entry['node']['pose']['orientation'] - - # Create the transformation matrix for this waypoint - translation = [pos['x'], pos['y'], pos['z']] - rotation = [orient['x'], orient['y'], orient['z'], orient['w']] - - trans_matrix = tf_transformations.translation_matrix(translation) - rot_matrix = tf_transformations.quaternion_matrix(rotation) - transform_matrix = np.dot(trans_matrix, rot_matrix) - - # Transform local vertices into map frame vertices - local_verts = node_entry['node'].get('verts', []) - map_frame_verts = [] - for vert in local_verts: - # Represent local vertex as a 4D point for matrix multiplication - local_point = np.array([vert['x'], vert['y'], 0, 1]) - map_point = np.dot(transform_matrix, local_point) - # Store the transformed (x, y) coordinates - map_frame_verts.append((map_point[0], map_point[1])) - - # Store the pre-transformed data - waypoints_data[node_name] = { - 'pose': (pos['x'], pos['y']), - 'verts': map_frame_verts - } - - except KeyError as e: - continue - - return waypoints_data - - except Exception as e: - return {} - - -# =============================================================== -# NODE A: The Transformer Class -# =============================================================== -class PoseTransformerNode(Node): - def __init__(self): - super().__init__('pose_transformer') - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) - self.publisher = self.create_publisher(PoseArray, '/plan_in_map_frame', 10) - self.create_subscription(PoseArray, '/rownav_teb_poses', self.plan_callback, 10) - self.get_logger().info('Pose Transformer component has started.') - - def plan_callback(self, msg): - """ - Transforms a PoseStamped from the odom frame to the map frame using TF2. - """ - poses = [] - - # Check if the transform is available before proceeding - try: - self.tf_buffer.lookup_transform('map', msg.header.frame_id, rclpy.time.Time()) - except tf2_ros.TransformException as ex: - self.get_logger().error(f'Could not transform {msg.header.frame_id} to map: {ex}') - return - - for pose_odom in msg.poses: - - pose_stamped_in = PoseStamped() - pose_stamped_in.header = msg.header - pose_stamped_in.pose = pose_odom - - try: - timeout = rclpy.duration.Duration(seconds=0.1) - transformed_pose_stamped = self.tf_buffer.transform(pose_stamped_in, 'map', timeout) - poses.append(transformed_pose_stamped.pose) - - except tf2_ros.TransformException as ex: - self.get_logger().error(f'Could not transform {msg.header.frame_id} to map: {ex}') - continue - - # Publish the transformed poses - if poses: - msgnew = PoseArray() - msgnew.header.frame_id = 'map' - msgnew.header.stamp = msg.header.stamp - msgnew.poses = poses - self.publisher.publish(msgnew) - - -# =============================================================== -# NODE B: The Checker Class -# =============================================================== -class OccupancyCheckerNode(Node): - def __init__(self): - super().__init__('occupancy_checker') - - # 1. DECLARE PARAMETERS for file path and threshold - self.declare_parameter('tmap', rclpy.Parameter.Type.STRING) - self.declare_parameter('occupancy_threshold', rclpy.Parameter.Type.DOUBLE) - - # 2. GET PARAMETER VALUES - self.tmap_path = self.get_parameter('tmap').get_parameter_value().string_value - self.occupancy_threshold = self.get_parameter('occupancy_threshold').get_parameter_value().double_value - - # 3. subscribe to the '/plan_in_map_frame' topic. - self.create_subscription(PoseArray, '/plan_in_map_frame', self.teb_poses_cb, 10) - self.create_subscription(String, '/topological_navigation/current_destination', self.current_destination_cb, 10) - - # 4. Load waypoints from the specified topological map file - self.tmap = load_waypoints_from_tmap(self.tmap_path) - - # 5. Create Publisher and State variable to only publish when the set of waypoints changes - self.occupancy_pub = self.create_publisher(TopologicalOccupiedNode, '/topological_navigation/occupied_node', 10) - self.last_published_wps = set() - self.current_destination = None - - self.get_logger().info('Occupancy Checker has started.') - if self.tmap: - self.get_logger().info(f'Successfully loaded and now monitoring {len(self.tmap)} waypoints.') - else: - self.get_logger().warn('No waypoints loaded. Please check the topological map file path.') - return - self.get_logger().info(f'occupancy threshold is {self.occupancy_threshold} meters.') - - - def teb_poses_cb(self, msg): - """ - Callback to check if any received pose overlaps with waypoints. - Uses a polygon check if vertices are available, otherwise uses a distance threshold. - """ - occupied_wps = set() - if self.current_destination is not None: occupied_wps.add(self.current_destination) - for i, pose in enumerate(msg.poses): - for wp, wp_data in self.tmap.items(): - is_occupied = False - verts = wp_data.get('verts', []) - if verts: - if self._is_inside_polygon(pose.position.x, pose.position.y, verts): - is_occupied = True - occupied_wps.add(wp) - else: - self.get_logger().warn("Checking overlap using DISTANCE THRESHOLD.") - waypoint_pose = wp_data['pose'] - distance = math.hypot(pose.position.x - waypoint_pose[0], pose.position.y - waypoint_pose[1]) - - if distance < self.occupancy_threshold: - self.get_logger().warn(f"OCCUPANCY: Pose #{i} is NEAR Waypoint {wp} (using distance check).") - is_occupied = True - occupied_wps.add(wp) - - if is_occupied: - break - - if occupied_wps != self.last_published_wps: - # Create the message object - occupancy_msg = TopologicalOccupiedNode() - occupancy_msg.nodes = list(occupied_wps) - self.occupancy_pub.publish(occupancy_msg) - - # Log the change for debugging - if occupancy_msg.nodes: self.get_logger().debug(f"Occupied nodes: {occupancy_msg.nodes}") - - # Update the state for the next check - self.last_published_wps = occupied_wps - - def current_destination_cb(self, msg: String): - self.current_destination = msg.data - - def _is_inside_polygon(self, point_x, point_y, polygon_verts): - num_verts = len(polygon_verts) - is_inside = False - p1_x, p1_y = polygon_verts[0] - for i in range(1, num_verts + 1): - p2_x, p2_y = polygon_verts[i % num_verts] # Use modulo to wrap around to the first vertex - if point_y > min(p1_y, p2_y): - if point_y <= max(p1_y, p2_y): - if point_x <= max(p1_x, p2_x): - if p1_y != p2_y: - # Calculate the x-intersection of the line - x_intersection = (point_y - p1_y) * (p2_x - p1_x) / (p2_y - p1_y) + p1_x - if p1_x == p2_x or point_x <= x_intersection: - is_inside = not is_inside - - p1_x, p1_y = p2_x, p2_y - - return is_inside - - -# # ============================================================================== -# # NODE C: The Visualiser Class -# # ============================================================================== -# class WaypointVisualiserNode(Node): -# """ -# Subscribes to the list of occupied waypoints and publishes visualization markers. -# """ -# def __init__(self): -# super().__init__('occupied_waypoint_visualiser') -# self.declare_parameter('tmap', rclpy.Parameter.Type.STRING) -# self.tmap_path = self.get_parameter('tmap').get_parameter_value().string_value -# self.tmap = load_waypoints_from_tmap(self.tmap_path) - -# self.last_marker_count = 0 - -# latching_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) -# self.marker_pub = self.create_publisher(MarkerArray, '/topological_navigation/visual/occupied_node', latching_qos) - -# self.create_subscription(TopologicalOccupiedNode, '/topological_navigation/occupied_node', self.occupancy_cb, 10) -# self.get_logger().info('Waypoint Visualiser component has started.') - -# def occupancy_cb(self, msg): -# waypoint_names = msg.nodes if msg.nodes else [] -# marker_array = MarkerArray() - -# # Create ADD markers for all currently occupied waypoints -# for i, wp_name in enumerate(waypoint_names): -# wp_data = self.tmap.get(wp_name) -# if not wp_data: continue - -# marker = Marker() -# marker.header.frame_id = "map" -# marker.header.stamp = self.get_clock().now().to_msg() -# marker.ns = "occupied_waypoints" -# marker.id = i -# marker.type = Marker.SPHERE -# marker.action = Marker.ADD - -# pose_coords = wp_data['pose'] -# marker.pose.position.x = pose_coords[0] -# marker.pose.position.y = pose_coords[1] -# marker.pose.position.z = 0.15 # Lift it off the ground slightly -# marker.pose.orientation.w = 1.0 - -# marker.scale.x = 1.0 -# marker.scale.y = 1.0 -# marker.scale.z = 0.1 - -# marker.color.r = 1.0 # Red -# marker.color.g = 0.0 -# marker.color.b = 0.0 -# marker.color.a = 0.8 # Slightly transparent - -# marker_array.markers.append(marker) - -# # Create DELETE markers for any old markers that are no longer needed -# for i in range(len(waypoint_names), self.last_marker_count): -# marker = Marker() -# marker.header.frame_id = "map" -# marker.ns = "occupied_waypoints" -# marker.id = i -# marker.action = Marker.DELETE -# marker_array.markers.append(marker) - -# self.marker_pub.publish(marker_array) -# self.last_marker_count = len(waypoint_names) - -# =============================================================== -# The Main Execution Block -# =============================================================== -def main(args=None): - rclpy.init(args=args) - - # Create instances of both nodes - transformer_node = PoseTransformerNode() - checker_node = OccupancyCheckerNode() - # visualizer_node = WaypointVisualiserNode() - - # Create a MultiThreadedExecutor - executor = MultiThreadedExecutor() - - # Add both nodes to the executor - executor.add_node(transformer_node) - executor.add_node(checker_node) - # executor.add_node(visualizer_node) - - try: - # Spin the executor to run both nodes - executor.spin() - except KeyboardInterrupt: - pass - finally: - executor.shutdown() - transformer_node.destroy_node() - checker_node.destroy_node() - # visualizer_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/param_processing.py b/topological_navigation/topological_navigation/scripts/param_processing.py deleted file mode 100644 index 2519e630..00000000 --- a/topological_navigation/topological_navigation/scripts/param_processing.py +++ /dev/null @@ -1,139 +0,0 @@ -from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -from rcl_interfaces.msg import ParameterDescriptor, ParameterValue, ParameterType -from rclpy import Parameter -import rclpy -from rclpy.node import Node -from rclpy.executors import SingleThreadedExecutor - -from rclpy.callback_groups import ReentrantCallbackGroup - -class ParameterUpdaterNode(Node): - def __init__(self, server_name): - super().__init__(server_name + '_param_update_node') - self.callback_group = ReentrantCallbackGroup() - self.cli_set_param = self.create_client(SetParameters, '/' + server_name + '/set_parameters', callback_group=self.callback_group) - self.cli_list_params = self.create_client(ListParameters, '/' + server_name + '/list_parameters' , callback_group=self.callback_group) - self.cli_get_params = self.create_client(GetParameters, '/' + server_name + '/get_parameters', callback_group=self.callback_group) - counter = -1 - while not self.cli_set_param.wait_for_service(timeout_sec=1.0): - self.get_logger().warning('service not available, waiting again... {}'.format('/' + server_name + '/set_parameters')) - counter += 1 - if(counter > 2): - self.get_logger().warning('service not available {}'.format('/' + server_name + '/set_parameters')) - counter = 0 - break - - if(counter == 0): - self.get_logger().error('service /{} is not available'.format(server_name)) - else: - self.get_logger().info('service /{} is not available'.format(server_name)) - - self.internal_executor = SingleThreadedExecutor() - - def get_parameter_value(self, parameter_value): - if parameter_value.type == Parameter.Type.INTEGER: - return parameter_value.integer_value - elif parameter_value.type == Parameter.Type.DOUBLE: - return parameter_value.double_value - elif parameter_value.type == Parameter.Type.BOOL: - return parameter_value.bool_value - elif parameter_value.type == Parameter.Type.STRING: - return parameter_value.string_value - elif parameter_value.type == Parameter.Type.INTEGER_ARRAY: - return parameter_value.integer_array_value - elif parameter_value.type == Parameter.Type.DOUBLE_ARRAY: - return parameter_value.double_array_value - elif parameter_value.type == Parameter.Type.BOOL_ARRAY: - return parameter_value.bool_array_value - elif parameter_value.type == Parameter.Type.STRING_ARRAY: - return parameter_value.string_array_value - else: - # Handle unsupported or unknown types - return None - - def set_params(self, params): - self.req = SetParameters.Request() - self.req.parameters = [] - for key, value in params.items(): - param_name, param_value = key, value - self.req.parameters.append(Parameter(name=param_name, value=param_value).to_parameter_msg()) - - while not self.cli_set_param.wait_for_service(timeout_sec=0.5): - return False - - self.future = self.cli_set_param.call_async(self.req) - while rclpy.ok(): - try: - rclpy.spin_once(self, executor=self.internal_executor) - if self.future.done(): - try: - response = self.future.result().results - if response[0].successful: - return True - except Exception as e: - self.get_logger().error(" Error while setting params {} ".format(e)) - return False - return False - except Exception as e: - self.get_logger().error(" Error while setting params {} ".format(e)) - return False - - def list_params(self, ): - self.req_list = ListParameters.Request() - while not self.cli_set_param.wait_for_service(timeout_sec=0.5): - return [] - self.future = self.cli_list_params.call_async(self.req_list) - while rclpy.ok(): - try: - rclpy.spin_once(self, executor=self.internal_executor) - if self.future.done(): - try: - response = self.future.result().result - if response: - return response.names - except Exception as e: - self.get_logger().error("Error while list_params list {}".format(e)) - return [] - except Exception as e: - self.get_logger().error("Error while list_params list {}".format(e)) - return [] - - def get_params(self, ): - param_names = self.list_params() - param_values = {} - params = {} - self.req_get = GetParameters.Request() - self.req_get.names = param_names - while not self.cli_set_param.wait_for_service(timeout_sec=0.5): - return {} - self.future = self.cli_get_params.call_async(self.req_get) - while rclpy.ok(): - try: - rclpy.spin_once(self, executor=self.internal_executor) - if self.future.done(): - try: - response = self.future.result() - if response: - param_values = response.values - break - except Exception as e: - self.get_logger().error("Error while getting paramer list {}".format(e)) - break - except Exception as e: - self.get_logger().error("Error while getting paramer list {}".format(e)) - break - if param_values: - for key, value in zip(param_names, param_values): - - params[key] = self.get_parameter_value(value) - return params - - -# if __name__ == '__main__': -# rclpy.init(args=None) -# try_params = ParameterUpdaterNode("controller_server") -# # params = try_params.get_params() -# # try_params.get_logger().info(" {}".format(params)) - -# params = {'FollowPath.xy_goal_tolerance': 0.3} -# try_params.set_params(params) \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/reconf_at_edges_server.py b/topological_navigation/topological_navigation/scripts/reconf_at_edges_server.py deleted file mode 100755 index d4248ca9..00000000 --- a/topological_navigation/topological_navigation/scripts/reconf_at_edges_server.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python -import rospy, dynamic_reconfigure.client -from topological_navigation_msgs.srv import ReconfAtEdges, ReconfAtEdgesResponse - - -class reconf_at_edges_server(object): - - - def __init__(self): - self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {}) - rospy.Service('reconf_at_edges', ReconfAtEdges, self.handle_reconf_at_edges) - - - def handle_reconf_at_edges(self, req): - - self.success = False - - for group in self.edge_groups: - - if req.edge_id in self.edge_groups[group]["edges"]: - rospy.loginfo("\nSetting parameters for edge group {} ...".format(group)) - - self.set_parameters(group) - break - - return ReconfAtEdgesResponse(self.success) - - - def set_parameters(self, group): - - try: - for param in self.edge_groups[group]["parameters"]: - print("Setting {} = {}".format("/".join((param["ns"], param["name"])), param["value"])) - - rcnfclient = dynamic_reconfigure.client.Client(param["ns"], timeout=5.0) - rcnfclient.update_configuration({param["name"]: param["value"]}) - - self.success = True - - except rospy.ROSException as e: - rospy.logerr(e) - - - - -if __name__ == "__main__": - - rospy.init_node('reconf_at_edges_server', anonymous=True) - reconf_at_edges_server() - rospy.spin() -##################################################################################### \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/restrictions_manager.py b/topological_navigation/topological_navigation/scripts/restrictions_manager.py deleted file mode 100755 index cf12e513..00000000 --- a/topological_navigation/topological_navigation/scripts/restrictions_manager.py +++ /dev/null @@ -1,382 +0,0 @@ -#!/usr/bin/env python -import rospy -import sympy -import copy -import json -import yaml -import sys -import os -import inspect -from topological_navigation_msgs.srv import RestrictMap, RestrictMapResponse,\ - EvaluateNode, EvaluateNodeResponse, EvaluateEdge, EvaluateEdgeResponse -from topological_navigation.manager2 import map_manager_2 -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation_msgs.srv import GetTaggedNodes, GetTaggedNodesRequest -from std_msgs.msg import String -from sympy.parsing.sympy_parser import parse_expr -from topological_navigation.restrictions_impl import * - - -class RestrictionsManager(): - def __init__(self, robots, tasks, out_topic): - self.robots = robots - self.tasks = tasks - - # here are the conditions that can be evaluiated in the predicates - self.conditions = {} - # get the topological map - self.topo_map = None - # dictionary of nodes indexed by name - self.nodes = {} - # dictionary of edges indexed by name - self.edges = {} - # for each node contains the indexes of the nodes which have an edge ending in it - self.back_edges_idx = {} - - - # publishers for restricted tmap - self.restricted_tmap_pub = rospy.Publisher(out_topic, - TopologicalMap, queue_size=10, latch=True) - self.restricted_tmap2_pub = rospy.Publisher( - "{}_2".format(out_topic), String, queue_size=10, - latch=True) - - def register_restriction(self, condition_obj): - """ This method receive a condition implementing AbstractRestriction class that can be evaluated""" - self.conditions.update({ - condition_obj.name: condition_obj - }) - rospy.loginfo( - "New condition `{}` registered".format(condition_obj.name)) - - def register_connections(self): - rospy.Subscriber("/topological_map_2", - String, self._topomap_cb) - rospy.loginfo("Restrictions manager waiting for the Topological Map...") - while self.topo_map is None: - rospy.sleep(0.5) - rospy.loginfo("Restrictions manager received the Topological Map") - - rospy.Service("restrictions_manager/restrict_planning_map", - RestrictMap, self.restrict_planning_map_handle) - rospy.Service("restrictions_manager/restrict_runtime_map", - RestrictMap, self.restrict_runtime_map_handle) - rospy.Service("restrictions_manager/evaluate_node", - EvaluateNode, self.evaluate_node_handle) - rospy.Service("restrictions_manager/evaluate_edge", - EvaluateEdge, self.evaluate_edge_handle) - - # Create a predicate from the restrictions string and returns the conditions to be checked - def _predicate_from_string(self, restriction_str): - predicate = True - if restriction_str == "": - restriction_str = "True" - try: - predicate = parse_expr(restriction_str) - except Exception as e: - rospy.logerr( - "The restriction expression is malformed, error: {}".format(e)) - rospy.logerr("The expression can contain the following conditions: {}\n and the following boolean symbols: &, |, ~, (, ).".format( - self.conditions.keys())) - else: - if isinstance(predicate, dict): - rospy.logerr( - "The restriction expression is malformed: {}".format(predicate)) - predicate = True - elif not isinstance(predicate, bool): - conds = predicate.atoms() - for cond in conds: - cond_els = cond.name.split("_") - if cond_els[0] not in self.conditions.keys(): - rospy.logwarn("The restriction `{}` has not been implemented yet, setting `{} = False`.".format( - cond_els[0], cond.name)) - # setting the restriction to false - predicate = predicate.subs({ - cond.name: False - }) - predicate = predicate.simplify() - return predicate - - def _evaluate_restrictions(self, restrictions, entity_name, robot_state, for_node=True): - # print("evaluate restrictions `{}` for {} and robot_state {}".format(restrictions, entity_name, robot_state)) - - # get the sympy boolean predicate removing the atoms that cannot be grounded - restriction_predicate = self._predicate_from_string(restrictions) - # print("\tsimplified predicate: {}".format(restriction_predicate)) - - # evaluate each restriction of the predicate until it simplifies to a boolean - while not isinstance(restriction_predicate, bool) and\ - not restriction_predicate is sympy.true and \ - not restriction_predicate is sympy.false: - restriction = restriction_predicate.atoms().pop() - restr_elements = restriction.name.split("_") - condition = self.conditions[restr_elements[0]] - if for_node: - evaluation = condition.evaluate_node( - node = entity_name, - value = restr_elements[1] if len( - restr_elements) > 1 else None, - robot_state=robot_state, - tmap=self.topo_map - ) - else: - evaluation = condition.evaluate_edge( - edge = entity_name, - value = restr_elements[1] if len( - restr_elements) > 1 else None, - robot_state=robot_state, - tmap=self.topo_map - ) - # print("\tsubstituting {} to {}".format(evaluation, restriction.name)) - - # ground the restriction and simplify the predicate - restriction_predicate = restriction_predicate.subs({ - restriction.name: evaluation - }).simplify() - - # print("\t\t res: {}, {}".format( - # restriction_predicate, type(restriction_predicate))) - - # here we negate because if the restriction matches (True) it means the robot is allowed to pass (not restricted) - return not restriction_predicate - - def restrict_planning_map_handle(self, request): - response = RestrictMapResponse() - response.success = True - - new_topo_map = self._restrict_map_handle(request, "restrictions_planning") - - response.restricted_tmap = json.dumps(new_topo_map) - - return response - - def restrict_runtime_map_handle(self, request): - response = RestrictMapResponse() - response.success = True - - new_topo_map = self._restrict_map_handle(request, "restrictions_runtime") - - response.restricted_tmap = json.dumps(new_topo_map) - - return response - - def evaluate_node_handle(self, request): - response = EvaluateNodeResponse() - response.success = True - - try: - robot_state = eval(request.state) - except: - # rospy.logwarn("Robot state data conversion to dictionary not valid, skip message") - robot_state = {} - - if request.node not in self.nodes: - response.success = False - rospy.logwarn("Received node name {} which is not present in the tmap".format(request.node)) - else: - if request.runtime: - node_restrictions = self.nodes[request.node]["node"]["restrictions_runtime"] - else: - node_restrictions = self.nodes[request.node]["node"]["restrictions_planning"] - - response.evaluation = self._evaluate_restrictions(node_restrictions, request.node, robot_state, for_node=True) - - return response - - def evaluate_edge_handle(self, request): - response = EvaluateEdgeResponse() - response.success = True - try: - robot_state = eval(request.state) - except: - # rospy.logwarn("Robot state data conversion to dictionary not valid, skip message") - robot_state = {} - - if request.edge not in self.edges: - response.success = False - rospy.logwarn("Received edge name {} which is not present in the tmap".format(request.edge)) - else: - if request.runtime: - edge_restrictions = self.edges[request.edge]["restrictions_runtime"] - else: - edge_restrictions = self.edges[request.edge]["restrictions_planning"] - - response.evaluation = self._evaluate_restrictions(edge_restrictions, request.edge, robot_state, for_node=False) - - return response - - def satisfy_runtime_restrictions(self, request): - response = SatisfyRuntimeResponse() - response.success = True - response.satisfied = True - - if request.edge not in self.edges: - response.success = False - rospy.logwarn("Received edge name {} which is not present in the tmap".format(request.edge)) - else: - edge_restrictions = self.edges[request.edge]["restrictions_runtime"] - - predicate = self._predicate_from_string(edge_restrictions) - if not isinstance(predicate, bool) and\ - not predicate is sympy.true and \ - not predicate is sympy.false: - # satisfy the restrictions of each atom for now...but it doesn't make much sense conceptually - for atom in predicate.atoms(): - restriction_name = atom.name.split("_")[0] - - response.satisfied = response.satisfied and self.conditions[restriction_name].satisfy_restriction() - - return response - - def _restrict_map_handle(self, request, restrictions_arg): - new_topo_map = copy.deepcopy(self.topo_map) - robot_state = {} - try: - robot_state = eval(request.state) - except: - pass - # rospy.logwarn("Robot state data conversion to dictionary not valid, skip message") - # robot_state = None - finally: - # tmp_topo_map = copy.deepcopy(self.topo_map) - to_remove_edges = {} - to_remove_nodes = set({}) - # for each node, check restrictions - for i, node in enumerate(self.topo_map["nodes"]): - node_restrictions = str(node["node"][restrictions_arg]) - - is_restricted = self._evaluate_restrictions(node_restrictions, node["meta"]["node"], robot_state, for_node=True) - - # remove the node if the restriction evaluates as True - if is_restricted: - # print("{}\n\tremoving node {}".format( - # node_restrictions, new_topo_map["nodes"][i]["meta"]["node"])) - to_remove_nodes.add(i) - - # remove all the back edges - for (ni, ei) in self.back_edges_idx[node["meta"]["node"]].items(): - # print("\n\t removing edge {}".format( - # new_topo_map["nodes"][ni]["node"]["edges"][ei]["edge_id"])) - if ni in to_remove_edges: - to_remove_edges[ni].add(ei) - else: - to_remove_edges.update({ni: set({ei})}) - else: - # here now check the restrictions for the node's edges - for ei, edge in enumerate(node["node"]["edges"]): - edge_restrictions = str(edge[restrictions_arg]) - - is_restricted = self._evaluate_restrictions(edge_restrictions, edge["edge_id"], robot_state, for_node=False) - - if is_restricted: - # print("{}\n\t removing edge {}".format(node_restrictions, - # new_topo_map["nodes"][i]["node"]["edges"][ei]["edge_id"])) - if i in to_remove_edges: - to_remove_edges[i].add(ei) - else: - to_remove_edges.update({i: set({ei})}) - - # now remove all the edges - for ni, eis in to_remove_edges.items(): - for ei in sorted(eis, reverse=True): - del new_topo_map["nodes"][ni]["node"]["edges"][ei] - # now remove all the nodes - for ni in sorted(to_remove_nodes, reverse=True): - del new_topo_map["nodes"][ni] - - return new_topo_map - - def _topomap_cb(self, msg): - rospy.loginfo("Received updated topomap") - self.topo_map = json.loads(msg.data) - # print("got topomap", self.topo_map["nodes"][0]) - - for ni, node in enumerate(self.topo_map["nodes"]): - self.nodes[node["meta"]["node"]] = node # save a dictionary of nodes - self.back_edges_idx[node["meta"]["node"]] = {} # it should contain all the nodes even if they don't have edges - for ei, edge in enumerate(node["node"]["edges"]): - self.edges[edge["edge_id"]] = edge # save a disctionary of edges - if edge["node"] in self.back_edges_idx: - self.back_edges_idx[edge["node"]].update({ - ni: ei - }) - else: - self.back_edges_idx.update({ - edge["node"]: {ni: ei} - }) - - rospy.loginfo("Creating restricted topomaps") - restricted_tmap2 = self._restrict_map_handle({}, "restrictions_planning") - - self._publish_updated_restricted_maps(restricted_tmap2) - rospy.loginfo("Restricted map updated and published") - - def _publish_updated_restricted_maps(self, restricted_tmap2): - self.restricted_tmap2_pub.publish(json.dumps(restricted_tmap2)) - - old_restricted_tmap = map_manager_2.convert_tmap2_to_tmap( - restricted_tmap2, restricted_tmap2["pointset"], restricted_tmap2["metric_map"]) - - self.restricted_tmap_pub.publish(old_restricted_tmap) - -def get_admissible_robots(config): - robots = [] - if "admissible_robot_ids" in config: - robots = config["admissible_robot_ids"] - - return robots - -def get_admissible_tasks(config): - tasks = [] - if "active_tasks" in config: - tasks = config["active_tasks"] - - return tasks - -if __name__ == '__main__': - rospy.init_node("topological_restrictions_manager") - - _tasks = [] - _robots = [] - - restricted_map_topic = rospy.get_param("~out_topic", "restricted_topological_map") - - try: - config_file = rospy.get_param("~config_file") - except KeyError: - rospy.logwarn("Config file not provided.") - pass - else: - # this is the coordinator config file - if os.path.isfile(config_file): - _config = {} - with open(config_file, "r") as f_handle: - _config = yaml.load(f_handle) - - # which robots can stay in the field - _robots = get_admissible_robots(_config) - # which tasks we can executed in the field - _tasks = get_admissible_tasks(_config) - - if len(_tasks) == 0: - rospy.logwarn("No admissible tasks read from config file.") - if len(_robots) == 0: - rospy.logwarn("No admissible robot read in config file.") - - manager = RestrictionsManager(robots=_robots, tasks=_tasks, out_topic=restricted_map_topic) - - # automatically find and register all the classes of AbstractRestriction type - clsmembers = inspect.getmembers(sys.modules[__name__], inspect.isclass) - for restriction in clsmembers: - if issubclass(restriction[1], AbstractRestriction) and not inspect.isabstract(restriction[1]): - cls_argnames = inspect.getargspec(restriction[1].__init__).args - args = {} - if "robots" in cls_argnames: - args["robots"] = _robots - if "tasks" in cls_argnames: - args["tasks"] = _tasks - manager.register_restriction(restriction[1](**args)) - - manager.register_connections() - - rospy.spin() diff --git a/topological_navigation/topological_navigation/scripts/search_route.py b/topological_navigation/topological_navigation/scripts/search_route.py deleted file mode 100755 index 870b38c7..00000000 --- a/topological_navigation/topological_navigation/scripts/search_route.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -#import actionlib -#import pymongo -#import json -import sys -import math - - -#from geometry_msgs.msg import Pose -from std_msgs.msg import String -#import scitos_apps_msgs.msg - -#from topological_navigation_msgs.msg import TopologicalNode -#from mongodb_store.message_store import MessageStoreProxy - -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation.tmap_utils import * -from topological_navigation.route_search import * - - -class SearchRoute(object): - - def __init__(self, goal) : - rospy.loginfo("Waiting for Topological map ...") - - try: - msg = rospy.wait_for_message('topological_map', TopologicalMap, timeout=10.0) - self.top_map = msg - self.lnodes = msg.nodes - except rospy.ROSException : - rospy.logwarn("Failed to get topological map") - return - - rospy.loginfo("Waiting for Current Node ...") - - try: - msg = rospy.wait_for_message('closest_node', String, timeout=10.0) - cnode = msg.data - except rospy.ROSException : - rospy.logwarn("Failed to get closest node") - return - - - rsearch = TopologicalRouteSearch(self.top_map) - route = rsearch.search_route(cnode, goal) - print(route) - - rospy.loginfo("All Done ...") - #rospy.spin() - - -if __name__ == '__main__': - rospy.init_node('route_search') - goal = str(sys.argv[1]) - searcher = SearchRoute(goal) \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/speed_based_prediction.py b/topological_navigation/topological_navigation/scripts/speed_based_prediction.py deleted file mode 100755 index 105eef5c..00000000 --- a/topological_navigation/topological_navigation/scripts/speed_based_prediction.py +++ /dev/null @@ -1,119 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import actionlib -import pymongo -import json -import sys -import math -import time - -from datetime import datetime - -import actionlib -from actionlib_msgs.msg import * -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseStamped -from std_msgs.msg import Header -from std_msgs.msg import String -from nav_msgs.srv import * - - -import topological_navigation_msgs.msg -from topological_navigation_msgs.msg import TopologicalNode -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation_msgs.msg import NavStatistics -from topological_navigation_msgs.msg import TopologicalMap - -from topological_navigation.tmap_utils import * - -from topological_navigation_msgs.srv import * - - -def usage(): - print("For using all the available stats use:") - print("\t rosrun topological_navigation topological_prediction.py") - print("For all the stats in a range use:") - print("\t rosrun topological_navigation topological_prediction.py from_epoch to_epoch") - print("For all the stats from a date until now use:") - print("\t rosrun topological_navigation topological_prediction.py from_epoch -1") - print("For all the stats until one date:") - print("\t rosrun topological_navigation topological_prediction.py 0 to_epoch") - - -class TopologicalSpeedPred(object): - - def __init__(self) : - - name= rospy.get_name() - action_name = name+'/build_temporal_model' - - rospy.Subscriber('topological_map', TopologicalMap, self.map_callback) - - rospy.loginfo("Waiting for Topological map ...") - - self.map_received = False - self.edge_to_duration = {} - - while not self.map_received: - rospy.sleep(rospy.Duration(1)) - rospy.loginfo("Waiting for Topological map ...") - - - rospy.loginfo("... Got Topological map") - - - #Creating Action Server - rospy.loginfo("Creating action server.") - self._as = actionlib.SimpleActionServer(action_name, topological_navigation_msgs.msg.BuildTopPredictionAction, execute_cb = self.build_callback, auto_start = False) - - rospy.loginfo(" ...starting") - self._as.start() - rospy.loginfo(" ...done") - - - self.predict_srv=rospy.Service('topological_prediction/predict_edges', topological_navigation_msgs.srv.PredictEdgeState, self.predict_edge_cb) - rospy.loginfo("All Done ...") - rospy.spin() - - - - def predict_edge_cb(self, req): - return self.edge_to_duration.keys(), [1] * len(self.edge_to_duration), self.edge_to_duration.values() - - """ - map_callback - - This function receives the topological map the computes duration estimates for all edges - """ - def map_callback(self, tmap) : - self.edge_to_duration = {} - - print('tmap ', tmap) - - for i in tmap.nodes : - print(i) - for j in i.edges: - print(j) - if j.edge_id not in self.edge_to_duration: - destination = get_node(tmap, j.node) - if j.top_vel >= 0.1: - self.edge_to_duration[j.edge_id] = rospy.Duration(get_distance_to_node(i, destination)/j.top_vel) - else : - self.edge_to_duration[j.edge_id] = rospy.Duration(get_distance_to_node(i, destination)/0.1) - - self.map_received = True - - """ - build_callback - - Does nothing for this version. - """ - def build_callback(self, goal): - self._as.set_succeeded() - - -if __name__ == '__main__': - rospy.init_node('topological_prediction') - server = TopologicalSpeedPred() diff --git a/topological_navigation/topological_navigation/scripts/topological_prediction.py b/topological_navigation/topological_navigation/scripts/topological_prediction.py deleted file mode 100755 index 7dbee41b..00000000 --- a/topological_navigation/topological_navigation/scripts/topological_prediction.py +++ /dev/null @@ -1,770 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import actionlib -import random -import numpy - - -from threading import Lock -from datetime import datetime - - -import std_msgs.msg - -import topological_navigation_msgs.msg -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation_msgs.msg import NavStatistics -from topological_navigation_msgs.msg import TopologicalMap - -from topological_navigation.tmap_utils import * - -from topological_navigation_msgs.srv import * -import fremenserver.msg - - -def usage(): - print("\nFor using all the available stats use:") - print("\t rosrun topological_navigation topological_prediction.py") - print("For all the stats in a range use:") - print("\t rosrun topological_navigation topological_prediction.py -range from_epoch to_epoch") - print("For all the stats from a date until now use:") - print("\t rosrun topological_navigation topological_prediction.py -range from_epoch -1") - print("For all the stats until one date:") - print("\t rosrun topological_navigation topological_prediction.py -range 0 to_epoch") - - -def get_model(name, models): - for i in models: - if i["model_id"] == name: - return i - return None - - -class TopologicalNavPred(object): - - _feedback = topological_navigation_msgs.msg.BuildTopPredictionFeedback() - _result = topological_navigation_msgs.msg.BuildTopPredictionResult() - - def __init__(self, epochs) : - rospy.on_shutdown(self._on_node_shutdown) - self.lnodes = [] - self.map_received =False - self.range = epochs - self.srv_lock=Lock() - name= rospy.get_name() - action_name = name+'/build_temporal_model' - - self.ignore_map_name = rospy.get_param("~ignore_map_name", False) - - if self.ignore_map_name: - rospy.logwarn("Ignoring map name in model creation. All stats will be considered") - print(self.ignore_map_name) - else: - rospy.logwarn("Using map name in model creation. Only stats for current map will be considered") - print(self.ignore_map_name) - - - # Creating fremen server client - rospy.loginfo("Creating fremen server client.") - self.FremenClient= actionlib.SimpleActionClient('fremenserver', fremenserver.msg.FremenAction) - self.FremenClient.wait_for_server() - rospy.loginfo(" ...done") - - - #Creating Action Server - rospy.loginfo("Creating action server.") - self._as = actionlib.SimpleActionServer(action_name, topological_navigation_msgs.msg.BuildTopPredictionAction, execute_cb = self.BuildCallback, auto_start = False) - self._as.register_preempt_callback(self.preemptCallback) - rospy.loginfo(" ...starting") - self._as.start() - rospy.loginfo(" ...done") - - - # Get Topological Map - rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) - - rospy.loginfo("Waiting for Topological map ...") - while not self.map_received: - rospy.sleep(rospy.Duration(1.0)) - #rospy.loginfo("Waiting for Topological map ...") - rospy.loginfo("... Got Topological map") - - - self.predict_srv=rospy.Service('topological_prediction/predict_edges', topological_navigation_msgs.srv.PredictEdgeState, self.predict_edge_cb) - self.predict_srv=rospy.Service('topological_prediction/edge_entropies', topological_navigation_msgs.srv.PredictEdgeState, self.edge_entropies_cb) - - - rospy.loginfo("Set-Up Fremenserver monitors") - #Fremen Server Monitor - self.fremen_monitor = rospy.Timer(rospy.Duration(10), self.monitor_cb) - # Subscribe to fremen server start topic - rospy.Subscriber('fremenserver_start', std_msgs.msg.Bool, self.fremen_start_cb) - rospy.loginfo("... Done") - - - rospy.loginfo("Subscribing to new stats ...") - rospy.Subscriber('topological_navigation/Statistics', NavStatistics, self.stats_callback, queue_size=10) - - rospy.loginfo("All Done ...") - rospy.spin() - - - - def MapCallback(self, msg) : - """ - MapCallback - - This function receives the Topological Map - """ - self.lnodes = msg - - if self.ignore_map_name: - self.model_base_name='topological-prediction' - else: - self.model_base_name= self.lnodes.name - - if self.map_received: - rospy.logwarn("New topological map detected will generate new models now") - with self.srv_lock: - self.create_temporal_models() - - self.map_received = True - - - def fremen_start_cb(self, msg) : - """ - fremen_start_cb - - This function creates the models when the fremenserver is started - """ - if msg.data: - rospy.logwarn("FREMENSERVER restart detected will generate new models now") - with self.srv_lock: - self.create_temporal_models() - - - def stats_callback(self, msg): - """ - stats_callback - - This function adds values to known models when an edge is traversed - """ - #print "New Stat: ", msg - model_name = self.model_base_name+'__'+msg.edge_id - #time_model_name = self.lnodes.name+'__'+msg.edge_id+'_time' - epoch = int(datetime.strptime(msg.date_started, "%A, %B %d %Y, at %H:%M:%S hours").strftime('%s')) - - model = get_model(model_name, self.models) - - if model: - time_model_name = model["time_model_id"] - distance = model['dist'] - - - #print epoch, model_name, time_model_name - - if msg.status in self.sucesses: - status = 1 - speed = distance/msg.operation_time - if speed>1: - speed=1.0 - #time = msg.operation_time - elif msg.status in self.fails: - status = 0 - speed = 0.0 - #time = msg.operation_time - - #print [status], time, [speed] - - fremgoal = fremenserver.msg.FremenGoal() - fremgoal.operation = 'add' - fremgoal.id = model_name - fremgoal.times = [epoch] - fremgoal.states = [status] - - # Sends the goal to the action server. - self.FremenClient.send_goal(fremgoal) - - # Waits for the server to finish performing the action. - self.FremenClient.wait_for_result() - - # Prints out the result of executing the action - ps = self.FremenClient.get_result() - - #print fremgoal, ps - - - fremgoalt = fremenserver.msg.FremenGoal() - fremgoalt.operation = 'addvalues' - fremgoalt.id = time_model_name - fremgoalt.times = [epoch] - fremgoalt.states = [0.5] - fremgoalt.values = [speed] - - # Sends the goal to the action server. - self.FremenClient.send_goal(fremgoalt) - - #print "Sending data to fremenserver" - - - # Waits for the server to finish performing the action. - self.FremenClient.wait_for_result() - - #print "fremenserver done" - - # Prints out the result of executing the action - ps = self.FremenClient.get_result() - #print fremgoalt, ps - else: - rospy.logwarn("Model %s hasn't been created yet. No info will be added to the models until models are built again" %model_name) - - - - - def create_temporal_models(self): - """ - create_temporal_models - - This function creates the temporal models used for prediction - """ - self.unknowns = [] - - rospy.loginfo("Creating Data Structures") - #start_time = rospy.Time.now() - self.get_list_of_edges() - #elapsed_time1 = rospy.Time.now()-start_time - #print elapsed_time1.secs+(elapsed_time1.nsecs/1e09) - rospy.loginfo("Gathering Data") - to_add=self.gather_stats() - #elapsed_time2 = rospy.Time.now()-start_time - #print elapsed_time2.secs+(elapsed_time2.nsecs/1e09) - rospy.loginfo("Create Fremen Models") - self.create_fremen_models(to_add) - rospy.loginfo(" ...done") - #elapsed_time3 = rospy.Time.now()-start_time - #print elapsed_time3.secs+(elapsed_time3.nsecs/1e09) - #print elapsed_time1.secs+(elapsed_time1.nsecs/1e09), elapsed_time2.secs+(elapsed_time2.nsecs/1e09) - - - def get_list_of_edges(self): - """ - get_list_of_edges - - This function creates the memory structures per edge of the topological map - """ - self.edgid=[] - self.eids = [] #Empty previous models - rospy.loginfo("Querying for list of edges") - for i in self.lnodes.nodes : - for j in i.edges: - if j.edge_id not in self.edgid : - self.edgid.append(j.edge_id) - val={} - val["edge_id"]=j.edge_id - val["model_id"]=self.model_base_name+'__'+j.edge_id - val["time_model_id"]=self.model_base_name+'__'+j.edge_id+'_time' - val["ori"]=i.name - val["dest"]=j.node - ddn=get_node(self.lnodes, j.node) - val["dist"]= get_distance_to_node(i,ddn) - self.eids.append(val) - fdbmsg = 'Done. %d edges found' %len(self.edgid) - rospy.loginfo(fdbmsg) - - - def gather_stats(self): - """ - gather_stats - - This function fill the data structures using the navigation stats - """ - stats_collection = rospy.get_param('~stats_collection', 'nav_stats') - msg_store = MessageStoreProxy(collection=stats_collection) - to_add=[] - - self.sucesses = rospy.get_param('topological_prediction/success_values', ['success','failed']) - self.fails = rospy.get_param('topological_prediction/fail_values', ['fatal']) - - rospy.set_param('topological_prediction/success_values',self.sucesses) - rospy.set_param('topological_prediction/fail_values',self.fails) - print("++++++++++++++++++++++++++++++++++") - print("++++++++++++++++++++++++++++++++++" ) - print("successes:") - print(self.sucesses) - print("fails:") - print(self.fails) - print("++++++++++++++++++++++++++++++++++") - print("++++++++++++++++++++++++++++++++++") - - - for i in self.eids: - if self.ignore_map_name: - query = {"edge_id":i["edge_id"]} - else: - query = {"topological_map": self.lnodes.name, "edge_id":i["edge_id"]} - query_meta={} - - if len(self.range) == 2: - if self.range[1] < 1: - upperlim = rospy.Time.now().secs - else: - upperlim = self.range[1] - query_meta["epoch"] = {"$gte": self.range[0], "$lt" : upperlim} - - #print query - #print query_meta - - available = msg_store.query(NavStatistics._type, query, query_meta) - # print len(available) - edge_mod={} - edge_mod["model_id"]= i["model_id"]#self.lnodes.name+'__'+i["edge_id"] - edge_mod["time_model_id"]=i["time_model_id"] - edge_mod["dist"]= i["dist"]#self.lnodes.name+'__'+i["edge_id"] - edge_mod["models"]=[] - edge_mod["order"]=-1 - edge_mod["t_order"]=-1 - edge_mod["edge_id"]=i["edge_id"] - - for j in available: - val = {} - if j[0].status in self.sucesses: - val["st"] = 1 - val["speed"] = i["dist"]/j[0].operation_time - if val["speed"]>1: - val["speed"]=1.0 - val["epoch"] = int(datetime.strptime(j[0].date_started, "%A, %B %d %Y, at %H:%M:%S hours").strftime('%s')) - val["optime"] = j[0].operation_time - edge_mod["models"].append(val) - elif j[0].status in self.fails: - val["st"] = 0 - val["speed"] = 0.0 - val["epoch"] = int(datetime.strptime(j[0].date_started, "%A, %B %d %Y, at %H:%M:%S hours").strftime('%s')) - val["optime"] = j[0].operation_time - edge_mod["models"].append(val) - - edge_mod["samples"]=len(edge_mod["models"]) - if len(available) > 0 : - to_add.append(edge_mod) - else : - self.unknowns.append(edge_mod) - return to_add - - - - def create_fremen_models(self, models): - self.models = models - for i in models: - # print "-------CREATING MODEL----------" - - mid = i["model_id"] - tmid = i["time_model_id"] - - print("Creating models:") - print(mid, tmid) - #print i["dist"] - stimes=[] - times=[] - states=[] - speeds=[] - #print "adding %d meassurements" %len(i["models"]) - for j in i["models"]: - times.append(j["epoch"]) - states.append(j["st"]) - #print j - if j["speed"]>0: - speeds.append(j["speed"]) - stimes.append(j["epoch"]) - i["order"] = self.add_and_eval_models(mid,times,states) - -# print "###########################################" -# print "Generating speed models: ", tmid -# print "epochs (%d): " %len(stimes) -# print stimes -# print "speeds (%d): " %len(speeds) -# print speeds - - i["t_order"] = self.add_and_eval_value_models(tmid,stimes,speeds) -# i["t_order"] = self.add_and_eval_models(tmid,stimes,speeds) - print("Done Model Order %d" %i["t_order"]) - print("samples", len(i["models"])) - print("------------------------------------") - #print times - #print states - - - def add_and_eval_models(self, model_id, epochs, states): - # Choosing the samples used for model building and evaluation - sampling_type = rospy.get_param('door_prediction/model_building/sampling_type', 0) #0 for ordered (extrapolation), 1 for random (intrapolation) - - if len(epochs)>0: - if sampling_type == 0: - #ordered sampling - - #samples for result sampling - index_b = range(int(numpy.ceil(len(epochs)*0.75))) - index_e = range(int(numpy.ceil(len(epochs)*0.75)),len(epochs)) - - else: - #random sampling - - #samples for result sampling - index_b = sorted(random.sample(xrange(len(epochs)), int(numpy.ceil(len(epochs)*0.8)))) - index_e = [] - for i in range(len(epochs)): - if i not in index_b: - index_e.append(i) - - if not index_e: - index_e = random.sample(xrange(len(epochs)), 1) - - epochs_build = [ epochs[i] for i in index_b] - epochs_eval = [ epochs[i] for i in index_e] - states_build = [ states[i] for i in index_b] - states_eval = [ states[i] for i in index_e] - - else: - epochs_build = epochs - epochs_eval = epochs - states_build = states - states_eval = states - - fremgoal = fremenserver.msg.FremenGoal() - fremgoal.operation = 'add' - fremgoal.id = model_id - fremgoal.times = epochs_build - fremgoal.states = states_build - - # Sends the goal to the action server. - self.FremenClient.send_goal(fremgoal) - - # Waits for the server to finish performing the action. - self.FremenClient.wait_for_result() - - # Prints out the result of executing the action - ps = self.FremenClient.get_result() - #print ps - - # print "--- EVALUATE ---" - frevgoal = fremenserver.msg.FremenGoal() - frevgoal.operation = 'evaluate' - frevgoal.id = model_id - frevgoal.times = epochs_eval - frevgoal.states = states_eval - frevgoal.order = 5 - - self.FremenClient.send_goal(frevgoal) - self.FremenClient.wait_for_result() - pse = self.FremenClient.get_result() - # print pse.errors - # print "chosen order %d" %pse.errors.index(min(pse.errors)) - return pse.errors.index(min(pse.errors)) - - - def add_and_eval_value_models(self, model_id, epochs, states): - # Choosing the samples used for model building and evaluation - sampling_type = rospy.get_param('door_prediction/model_building/sampling_type', 0) #0 for ordered (extrapolation), 1 for random (intrapolation) - - if len(epochs)>0: - if sampling_type == 0: - #ordered sampling - - #samples for result sampling - index_b = range(int(numpy.ceil(len(epochs)*0.75))) - index_e = range(int(numpy.ceil(len(epochs)*0.75)),len(epochs)) - - else: - #random sampling - - #samples for result sampling - index_b = sorted(random.sample(xrange(len(epochs)), int(numpy.ceil(len(epochs)*0.8)))) - index_e = [] - for i in range(len(epochs)): - if i not in index_b: - index_e.append(i) - - if not index_e: - index_e = random.sample(xrange(len(epochs)), 1) - - epochs_build = [ epochs[i] for i in index_b] - epochs_eval = [ epochs[i] for i in index_e] - states_build = [ states[i] for i in index_b] - states_eval = [ states[i] for i in index_e] - - else: - epochs_build = epochs - epochs_eval = epochs - states_build = states - states_eval = states - - - fremgoal = fremenserver.msg.FremenGoal() - fremgoal.operation = 'addvalues' - fremgoal.id = model_id - fremgoal.times = epochs_build - #fremgoal.states = states - fremgoal.values = states_build - - # Sends the goal to the action server. - self.FremenClient.send_goal(fremgoal) - - #print "Sending data to fremenserver" - - - # Waits for the server to finish performing the action. - self.FremenClient.wait_for_result() - - #print "fremenserver done" - - # Prints out the result of executing the action - ps = self.FremenClient.get_result() - #print "fremenserver result:" - #print ps - - # print "--- EVALUATE ---" - frevgoal = fremenserver.msg.FremenGoal() - frevgoal.operation = 'evaluate' - frevgoal.id = model_id - frevgoal.times = epochs_eval - frevgoal.states = states_eval - frevgoal.order = 5 - - # Sends the goal to the action server. - self.FremenClient.send_goal(frevgoal) - - # Waits for the server to finish performing the action. - self.FremenClient.wait_for_result() - - # Prints out the result of executing the action - pse = self.FremenClient.get_result() # A FibonacciResult - # print pse.errors - # print "chosen order %d" %pse.errors.index(min(pse.errors)) - return pse.errors.index(min(pse.errors)) - - - - - def get_predict(self, epoch): - # print "requesting prediction for time %d" %epoch - edges_ids=[] - dur=[] - - eids = [x['edge_id'] for x in self.models] - mods = [x['model_id'] for x in self.models] - ords = [x['order'] for x in self.models] - tids = [x['time_model_id'] for x in self.models] - tords = [x['t_order'] for x in self.models] - samples = [x['samples'] for x in self.models] - - fremgoal = fremenserver.msg.FremenGoal() - fremgoal.operation = 'forecast' - fremgoal.ids = mods - fremgoal.times.append(epoch) - - fremgoal.order = -1 - fremgoal.orders = ords - - self.FremenClient.send_goal(fremgoal) - self.FremenClient.wait_for_result(timeout=rospy.Duration(10.0)) - - if self.FremenClient.get_state() == actionlib.GoalStatus.SUCCEEDED: - - ps = self.FremenClient.get_result() - #print ps - prob = list(ps.probabilities) - - for j in range(len(mods)): - #alpha=numpy.exp(-samples[j]/50) - #prob[j]=(prob[j]*(1-alpha))+(0.5*alpha) - if samples[j] <= 10: - prob[j]=1.0 - - if prob[j] < 0.05 : - prob[j] = 0.05 - i=get_model(mods[j], self.models) - edges_ids.append(eids[j]) - - - fremgoal = fremenserver.msg.FremenGoal() - fremgoal.operation = 'forecast' - fremgoal.ids = tids - fremgoal.times.append(epoch) - #print i["order"] - fremgoal.order = -1 - fremgoal.orders = tords#i["order"] - - self.FremenClient.send_goal(fremgoal)#,self.done_cb, self.active_cb, self.feedback_cb) - - # Waits for the server to finish performing the action. - self.FremenClient.wait_for_result(timeout=rospy.Duration(10.0)) - - # Prints out the result of executing the action - ps = self.FremenClient.get_result() # A FibonacciResult - - #print ps - - speeds = list(ps.probabilities) - - for j in range(len(mods)): - if samples[j] <= 10: - dur.append(rospy.Duration(self.models[j]["dist"]/0.5)) #default scitos speed - else: - if speeds[j]>0.01: - dur.append(rospy.Duration(self.models[j]["dist"]/speeds[j])) - else: - dur.append(rospy.Duration(self.models[j]["dist"]/0.01)) - - ## Filling up values for edges with no stats available - for i in self.unknowns: - edges_ids.append(i["edge_id"]) - prob.append(1.0) # a priory probabilities (no stats) - est_dur = rospy.Duration(i["dist"]/0.1) - speeds.append(0.1) - dur.append(est_dur) - - -# for k in range(len(edges_ids)): -# print edges_ids[k], prob[k], dur[k].secs, speeds[k] - - return edges_ids, prob, dur - elif not rospy.is_shutdown(): - rospy.logwarn("NO PREDICTIONS RECEIVED FROM FREMENSERVER WILL TRY AGAIN...") - if not self.FremenClient.wait_for_server(rospy.Duration(10.0)): - rospy.logerr("NO CONNECTION TO FREMENSERVER...") - rospy.logwarn("WAITING FOR FREMENSERVER...") - self.FremenClient= actionlib.SimpleActionClient('fremenserver', fremenserver.msg.FremenAction) - self.FremenClient.wait_for_server() - rospy.loginfo(" ...done") - self.create_temporal_models() - rospy.logwarn("WILL TRY TO GET PREDICTIONS AGAIN...") - return self.get_predict(epoch) - - - def predict_edge_cb(self, req): - with self.srv_lock: - return self.get_predict(req.epoch.secs) - - - - def get_entropies(self, epoch): - # print "requesting prediction for time %d" %epoch - #edges_ids=[] - dur=[] - - eids = [x['edge_id'] for x in self.models] - mods = [x['model_id'] for x in self.models] - ords = [x['order'] for x in self.models] - - fremgoal = fremenserver.msg.FremenGoal() - fremgoal.operation = 'forecastEntropy' - fremgoal.ids = mods - fremgoal.times.append(epoch) - #print i["order"] - fremgoal.order = -1 - fremgoal.orders = ords#i["order"] - - self.FremenClient.send_goal(fremgoal) - self.FremenClient.wait_for_result() - ps = self.FremenClient.get_result() - - #print ps - - prob = list(ps.entropies) - - - for i in self.unknowns: - eids.append(i["edge_id"]) - prob.append(1.0) # a priory entropies (no stats) - - #print len(eids), len(prob) - - return eids, prob, dur - - - - def edge_entropies_cb(self, req): - return self.get_entropies(req.epoch.secs) - - - def BuildCallback(self, goal): - """ - BuildCallBack - - This Functions is called when the Action Server is called to build the models again - """ - with self.srv_lock: - self.cancelled = False - - # print goal - - # set epoch ranges based on goal - if goal.start_range.secs > 0: - self.range[0] = goal.start_range.secs - if goal.end_range.secs > 0: - self.range[1] = goal.end_range.secs - - rospy.loginfo('Building model for epoch range: %s' % self.range) - - start_time = rospy.Time.now() - - #start_time = time.time() - #self.get_list_of_edges() - #elapsed_time = time.time() - start_time - #self._feedback.result = "%d edges found in %.3f seconds \nGathering stats ..." %(len(self.eids),elapsed_time) - #self._as.publish_feedback(self._feedback) - #self.gather_stats() - self.create_temporal_models() - - elapsed_timed = rospy.Time.now()-start_time - elapsed_time = elapsed_timed.secs+(elapsed_timed.nsecs/1e09) - self._feedback.result = "Finished after %.3f seconds" %elapsed_time - self._as.publish_feedback(self._feedback) #Publish Feedback - - rospy.loginfo("Finished after %.3f secs" %elapsed_time) - - if not self.cancelled : - self._result.success = True - self._as.set_succeeded(self._result) - else: - self._result.success = False - self._as.set_preempted(self._result) - - - def preemptCallback(self): - self.cancelled = True - - - def monitor_cb(self, events) : - """ - monitor_cb - - This function monitors if fremenserver is still active - """ - if not self.FremenClient.wait_for_server(timeout = rospy.Duration(1)): - rospy.logerr("NO FREMEN SERVER FOUND. Fremenserver restart might be required") - - - def _on_node_shutdown(self): - self.fremen_monitor.shutdown() - - - -if __name__ == '__main__': - rospy.init_node('topological_prediction') - epochs=[] - #if len(sys.argv) < 2: - if '-h' in sys.argv or '--help' in sys.argv: - usage() - sys.exit(1) - else: - if '-range' in sys.argv: - ind = sys.argv.index('-range') - epochs.append(int(sys.argv[ind+1])) - epochs.append(int(sys.argv[ind+2])) - #print epochs - else: - #print "gathering all the stats" - epochs=[0, rospy.get_rostime().to_sec()] - - server = TopologicalNavPred(epochs) \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/topological_transform_publisher.py b/topological_navigation/topological_navigation/scripts/topological_transform_publisher.py deleted file mode 100755 index e0b59778..00000000 --- a/topological_navigation/topological_navigation/scripts/topological_transform_publisher.py +++ /dev/null @@ -1,74 +0,0 @@ -# -*- coding: utf-8 -*- -#!/usr/bin/env python3 -# ---------------------------------- -# @author: Adam Binch -# @email: abinch@sagarobotics.com -# @date: Fri Feb 26 10:37:50 2021 -# ---------------------------------- - -import json -import yaml -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy -import tf2_ros - -from std_msgs.msg import String -from geometry_msgs.msg import Vector3, Quaternion, TransformStamped - -# this ensures that all the poses and translates -# are float-type and not int-type as there is an -# assertion in ros2 messages (vector3, pose etc.) -# for float-type [x,y,z,w] keys. -class CustomSafeLoader(yaml.SafeLoader): - def construct_mapping(self, node, deep=False): - mapping = super().construct_mapping(node, deep=deep) - - # this can be extended to test the validity of the tmap2 - # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w']: - if key in mapping and isinstance(mapping[key], int): - mapping[key] = float(mapping[key]) - - return mapping - - -class TopologicalTransformPublisher(Node): - - def __init__(self): - super().__init__("topological_transform_publisher") - self.tf_broadcaster = tf2_ros.static_transform_broadcaster.StaticTransformBroadcaster(self) - qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.create_subscription(String, '/topological_map_2', self.map_callback, qos) - self.get_logger().info("Transform Publisher waiting for the Topological Map...") - - def map_callback(self, msg): - # tmap = json.loads(msg.data) - tmap = yaml.load( msg.data, Loader=CustomSafeLoader ) - self.get_logger().info("Transform Publisher received the Topological Map") - transformation = tmap["transformation"] - msg = TransformStamped() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = transformation["parent"] - msg.child_frame_id = transformation["child"] - msg.transform.translation.x = transformation["translation"]["x"] - msg.transform.translation.y = transformation["translation"]["y"] - msg.transform.translation.z = transformation["translation"]["z"] - msg.transform.rotation.x = transformation["rotation"]["x"] - msg.transform.rotation.y = transformation["rotation"]["y"] - msg.transform.rotation.z = transformation["rotation"]["z"] - msg.transform.rotation.w = transformation["rotation"]["w"] - self.tf_broadcaster.sendTransform(msg) - - -def main(args=None): - rclpy.init(args=args) - - TTP = TopologicalTransformPublisher() - rclpy.spin(TTP) - - TTP.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/topological_navigation/topological_navigation/scripts/topological_visual.py b/topological_navigation/topological_navigation/scripts/topological_visual.py deleted file mode 100755 index 84633003..00000000 --- a/topological_navigation/topological_navigation/scripts/topological_visual.py +++ /dev/null @@ -1,252 +0,0 @@ -import rclpy -from rclpy.node import Node -from rclpy.executors import MultiThreadedExecutor -import tf2_ros -import tf2_geometry_msgs -import yaml -import numpy as np -import tf_transformations -from visualization_msgs.msg import Marker, MarkerArray -from rclpy.qos import QoSProfile, DurabilityPolicy -from topological_navigation_msgs.msg import TopologicalOccupiedNode, TopologicalRoute -from geometry_msgs.msg import Point, Pose -from action_msgs.msg import GoalStatusArray - -# =============================================================== -# UTILS -# =============================================================== -def load_waypoints_from_tmap(file_path): - """ - Loads and parses a tmap file, and pre-transforms the local vertices of each - waypoint into the global 'map' frame. - """ - try: - with open(file_path, 'r') as file: - tmap_data = yaml.safe_load(file) - - waypoints_data = {} - for node_entry in tmap_data.get('nodes', []): - try: - node_name = node_entry['node']['name'] - pos = node_entry['node']['pose']['position'] - orient = node_entry['node']['pose']['orientation'] - - # Create the transformation matrix for this waypoint - translation = [pos['x'], pos['y'], pos['z']] - rotation = [orient['x'], orient['y'], orient['z'], orient['w']] - - trans_matrix = tf_transformations.translation_matrix(translation) - rot_matrix = tf_transformations.quaternion_matrix(rotation) - transform_matrix = np.dot(trans_matrix, rot_matrix) - - # Transform local vertices into map frame vertices - local_verts = node_entry['node'].get('verts', []) - map_frame_verts = [] - for vert in local_verts: - # Represent local vertex as a 4D point for matrix multiplication - local_point = np.array([vert['x'], vert['y'], 0, 1]) - map_point = np.dot(transform_matrix, local_point) - # Store the transformed (x, y) coordinates - map_frame_verts.append((map_point[0], map_point[1])) - - # Store the pre-transformed data - waypoints_data[node_name] = { - 'position': (pos['x'], pos['y'], pos['z']), - 'orientation': (orient['x'], orient['y'], orient['z'], orient['w']), - 'verts': map_frame_verts - } - - except KeyError as e: - continue - - return waypoints_data - - except Exception as e: - return {} - -# ============================================================================== -# Route Visualiser Node -# ============================================================================== -class RouteVisualiserNode(Node): - """ - Subscribes to a list of waypoint names and publishes visualization markers. - """ - def __init__(self): - """ Initializes the Route Visualiser Node.""" - super().__init__('route_visualiser') - self.declare_parameter('tmap', rclpy.Parameter.Type.STRING) - self.tmap_path = self.get_parameter('tmap').get_parameter_value().string_value - self.tmap = load_waypoints_from_tmap(self.tmap_path) - - latching_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.routevis_pub = self.create_publisher(MarkerArray, 'topological_route_visualisation', qos_profile=latching_qos) - - self.create_subscription(TopologicalRoute, "topological_navigation/Route", self.route_cb, qos_profile=latching_qos) - self.create_subscription(TopologicalRoute, "topological_navigation/Route", self.route_cb, qos_profile=latching_qos) - # TODO: it would be nice to clear the route when the action completes, but the status topic is unreliable - # self.create_subscription(GoalStatusArray, "/topological_navigation/_action/status", self.status_callback, qos_profile=latching_qos) - # self.create_subscription(GoalStatusArray, "/topological_navigation/execute_policy_mode/_action/status", self.status_callback, qos_profile=latching_qos) - self.get_logger().info('Route Visualiser node has started.') - - - # TODO: it would be nice to clear the route when the action completes, but the status topic is unreliable - # def status_callback(self, msg: GoalStatusArray): - # """ - # Checks the status of all goals and clears the route if any have finished. - # """ - # # Define terminal states: SUCCEEDED, CANCELED, ABORTED - # terminal_states = [4, 5, 6] - # status = msg.status_list[-1] - # if status.status in terminal_states: - # self.get_logger().info("Action finished. Clearing route visualization.") - # self.clear_route() - - - def route_cb(self, msg): - """ Callback for receiving a new route. """ - self.clear_route() # TODO: it would be nice to clear the route when the action completes, but the status topic is unreliable - self.route_marker = MarkerArray() - self.route_marker.markers=[] - idn = 0 - if self.tmap is not None: - for i in range(1,len(msg.nodes)): - marker = self.get_route_marker(msg.nodes[i-1], msg.nodes[i], idn) - self.route_marker.markers.append(marker) - idn+=1 - self.routevis_pub.publish(self.route_marker) - - - def clear_route(self): - """ Clears the current route visualisation. """ - self.route_marker = MarkerArray() - self.route_marker.markers=[] - marker = Marker() - marker.action = marker.DELETEALL - self.route_marker.markers.append(marker) - self.routevis_pub.publish(self.route_marker) - - - def get_route_marker(self, origin, end, idn): - """ Creates a marker between two waypoints. """ - marker = Marker() - marker.id = idn - marker.header.frame_id = 'topo_map' - marker.type = marker.ARROW - origin_node = self.tmap[origin] - end_node = self.tmap[end] - V1 = Point(x=origin_node['position'][0], y=origin_node['position'][1], z=origin_node['position'][2]) - V1.z += 0.25 - V2 = Point(x=end_node['position'][0], y=end_node['position'][1], z=end_node['position'][2]) - V2.z += 0.25 - marker.pose.orientation.w= 1.0 - marker.scale.x = 0.2 - marker.scale.y = 0.2 - marker.scale.z = 0.4 - marker.color.a = 1.0 - marker.color.r = 0.33 - marker.color.g = 0.99 - marker.color.b = 0.55 - marker.points.append(V1) - marker.points.append(V2) - marker.ns='/route_pathg' - return marker - - -# ============================================================================== -# Occupied Visualiser Node -# ============================================================================== -class OccupancyVisualiserNode(Node): - """ - Subscribes to the list of occupied waypoints and publishes visualization markers. - """ - def __init__(self): - """ Initializes the Occupancy Visualiser Node. """ - super().__init__('occupancy_visualiser') - self.declare_parameter('tmap', rclpy.Parameter.Type.STRING) - self.tmap_path = self.get_parameter('tmap').get_parameter_value().string_value - self.tmap = load_waypoints_from_tmap(self.tmap_path) - - self.last_marker_count = 0 - - latching_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.marker_pub = self.create_publisher(MarkerArray, '/topological_navigation/visual/occupied_node', latching_qos) - - self.create_subscription(TopologicalOccupiedNode, '/topological_navigation/occupied_node', self.occupancy_cb, 10) - self.get_logger().info('Waypoint Visualiser component has started.') - - def occupancy_cb(self, msg): - """ Callback for receiving the list of currently occupied waypoints. """ - waypoint_names = msg.nodes if msg.nodes else [] - marker_array = MarkerArray() - - # Create ADD markers for all currently occupied waypoints - for i, wp_name in enumerate(waypoint_names): - wp_data = self.tmap.get(wp_name) - if not wp_data: continue - - marker = Marker() - marker.header.frame_id = "map" - marker.header.stamp = self.get_clock().now().to_msg() - marker.ns = "occupied_waypoints" - marker.id = i - marker.type = Marker.SPHERE - marker.action = Marker.ADD - - pose_coords = wp_data['position'] - marker.pose.position.x = pose_coords[0] - marker.pose.position.y = pose_coords[1] - marker.pose.position.z = 0.15 # Lift it off the ground slightly - marker.pose.orientation.w = 1.0 - - marker.scale.x = 1.0 - marker.scale.y = 1.0 - marker.scale.z = 0.1 - - marker.color.r = 1.0 # Red - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 0.8 # Slightly transparent - - marker_array.markers.append(marker) - - # Create DELETE markers for any old markers that are no longer needed - for i in range(len(waypoint_names), self.last_marker_count): - marker = Marker() - marker.header.frame_id = "map" - marker.ns = "occupied_waypoints" - marker.id = i - marker.action = Marker.DELETE - marker_array.markers.append(marker) - - self.marker_pub.publish(marker_array) - self.last_marker_count = len(waypoint_names) - -# =============================================================== -# The Main Execution Block -# =============================================================== -def main(args=None): - rclpy.init(args=args) - - route_visualiser_node = RouteVisualiserNode() - occupancy_visualiser_node = OccupancyVisualiserNode() - - # Create a MultiThreadedExecutor - executor = MultiThreadedExecutor() - - # Add nodes to the executor - executor.add_node(route_visualiser_node) - executor.add_node(occupancy_visualiser_node) - - try: - # Spin the executor to run both nodes - executor.spin() - except KeyboardInterrupt: - pass - finally: - executor.shutdown() - route_visualiser_node.destroy_node() - occupancy_visualiser_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/topological_navigation/topological_navigation/scripts/travel_time_estimator.py b/topological_navigation/topological_navigation/scripts/travel_time_estimator.py deleted file mode 100755 index 74846f50..00000000 --- a/topological_navigation/topological_navigation/scripts/travel_time_estimator.py +++ /dev/null @@ -1,56 +0,0 @@ -#! /usr/bin/env python - - -import rospy -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation_msgs.srv import EstimateTravelTime -from std_msgs.msg import Duration -import math -import threading - -class TravelTimeEstimator(object): - """docstring for TravelTimeEstimator""" - def __init__(self): - super(TravelTimeEstimator, self).__init__() - self.service_lock = threading.Lock() - self.nodes = dict() - rospy.Subscriber('topological_map', TopologicalMap, self.map_callback) - - while(not rospy.is_shutdown() and len(self.nodes) == 0): - rospy.sleep(1) - - rospy.Service('topological_navigation/travel_time_estimator', EstimateTravelTime, self.estimate_travel_time) - - - def map_callback(self, msg): - self.nodes.clear() - for node in msg.nodes: - self.nodes[node.name] = node - - - def estimate_travel_time(self, req): - self.service_lock.acquire() - - print(req) - - if req.start not in self.nodes: - raise Exception('Unknown start node: %s' % req.start) - if req.target not in self.nodes: - raise Exception('Unknown target node: %s' % req.target) - - startNode = self.nodes[req.start] - endNode = self.nodes[req.target] - - travel_distance = math.hypot((startNode.pose.position.x-endNode.pose.position.x),(startNode.pose.position.y-endNode.pose.position.y)) - - # this is the default scitos speed - travel_speed_ms = 0.5 - travel_time = rospy.Duration(travel_distance/travel_speed_ms) - self.service_lock.release() - return travel_time - -if __name__ == '__main__': - rospy.init_node('travel_time_estimator') - tte = TravelTimeEstimator() - rospy.spin() - diff --git a/topological_navigation/topological_navigation/scripts/visualise_map.py b/topological_navigation/topological_navigation/scripts/visualise_map.py deleted file mode 100755 index 6b230732..00000000 --- a/topological_navigation/topological_navigation/scripts/visualise_map.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy - -import math - - -from time import sleep - -from threading import Timer -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point -import std_msgs.msg - -#from mongodb_store.message_store import MessageStoreProxy - -from interactive_markers.interactive_marker_server import * -#from interactive_markers.menu_handler import * -from visualization_msgs.msg import * - -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation.topological_map import * -from topological_navigation.marker_arrays import * -from topological_navigation.node_controller import * -from topological_navigation.edge_controller import * -from topological_navigation.vertex_controller import * -from topological_navigation.node_manager import * -from topological_navigation.edge_std import * - - - -from topological_navigation.goto import * - - -from topological_navigation_msgs.msg import NavRoute -from topological_navigation_msgs.msg import TopologicalMap -import topological_navigation.policies -import topological_navigation.map_marker - - -class VisualiseMap(object): - _killall_timers=False - - def __init__(self, name, edit_mode, noedit_mode) : - rospy.on_shutdown(self._on_node_shutdown) - - self.update_needed=False - self.in_feedback=False - #self._point_set=filename - self._edit_mode = edit_mode - self._noedit_mode = noedit_mode - - self.map_markers = topological_navigation.map_marker.TopologicalVis() - self.pol_markers = topological_navigation.policies.PoliciesVis() - - if not noedit_mode: - rospy.loginfo("Edge Controllers ...") - self.edge_cont = edge_controllers() - rospy.loginfo("Vertex Controllers ...") - self.vert_cont = VertexControllers() - rospy.loginfo("Waypoint Controllers ...") - self.node_cont = WaypointControllers() - rospy.loginfo("Node Manager Controllers ...") - self.add_rm_node = node_manager() - else: - rospy.logwarn("No edit Visualisation mode ...") - - if not self._edit_mode : - rospy.loginfo("Go To Controllers ...") - self.goto_cont = go_to_controllers() - else: - rospy.logwarn("Edit only Visualisation mode ...") - - rospy.loginfo("Done ...") - - - rospy.loginfo("All Done ...") - - - def _on_node_shutdown(self): - print("bye") - - - -if __name__ == '__main__': - edit_mode=False - noedit_mode=False - #mapname=str(sys.argv[1]) - argc = len(sys.argv) - print(argc) - if argc > 1: - if '-edit' in sys.argv or '-e' in sys.argv : - edit_mode = True - if '-noedit' in sys.argv or '-n' in sys.argv : - noedit_mode=True - rospy.init_node('topological_visualisation') - server = VisualiseMap(rospy.get_name(), edit_mode, noedit_mode) - rospy.spin() diff --git a/topological_navigation/topological_navigation/scripts/visualise_map2.py b/topological_navigation/topological_navigation/scripts/visualise_map2.py deleted file mode 100755 index edbcd9af..00000000 --- a/topological_navigation/topological_navigation/scripts/visualise_map2.py +++ /dev/null @@ -1,370 +0,0 @@ -#!/usr/bin/env python - -import rospy -import json -import sys -import math -import actionlib - -from time import sleep - -from threading import Timer -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point -from std_msgs.msg import String - -from visualization_msgs.msg import * - -from interactive_markers.interactive_marker_server import * - -from topological_navigation_msgs.msg import GotoNodeGoal, GotoNodeAction, TopologicalRoute - -def get_node(nodes_list, name): - for i in nodes_list: - if i['node']['name'] == name: - return i['node'] - - -class TopoMap2Vis(object): - _pallete=[[0,0,0],[1,0,0],[0,0,1],[0,1,0],[1,1,0],[1,0,1],[0,1,1],[1,1,1]] - def __init__(self, name, no_goto=False) : - """ - """ - self.no_goto = no_goto - self.killall=False - self.topological_map = None - self.map_markers = MarkerArray() - self.map_markers.markers=[] - self.in_feedback=False - - rospy.on_shutdown(self._on_node_shutdown) - - self.topmap_pub = rospy.Publisher('topological_map_visualisation', MarkerArray, queue_size = 1, latch=True) - self.routevis_pub = rospy.Publisher('topological_route_visualisation', MarkerArray, queue_size = 1) - self.topo_map_sub = rospy.Subscriber("topological_map_2", String, self.topo_map_cb) - self.topo_route_sub = rospy.Subscriber("topological_navigation/Route", TopologicalRoute, self.route_cb) - - rospy.loginfo("Waiting for topo_map") - - if not no_goto: - self._goto_server = InteractiveMarkerServer("go_to_node") - self.client = actionlib.SimpleActionClient('topological_navigation', GotoNodeAction) - self.client.wait_for_server() - - rospy.loginfo("All Done ...") - rospy.spin() - - def topo_map_cb(self, msg): - """ - """ - self.topological_map = json.loads(msg.data) - rospy.loginfo("Received new topo_map") - print(self.topological_map['name']) - self.create_map_marker() - - - def route_cb(self, msg): - self.clear_route() # clear the last route - self.logger() - self.route_marker = MarkerArray() - self.route_marker.markers=[] - idn=0 - for i in range(1,len(msg.nodes)): - self.route_marker.markers.append(self.get_route_marker(msg.nodes[i-1], msg.nodes[i], idn)) - idn+=1 - self.routevis_pub.publish(self.route_marker) - - - def clear_route(self): - self.route_marker = MarkerArray() - self.route_marker.markers=[] - marker = Marker() - marker.action = marker.DELETEALL - self.route_marker.markers.append(marker) - self.routevis_pub.publish(self.route_marker) - rospy.sleep(0.5) - - - def get_route_marker(self, origin, end, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = 'topo_map' - marker.type = marker.ARROW - V1=Point() - V2=Point() - origin_node = get_node(self.topological_map['nodes'], origin) - end_node = get_node(self.topological_map['nodes'], end) - V1=self.node2pose(origin_node['pose']).position - V1.z += 0.25 - V2=self.node2pose(end_node['pose']).position - V2.z += 0.25 - marker.pose.orientation.w= 1.0 - marker.scale.x = 0.2 - marker.scale.y = 0.2 - marker.scale.z = 0.4 - marker.color.a = 0.5 - marker.color.r = 0.33 - marker.color.g = 0.99 - marker.color.b = 0.55 - marker.points.append(V1) - marker.points.append(V2) - marker.lifetime = rospy.Duration(1800) - marker.ns='/route' - return marker - - - def create_map_marker(self): - """ - """ - del self.map_markers - self.map_markers = MarkerArray() - self.map_markers.markers=[] - self.actions=[] - idn=0 - for i in self.topological_map['nodes']: - self.map_markers.markers.append(self.get_node_marker(i['node'], idn)) - idn+=1 - self.map_markers.markers.append(self.get_name_marker(i['node'], idn)) - idn+=1 - self.map_markers.markers.append(self.get_zone_marker(i['node'], idn)) - idn+=1 - for j in i['node']['edges']: - self.update_actions(j['action']) - marker = self.get_edge_marker(i['node'], j, idn) - if marker: - self.map_markers.markers.append(marker) - idn += 1 - - if not self.no_goto: - for i in self.topological_map['nodes']: - self.create_goto_marker(i['node']) - - legend =0 - for k in self.actions: - marker = self.get_legend_marker(k, legend, idn) - self.map_markers.markers.append(marker) - idn += 1 - legend+=1 - self.topmap_pub.publish(self.map_markers) - - - def create_goto_marker(self, node): - """ - """ - marker = InteractiveMarker() - marker.header.frame_id = node['parent_frame'] - marker.scale = 1 - marker.name = node['name'] - marker.description = ''#node['name'] - - control = InteractiveMarkerControl() - control.interaction_mode = InteractiveMarkerControl.BUTTON - control.always_visible = True - - control.markers.append(self.makeArrow( marker.scale )) - marker.controls.append(control) - - self._goto_server.insert(marker, self.goto_feedback_cb) - self._goto_server.applyChanges() - - pos = self.node2pose(node['pose']) - - if pos is not None: - pos.position.z=pos.position.z+0.5 - self._goto_server.setPose( marker.name, pos ) - self._goto_server.applyChanges() - - - def makeArrow(self, scale): - marker = Marker() - - marker.type = Marker.ARROW - marker.scale.x = scale * 0.5 - marker.scale.y = scale * 0.25 - marker.scale.z = scale * 0.15 - marker.lifetime.secs = 3 - marker.color.r = 0.2 - marker.color.g = 0.9 - marker.color.b = 0.2 - marker.color.a = 1.0 - return marker - - - def goto_feedback_cb(self, feedback): - if not self.in_feedback: - self.in_feedback=True - print('GOTO: '+str(feedback.marker_name)) - self.client.cancel_all_goals() - navgoal = GotoNodeGoal() - navgoal.target = feedback.marker_name - #navgoal.origin = orig - # Sends the goal to the action server. - self.client.send_goal(navgoal) - rospy.Timer(rospy.Duration(1.0), self.timer_cb, oneshot=True) # This is needed so the callback is only triggered once - - - def timer_cb(self, event) : - self.in_feedback = False - - - def get_colour(self, number): - """ - """ - return self._pallete[number] - - def update_actions(self, action): - """ - """ - if action not in self.actions: - self.actions.append(action) - - - def get_legend_marker(self, action, row, idn): - """ - """ - col=self.get_colour(self.actions.index(action)) - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = marker.TEXT_VIEW_FACING - marker.text=action - marker.pose.position.x= 1.0 - marker.pose.position.y= 0.0 + (0.18*row) - marker.pose.position.z= 0.2 - marker.pose.orientation.w= 1.0 - marker.scale.z = 0.15 - marker.color.a = 1.0 - marker.color.r = col[0] - marker.color.g = col[1] - marker.color.b = col[2] - marker.ns='/legend' - return marker - - - def get_edge_marker(self, node, edge, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = node['parent_frame'] - marker.type = marker.LINE_LIST - V1=Point() - V2=Point() - V1=self.node2pose(node['pose']).position - V1.z += 0.1 - to_node=get_node(self.topological_map['nodes'], edge['node']) - col=self.get_colour(self.actions.index(edge['action'])) - #print col - if to_node: - V2= self.node2pose(to_node['pose']).position - V2.z += 0.1 - marker.pose.orientation.w= 1.0 - marker.scale.x = 0.1 - marker.color.a = 0.5 - marker.color.r = col[0] - marker.color.g = col[1] - marker.color.b = col[2] - marker.points.append(V1) - marker.points.append(V2) - marker.ns='/edges' - return marker - else: - rospy.logwarn("No node %s found" %edge.node) - return None - - - def get_node_marker(self, node, idn): - """ - """ - marker = Marker() - marker.id = idn - marker.header.frame_id = node['parent_frame'] - marker.type = Marker.SPHERE - marker.scale.x = 0.2 - marker.scale.y = 0.2 - marker.scale.z = 0.2 - marker.color.a = 0.6 - marker.color.r = 0.2 - marker.color.g = 0.2 - marker.color.b = 0.7 - marker.pose = self.node2pose(node['pose']) - marker.pose.position.z += 0.1 - marker.ns='/nodes' - return marker - - - def get_name_marker(self, node, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = node['parent_frame'] - marker.type = marker.TEXT_VIEW_FACING - marker.text= node['name'] - marker.pose = self.node2pose(node['pose']) - marker.pose.position.z += 0.2 - #marker.pose.position.y += 0.2 - marker.scale.z = 0.20 - marker.color.a = 1.00 - marker.color.r = 1.00 - marker.color.g = 1.00 - marker.color.b = 1.00 - marker.ns='/names' - return marker - - - def node2pose(self, pose): - node_pose=Pose() - node_pose.position.x = pose['position']['x'] - node_pose.position.y = pose['position']['y'] - node_pose.position.z = pose['position']['z'] - node_pose.orientation.w = pose['orientation']['w'] - node_pose.orientation.x = pose['orientation']['x'] - node_pose.orientation.y = pose['orientation']['y'] - node_pose.orientation.z = pose['orientation']['z'] - return node_pose - - - def get_zone_marker(self, node, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = marker.LINE_STRIP - marker.pose.orientation.w= 1.0 - marker.scale.x = 0.1 - marker.color.a = 0.8 - marker.color.r = 0.7 - marker.color.g = 0.1 - marker.color.b = 0.2 - - for j in node['verts'] : - Vert = Point() - Vert.z = 0.05 - Vert.x = node['pose']['position']['x'] + j['x'] - Vert.y = node['pose']['position']['y'] + j['y'] - marker.points.append(Vert) - - Vert = Point() - Vert.z = 0.05 - Vert.x = node['pose']['position']['x'] + node['verts'][0]['x'] - Vert.y = node['pose']['position']['y'] + node['verts'][0]['y'] - marker.points.append(Vert) - marker.ns='/zones' - return marker - - - def _on_node_shutdown(self): - """ - """ - self.clear_route() - self.killall=True - rospy.loginfo("See you later") - #sleep(2) - - -if __name__ == '__main__': - nogoto_mode=False - argc = len(sys.argv) - print(argc) - if argc > 1: - if '-no_goto' in sys.argv or '-n' in sys.argv : - nogoto_mode = True - - rospy.init_node('topomap2_visualisation') - server = TopoMap2Vis(rospy.get_name(), no_goto=nogoto_mode) diff --git a/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py b/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py deleted file mode 100755 index e719ad6a..00000000 --- a/topological_navigation/topological_navigation/scripts/visualise_map_ros2.py +++ /dev/null @@ -1,564 +0,0 @@ -#!/usr/bin/env python - -import rclpy -import json -import sys -import math -from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor -from time import sleep -from threading import Timer -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point -from std_msgs.msg import String -from action_msgs.msg import GoalStatus -from topological_navigation_msgs.action import GotoNode, ExecutePolicyMode -from topological_navigation_msgs.action import ExecutePolicyMode -from rclpy.action import ActionClient -from geometry_msgs.msg import PoseStamped -from visualization_msgs.msg import * -from interactive_markers.interactive_marker_server import * -from topological_navigation_msgs.msg import TopologicalRoute -from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy, QoSDurabilityPolicy -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup -from builtin_interfaces.msg import Duration -from rclpy.task import Future -import threading -import yaml -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy - -# this ensures that all the poses and translates -# are float-type and not int-type as there is an -# assertion in ros2 messages (vector3, pose etc.) -# for float-type [x,y,z,w] keys. -class CustomSafeLoader(yaml.SafeLoader): - def construct_mapping(self, node, deep=False): - mapping = super().construct_mapping(node, deep=deep) - for key in ['x', 'y', 'z', 'w']: - if key in mapping and isinstance(mapping[key], int): - mapping[key] = float(mapping[key]) - - return mapping - -def get_node(nodes_list, name): - for i in nodes_list: - if i['node']['name'] == name: - return i['node'] - -class InteractiveMarkerServerHandlerNode(rclpy.node.Node): - def __init__(self, name): - super().__init__(name) - self._goto_server = InteractiveMarkerServer(self, "go_to_node") - - def getInteractiveMarkerServer(self,): - return self._goto_server - - -class TopoMap2Vis(rclpy.node.Node): - _pallete=[[0.0,0.0,0.0],[1.0,0.0,0.0],[0.0,0.0,1.0],[0.0,1.0,0.0],[1.0,1.0,0.0],[1.0,0.0,1.0],[0.0,1.0,1.0],[1.0,1.0,1.0]] - def __init__(self, name, interactive_marker_server) : - super().__init__(name) - self.declare_parameter('no_goto', rclpy.Parameter.Type.BOOL) - self.declare_parameter('publish_map', rclpy.Parameter.Type.BOOL) - - self.no_go = self.get_parameter_or("no_goto", rclpy.Parameter('bool', rclpy.Parameter.Type.BOOL, False)).value - self.publish_map = self.get_parameter_or("publish_map", rclpy.Parameter('bool', rclpy.Parameter.Type.BOOL, True)).value - self.no_goto = self.no_go - self.killall=False - self._goto_server = interactive_marker_server.getInteractiveMarkerServer() - self.topological_map = None - self.map_markers = MarkerArray() - self.map_markers.markers=[] - self.in_feedback=False - self._map_received = False - self.early_terminate_is_required = False - self.action_status = 0 - self.status_mapping = {} - self.status_mapping[0] = "STATUS_UNKNOWN" - self.status_mapping[1] = "STATUS_ACCEPTED" - self.status_mapping[2] = "STATUS_EXECUTING" - self.status_mapping[3] = "STATUS_CANCELING" - self.status_mapping[4] = "STATUS_SUCCEEDED" - self.status_mapping[5] = "STATUS_CANCELED" - self.status_mapping[6] = "STATUS_ABORTED" - self.goal_cancel_error_codes = {} - self.goal_cancel_error_codes[0] = "ERROR_NONE" - self.goal_cancel_error_codes[1] = "ERROR_REJECTED" - self.goal_cancel_error_codes[2] = "ERROR_UNKNOWN_GOAL_ID" - self.goal_cancel_error_codes[3] = "ERROR_GOAL_TERMINATED" - - rclpy.get_default_context().on_shutdown(self._on_node_shutdown) - - self.latching_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.goal_handle = None - self.go_to_node_exe_task = False - self.timer_set_mission = False - self.callback_group_map = ReentrantCallbackGroup() - self.callback_goto_client = ReentrantCallbackGroup() - self.callback_goto_subs = ReentrantCallbackGroup() - self.executor_goto_client = SingleThreadedExecutor() - # self.executor_goto_client_check = SingleThreadedExecutor() - self.goto_node_executor = None - - self.topmap_pub = self.create_publisher(MarkerArray, 'topological_map_visualisation', qos_profile=self.latching_qos) - self.routevis_pub = self.create_publisher(MarkerArray, 'topological_route_visualisation', qos_profile=self.latching_qos) - self.future_task = None - self.current_target_node = None - if not self.no_goto: - self.action_server_name = '/topological_navigation' - - self.client = ActionClient(self, GotoNode, self.action_server_name, callback_group=self.callback_goto_client) - while rclpy.ok(): - try: - rclpy.spin_once(self, timeout_sec=0.1) - if not self.client.server_is_ready(): - self.get_logger().info("Waiting for the action server {}...".format(self.action_server_name)) - self.client.wait_for_server(timeout_sec=1) - if self.client.server_is_ready(): - self.get_logger().info("the action server {} is avaible ".format(self.action_server_name)) - break - except Exception as e: - self.get_logger().error(" {} ".format(e)) - pass - - self.topo_map_sub = self.create_subscription(String, "/topological_map_2", self.topo_map_cb, qos_profile=self.latching_qos, callback_group=self.callback_group_map) - - while rclpy.ok(): - rclpy.spin_once(self, timeout_sec=0.1) - if self._map_received: - self.get_logger().info(" received the Topological Map") - if self.topological_map is not None: - self.get_logger().info("Start generating map...") - self.create_map_marker() - self.get_logger().info("End generating map...") - break - - # self.topo_route_sub = self.create_subscription(TopologicalRoute, "topological_navigation/Route", self.route_cb, qos_profile=self.latching_qos, callback_group=self.callback_goto_subs) - self.get_logger().info("All Done ...") - - def topo_map_cb(self, msg): - self.topological_map = yaml.load(msg.data, Loader = CustomSafeLoader) - self.get_logger().info("{}".format(self.topological_map['name'])) - self._map_received = True - self.create_map_marker() - - # def route_cb(self, msg): - # self.get_logger().error("[route_cb] - NEW ROUTE RECEIVED") - # self.clear_route() # clear the last route - # self.route_marker = MarkerArray() - # self.route_marker.markers=[] - # idn = 0 - # if self.topological_map is not None: - # for i in range(1,len(msg.nodes)): - # marker = self.get_route_marker(msg.nodes[i-1], msg.nodes[i], idn) - # self.route_marker.markers.append(marker) - # idn+=1 - # self.routevis_pub.publish(self.route_marker) - # self.get_logger().error("[route_cb] - ✅ DONE!") - - # def clear_route(self): - # self.route_marker = MarkerArray() - # self.route_marker.markers=[] - # marker = Marker() - # marker.action = marker.DELETEALL - # self.route_marker.markers.append(marker) - # self.routevis_pub.publish(self.route_marker) - - # def get_route_marker(self, origin, end, idn): - # marker = Marker() - # marker.id = idn - # marker.header.frame_id = 'topo_map' - # marker.type = marker.ARROW - # V1=Point() - # V2=Point() - # origin_node = get_node(self.topological_map['nodes'], origin) - # end_node = get_node(self.topological_map['nodes'], end) - # V1=self.node2pose(origin_node['pose']).position - # V1.z += 0.25 - # V2=self.node2pose(end_node['pose']).position - # V2.z += 0.25 - # marker.pose.orientation.w= 1.0 - # marker.scale.x = 0.2 - # marker.scale.y = 0.2 - # marker.scale.z = 0.4 - # marker.color.a = 1.0 - # # marker.color.a = 0.5 - # marker.color.r = 0.33 - # marker.color.g = 0.99 - # marker.color.b = 0.55 - # marker.points.append(V1) - # marker.points.append(V2) - # # marker.lifetime.sec = 2 - # marker.ns='/route_pathg' - # return marker - - - def create_map_marker(self): - del self.map_markers - self.map_markers = MarkerArray() - self.map_markers.markers=[] - self.actions=[] - idn=0 - for i in self.topological_map['nodes']: - self.map_markers.markers.append(self.get_node_marker(i['node'], idn)) - idn+=1 - self.map_markers.markers.append(self.get_name_marker(i['node'], idn)) - idn+=1 - self.map_markers.markers.append(self.get_zone_marker(i['node'], idn)) - idn+=1 - for j in i['node']['edges']: - self.update_actions(j['action']) - marker = self.get_edge_marker(i['node'], j, idn) - if marker: - self.map_markers.markers.append(marker) - idn += 1 - - if not self.no_goto: - for i in self.topological_map['nodes']: - self.create_goto_marker(i['node']) - - legend =0 - for k in self.actions: - marker = self.get_legend_marker(k, legend, idn) - self.map_markers.markers.append(marker) - idn += 1 - legend+=1 - if self.publish_map: - self.topmap_pub.publish(self.map_markers) - - - def create_goto_marker(self, node): - """ - """ - marker = InteractiveMarker() - marker.header.frame_id = node['parent_frame'] - marker.scale = 1.0 - marker.name = node['name'] - marker.description = ''#node['name'] - - control = InteractiveMarkerControl() - control.interaction_mode = InteractiveMarkerControl.BUTTON - control.always_visible = True - - control.markers.append(self.makeArrow( marker.scale )) - marker.controls.append(control) - - self._goto_server.insert(marker, feedback_callback=self.goto_feedback_cb) - self._goto_server.applyChanges() - - pos = self.node2pose(node['pose']) - - if pos is not None: - pos.position.z=pos.position.z+0.5 - self._goto_server.setPose( marker.name, pos ) - self._goto_server.applyChanges() - - - def makeArrow(self, scale): - marker = Marker() - marker.type = Marker.ARROW - marker.scale.x = scale * 0.5 - marker.scale.y = scale * 0.25 - marker.scale.z = scale * 0.15 - duration_msg = Duration() - duration_msg.sec = 3 - duration_msg.nanosec = 0 - marker.lifetime = duration_msg - marker.color.r = 0.2 - marker.color.g = 0.9 - marker.color.b = 0.2 - marker.color.a = 1.0 - return marker - - def preempt_action(self): - if self.client is not None: - if not self.client.server_is_ready(): - self.get_logger().info("Waiting for the action server {}...".format(self.action_server_name)) - self.client.wait_for_server(timeout_sec=2) - - if not self.client.server_is_ready(): - self.get_logger().info("action server {} not responding ... can not perform any action".format(self.action_server_name)) - return True - - if self.goal_handle is None: - self.get_logger().info("there is no goal to stop it is already cancelled with status {}".format(self.action_status)) - return True - - cancel_future = self.goal_handle.cancel_goal_async() - self.get_logger().info("Waiting till terminating the current preemption") - while rclpy.ok(): - try: - rclpy.spin_once(self, executor=self.executor_goto_client) - # rclpy.spin_until_future_complete(self, cancel_future, executor=self.executor_goto_client, timeout_sec=2.0) - if cancel_future.done() and self.goal_get_result_future.done(): - self.action_status = self.goal_get_result_future.result().status - self.get_logger().info("The goal cancel error code {} ".format(self.get_goal_cancel_error_msg(cancel_future.result().return_code))) - return True - except Exception as e: - # self.goal_handle = None - self.get_logger().error("Edge Action Manager: error while canceling the previous action") - return False - - def get_goal_cancel_error_msg(self, status_code): - try: - return self.goal_cancel_error_codes[status_code] - except Exception as e: - self.get_logger().error("Goal cancel code {}".format(status_code)) - return self.goal_cancel_error_codes[0] - - def go_to_node_task(self, ): - self.preempt_action() - self.execute() - self.in_feedback = False - self.early_terminate_is_required = False - - - def goto_feedback_cb(self, feedback): - if(self.current_target_node is not None): - if(self.current_target_node != feedback.marker_name): - self.in_feedback = False - self.early_terminate_is_required = True - else: - self.in_feedback = True - - self.get_logger().warning('GOTO: is early termination required ? ' + str(self.early_terminate_is_required) + ' ') - self.get_logger().warning('GOTO: self.goto_node_executor {} self.in_feedback {} self.early_terminate_is_required {}'.format(self.goto_node_executor, self.in_feedback, self.early_terminate_is_required)) - if ((self.goto_node_executor is not None) and self.goto_node_executor.is_alive()) and self.in_feedback: - self.get_logger().warning('GOTO: '+str(feedback.marker_name) + ' is not enable yet, waiting till terminating previous action') - return - else: - self.in_feedback = True - self.current_target_node = feedback.marker_name - self.get_logger().info('GOTO: '+str(feedback.marker_name)) - navgoal = GotoNode.Goal() - navgoal.target = feedback.marker_name - navgoal.no_orientation = True - self.goal = navgoal - self.go_to_node_exe_task = True - self.goto_node_executor = threading.Thread(target=self.go_to_node_task) - self.goto_node_executor.start() - return - - - def feedback_callback(self, feedback_msg): - self.nav_client_feedback = feedback_msg.feedback - self.get_logger().info("feedback: {} ".format(self.nav_client_feedback)) - return - - def get_status_msg(self, status_code): - try: - return self.status_mapping[status_code] - except Exception as e: - self.get_logger().error("Status code is invalid {}".format(status_code)) - return self.status_mapping[0] - - def execute(self): - if not self.client.server_is_ready(): - self.get_logger().info("Waiting for the action server {}...".format(self.action_server_name)) - self.client.wait_for_server(timeout_sec=2) - - if not self.client.server_is_ready(): - self.get_logger().info("action server {} not responding ... can not perform any action".format(self.action_server_name)) - return - - self.get_logger().info("Executing the action...") - send_goal_future = self.client.send_goal_async(self.goal, feedback_callback=self.feedback_callback) - while rclpy.ok(): - try: - rclpy.spin_once(self, executor=self.executor_goto_client) - # rclpy.spin_until_future_complete(self, send_goal_future, executor=self.executor_goto_client, timeout_sec=2.0) - if send_goal_future.done(): - self.goal_handle = send_goal_future.result() - break - except Exception as e: - self.get_logger().error("Error while sending the goal to GOTO node {} ".format(e)) - return False - - if not self.goal_handle.accepted: - self.get_logger().error('GOTO action is rejected') - return False - - self.get_logger().info('The goal accepted') - self.goal_get_result_future = self.goal_handle.get_result_async() - self.get_logger().info("Waiting for {} action to complete".format(self.action_server_name)) - while rclpy.ok(): - try: - rclpy.spin_once(self, timeout_sec=0.2) - if(self.early_terminate_is_required): - self.get_logger().warning("Not going to wait till finishing ongoing task, early termination is required ") - return False - if self.goal_get_result_future.done(): - status = self.goal_get_result_future.result().status - self.action_status = status - self.get_logger().info("Executing the action response with status {}".format(self.get_status_msg(self.action_status))) - return True - except Exception as e: - self.get_logger().error("Error while executing go to node policy {} ".format(e)) - # self.goal_get_result_future = None - return False - - def get_colour(self, number): - """ - """ - return self._pallete[number] - - def update_actions(self, action): - """ - """ - if action not in self.actions: - self.actions.append(action) - - - def get_legend_marker(self, action, row, idn): - """ - """ - col=self.get_colour(self.actions.index(action)) - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = marker.TEXT_VIEW_FACING - marker.text=action - marker.pose.position.x= 1.0 - marker.pose.position.y= 0.0 + (0.18*row) - marker.pose.position.z= 0.2 - marker.pose.orientation.w= 1.0 - marker.scale.z = 0.15 - marker.color.a = 1.0 - marker.color.r = col[0] - marker.color.g = col[1] - marker.color.b = col[2] - marker.ns='/legend' - return marker - - - def get_edge_marker(self, node, edge, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = node['parent_frame'] - marker.type = marker.LINE_LIST - V1=Point() - V2=Point() - V1=self.node2pose(node['pose']).position - V1.z += 0.1 - to_node=get_node(self.topological_map['nodes'], edge['node']) - col=self.get_colour(self.actions.index(edge['action'])) - if to_node: - V2= self.node2pose(to_node['pose']).position - V2.z += 0.1 - marker.pose.orientation.w= 1.0 - marker.scale.x = 0.1 - marker.color.a = 0.5 - marker.color.r = col[0] - marker.color.g = col[1] - marker.color.b = col[2] - marker.points.append(V1) - marker.points.append(V2) - marker.ns='/edges' - return marker - else: - self.get_logger().warning("No node %s found" %edge.node) - return None - - - def get_node_marker(self, node, idn): - """ - """ - marker = Marker() - marker.id = idn - marker.header.frame_id = node['parent_frame'] - marker.type = Marker.SPHERE - marker.scale.x = 0.2 - marker.scale.y = 0.2 - marker.scale.z = 0.2 - marker.color.a = 0.6 - marker.color.r = 0.2 - marker.color.g = 0.2 - marker.color.b = 0.7 - marker.pose = self.node2pose(node['pose']) - marker.pose.position.z += 0.1 - marker.ns='/nodes' - return marker - - - def get_name_marker(self, node, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = node['parent_frame'] - marker.type = marker.TEXT_VIEW_FACING - marker.text= node['name'] - marker.pose = self.node2pose(node['pose']) - marker.pose.position.z += 0.2 - #marker.pose.position.y += 0.2 - marker.scale.z = 0.20 - marker.color.a = 1.00 - marker.color.r = 1.00 - marker.color.g = 1.00 - marker.color.b = 1.00 - marker.ns='/names' - return marker - - - def node2pose(self, pose): - node_pose=Pose() - node_pose.position.x = pose['position']['x'] - node_pose.position.y = pose['position']['y'] - node_pose.position.z = pose['position']['z'] - node_pose.orientation.w = pose['orientation']['w'] - node_pose.orientation.x = pose['orientation']['x'] - node_pose.orientation.y = pose['orientation']['y'] - node_pose.orientation.z = pose['orientation']['z'] - return node_pose - - - def get_zone_marker(self, node, idn): - marker = Marker() - marker.id = idn - marker.header.frame_id = "map" - marker.type = marker.LINE_STRIP - marker.pose.orientation.w= 1.0 - marker.scale.x = 0.1 - marker.color.a = 0.8 - marker.color.r = 0.7 - marker.color.g = 0.1 - marker.color.b = 0.2 - - for j in node['verts'] : - Vert = Point() - Vert.z = 0.05 - Vert.x = node['pose']['position']['x'] + j['x'] - Vert.y = node['pose']['position']['y'] + j['y'] - marker.points.append(Vert) - - Vert = Point() - Vert.z = 0.05 - Vert.x = node['pose']['position']['x'] + node['verts'][0]['x'] - Vert.y = node['pose']['position']['y'] + node['verts'][0]['y'] - marker.points.append(Vert) - marker.ns='/zones' - return marker - - def _on_node_shutdown(self): - """ - """ - # self.clear_route() - self.killall=True - self.get_logger().info("See you later") - -def main(): - rclpy.init(args=None) - inter_marker_server = InteractiveMarkerServerHandlerNode("topomap2_interactive_marker_server") - vis_manager = TopoMap2Vis('topomap2_visualisation', inter_marker_server) - executor = MultiThreadedExecutor() - executor.add_node(vis_manager) - executor.add_node(inter_marker_server) - - try: - executor.spin() - except KeyboardInterrupt: - vis_manager.get_logger().info('Shutting down topomap2_visualisation node\n') - inter_marker_server.get_logger().info('Shutting down topomap2_interactive_marker_server node\n') - vis_manager.destroy_node() - inter_marker_server.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__' : - main() \ No newline at end of file diff --git a/topological_navigation/topological_navigation/testing.py b/topological_navigation/topological_navigation/testing.py deleted file mode 100644 index 13790baf..00000000 --- a/topological_navigation/topological_navigation/testing.py +++ /dev/null @@ -1,118 +0,0 @@ -#!/usr/bin/env python -import math -import rospy -import sys - -import std_msgs.msg -from topological_navigation.topological_node import * -from topological_navigation_msgs.msg import TopologicalNode, TopologicalMap, Edge -from geometry_msgs.msg import Pose, Point - - -def create_cross_map(width, height, nodeSeparation): - """ Creates a vertical cross with 5 nodes in each line, with the middle at 0,0 """ - xOrigin = 0 - yOrigin = 0 - nodeSeparation = 10 - - nodes = dict() - - originNode = TopologicalNode(name='ChargingPoint', pose=Pose(position=Point(xOrigin, yOrigin, 0))) - nodes['ChargingPoint'] = originNode - - - - # horizontal nodes - prevNodeName = '' - for x in range(-(width/2),(width/2)+1): - if x != 0: - nodeName = 'h_%s'%x - node = TopologicalNode(name=nodeName, pose=Pose(position=Point(x * nodeSeparation, yOrigin, 0))) - nodes[nodeName] = node - else: - nodeName = 'ChargingPoint' - node = originNode - - if prevNodeName: - # connect up this to previous - e = Edge() - e.node = prevNodeName - e.action = 'move_base' - node.edges.append(e) - # previous to this - node = nodes[prevNodeName] - e = Edge() - e.node = nodeName - e.action = 'move_base' - node.edges.append(e) - - prevNodeName = nodeName - - - # vertical nodes - prevNodeName = '' - for y in range(-(height/2),(height/2)+1): - if y != 0: - nodeName = 'v_%s'%y - node = TopologicalNode(name=nodeName, pose=Pose(position=Point(xOrigin, y * nodeSeparation, 0))) - nodes[nodeName] = node - else: - nodeName = 'ChargingPoint' - node = originNode - - if prevNodeName: - # connect up this to previous - e = Edge() - e.node = prevNodeName - e.action = 'move_base' - node.edges.append(e) - # previous to this - node = nodes[prevNodeName] - e = Edge() - e.node = nodeName - e.action = 'move_base' - node.edges.append(e) - - - prevNodeName = nodeName - - return nodes - - -def create_line_map(width, nodeSeparation): - """ Creates a line map of ChargingPoint, Station, then width - 2 waypoints """ - xOrigin = 0 - yOrigin = 0 - nodeSeparation = 10 - - nodes = [] - - nodes.append(TopologicalNode(name='ChargingPoint', pose=Pose(position=Point(xOrigin, yOrigin, 0)))) - nodes.append(TopologicalNode(name='Station', pose=Pose(position=Point(xOrigin + nodeSeparation, yOrigin, 0)))) - - for i in range(width - 2): - nodes.append(TopologicalNode(name='WayPoint%s'%i, pose=Pose(position=Point(xOrigin + nodeSeparation * i, yOrigin, 0)))) - - for i in range(len(nodes) - 1): - n1 = nodes[i] - n2 = nodes[i+1] - - n1n2 = Edge() - n1n2.node = n2.name - n1n2.action = 'move_base' - n1n2.edge_id = '%s_%s' % (n1.name, n2.name) - n1.edges.append(n1n2) - - n2n1 = Edge() - n2n1.node = n1.name - n2n1.action = 'move_base' - n2n1.edge_id = '%s_%s' % (n2.name, n1.name) - n2.edges.append(n2n1) - - - nodes = {node.name: node for node in nodes} - - # print nodes['Station'] - - return nodes - diff --git a/topological_navigation/topological_navigation/tmap_utils.py b/topological_navigation/topological_navigation/tmap_utils.py index 62fbc382..6eb553a8 100644 --- a/topological_navigation/topological_navigation/tmap_utils.py +++ b/topological_navigation/topological_navigation/tmap_utils.py @@ -1,155 +1,60 @@ #!/usr/bin/env python3 -import math +"""Topological map utility functions. +Provides helper functions for YAML loading and node/edge access +in the tmap2 format. +""" +import yaml -def get_node(top_map, node_name): - """ - Given a topological map and a node name it returns the node object - """ - for i in top_map.nodes: - if i.name == node_name: - return i - return None - - -def get_node_from_tmap2(top_map, node_name): - """ - Given a topological map 2 and a node name it returns the node object - """ - for i in top_map["nodes"]: - if i["node"]["name"] == node_name: - return i - return None +# ===== YAML Loader ===== -def get_distance(pose1, pose2): - """ - Returns the straight line distance between two poses - """ - return math.hypot((pose1.position.x - pose2.position.x), (pose1.position.y - pose2.position.y)) +class CustomSafeLoader(yaml.SafeLoader): + """Custom YAML loader that ensures poses and translations are float-type. - -def get_distance_node_pose(node, pose): - """ - Returns the straight line distance between a pose and a node + ROS 2 messages (Vector3, Pose, etc.) have assertions for float-type + [x, y, z, w] keys. """ - return math.hypot((pose.position.x - node.pose.position.x), (pose.position.y - node.pose.position.y)) + def construct_mapping(self, node, deep=False): + """Construct mapping, converting int to float for pose keys.""" + mapping = super().construct_mapping(node, deep=deep) -def get_distance_node_pose_from_tmap2(node, pose): - dist = math.hypot((pose.position.x - node["node"]["pose"]["position"]["x"]), - (pose.position.y - node["node"]["pose"]["position"]["y"])) - return dist + # Convert int to float for pose/vector keys + for key in ['x', 'y', 'z', 'w', 'yaw_goal_tolerance', 'xy_goal_tolerance']: + if key in mapping and isinstance(mapping[key], int): + mapping[key] = float(mapping[key]) + return mapping -def get_distance_to_node(nodea, nodeb): - """ - Given two nodes it returns the straight line distance between them - """ - dist = math.hypot((nodeb.pose.position.x - nodea.pose.position.x), - (nodeb.pose.position.y - nodea.pose.position.y)) - return dist +# ======================== -def get_distance_to_node_tmap2(nodea, nodeb): - """ - Given two nodes it returns the straight line distance between them for tmap2 - """ - dist = math.hypot((nodeb["node"]["pose"]["position"]["x"] - nodea["node"]["pose"]["position"]["x"]), - (nodeb["node"]["pose"]["position"]["y"] - nodea["node"]["pose"]["position"]["y"])) - return dist +class NoAliasDumper(yaml.SafeDumper): + """YAML dumper that disables aliases/anchors for cleaner output.""" -def get_conected_nodes(node): - """ - Given a node it returns the nodes connected to it by one single edge - """ - childs = [] - for i in node.edges: - childs.append(i.node) - return childs + def ignore_aliases(self, data): + """Return True for all data to disable aliases.""" + return True -def get_conected_nodes_tmap2(node): - """ - Given a node it returns the nodes connected to it by one single edge - """ - childs = [] - for i in node["node"]["edges"]: - childs.append(i["node"]) - return childs +# ======================== -def get_edges_between(top_map, nodea, nodeb): - """ - Given a node a it returns the connecting edges to node b - """ - ab = [] - noda = get_node(top_map, nodea) - for j in noda.edges: - if j.node == nodeb: - ab.append(j) - return ab - - -def get_edges_between_tmap2(top_map, nodea, nodeb): - """ - Given a node a it returns the connecting edges to node b - """ - ab = [] - noda = get_node_from_tmap2(top_map, nodea) - for j in noda["node"]["edges"]: - if j["node"] == nodeb: - ab.append(j) - return ab - - -def get_edge_from_id(top_map, node_name, edge_id): - """ - Given a node and the edge_id it returns the edges object - """ - node = get_node(top_map, node_name) - if node: - for i in node.edges: - if i.edge_id == edge_id: - return i +def get_node_from_tmap2(top_map, node_name): + """Given a topological map 2 and a node name return the node object.""" + for i in top_map["nodes"]: + if i["node"]["name"] == node_name: + return i return None def get_edge_from_id_tmap2(top_map, node_name, edge_id): - """ - Given a node and the edge_id it returns the edges object for topomap 2 - """ + """Given a node and the edge_id return the edge object for topomap 2.""" node = get_node_from_tmap2(top_map, node_name) if node: for i in node["node"]["edges"]: if i["edge_id"] == edge_id: return i return None - - -def get_node_names_from_edge_id(top_map, edge_id): - """ - Given a tmap and edge id it returns the names of the edge's origin and destination nodes - """ - for node in top_map.nodes: - origin = node.name - for edge in node.edges: - destination = edge.node - if edge.edge_id == edge_id: - return origin, destination - return None,None - - -def get_node_names_from_edge_id_2(top_map, edge_id): - """ - Given a tmap2 and edge id it returns the names of the edge's origin and destination nodes - """ - for node in top_map["nodes"]: - origin = node["node"]["name"] - for edge in node["node"]["edges"]: - destination = edge["node"] - if edge["edge_id"] == edge_id: - return origin, destination - return None,None -######################################################################################################### diff --git a/topological_navigation/topological_navigation/topological_map.py b/topological_navigation/topological_navigation/topological_map.py deleted file mode 100644 index ac76a331..00000000 --- a/topological_navigation/topological_navigation/topological_map.py +++ /dev/null @@ -1,396 +0,0 @@ -#!/usr/bin/env python -import math -import rospy -import warnings - -from topological_navigation.topological_node import * -from topological_navigation_msgs.msg import TopologicalNode, Edge, Vertex - - -from mongodb_store.message_store import MessageStoreProxy -#import topological_navigation.msg - - -# from http://code.activestate.com/recipes/391367-deprecated/ -def deprecated(func): - """This is a decorator which can be used to mark functions - as deprecated. It will result in a warning being emmitted - when the function is used.""" - def newFunc(*args, **kwargs): - warnings.warn("Call to deprecated function %s of topological_map. You should use functions from topological_navigation/manager.py instead." % func.__name__, - category=DeprecationWarning, - stacklevel=2) - rospy.logwarn("Call to deprecated function {0} of topological_map. You should use functions from topological_navigation/manager.py instead.".format(func.__name__)) - return func(*args, **kwargs) - newFunc.__name__ = func.__name__ - newFunc.__doc__ = func.__doc__ - newFunc.__dict__.update(func.__dict__) - return newFunc - -class topological_map(object): - - def __init__(self, name, msg = None): - if msg is None: - self.name = name - self.nodes = self.loadMap(name) - else: - self.name = msg.pointset - self.nodes = self.map_from_msg(msg.nodes) - - - def _get_node_index(self, node_name): - ind = -1 - counter = 0 - - for i in self.nodes: - if i.name == node_name: - ind = counter - counter+=1 - return ind - - - @deprecated - def update_node_waypoint(self, node_name, new_pose): - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name": node_name, "pointset": self.name} - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.map - available = msg_store.query(TopologicalNode._type, query, query_meta) - if len(available) == 1: - positionZ=available[0][0].pose.position.z - available[0][0].pose = new_pose - available[0][0].pose.position.z = positionZ - msg_store.update(available[0][0], query_meta, query, upsert=True) - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - - @deprecated - def update_node_vertex(self, node_name, vertex_index, vertex_pose): - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name": node_name, "pointset": self.name} - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.map - available = msg_store.query(TopologicalNode._type, query, query_meta) - if len(available) == 1: - #vertex_to_edit=available[0][0].verts[vertex_index] - new_x_pos = -(available[0][0].pose.position.x - vertex_pose.position.x) - new_y_pos = -(available[0][0].pose.position.y - vertex_pose.position.y) - available[0][0].verts[vertex_index].x = new_x_pos - available[0][0].verts[vertex_index].y = new_y_pos - msg_store.update(available[0][0], query_meta, query, upsert=True) - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - - @deprecated - def remove_edge(self, edge_name): - #print 'removing edge: '+edge_name - rospy.loginfo('Removing Edge: '+edge_name) - edged = edge_name.split('_') - #print edged - node_name = edged[0] - #nodeindx = self._get_node_index(edged[0]) - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name": node_name, "pointset": self.name} - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.map - available = msg_store.query(TopologicalNode._type, query, query_meta) - if len(available) == 1: - for i in available[0][0].edges: - #print i.node - if i.node == edged[1]: - available[0][0].edges.remove(i) - msg_store.update(available[0][0], query_meta, query, upsert=True) - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - - @deprecated - def update_edge(self, node_name, edge_id, new_action=None, new_top_vel=None): - msg_store = MessageStoreProxy(collection='topological_maps') - # The query retrieves the node name with the given name from the given pointset. - query = {"name": node_name, "pointset": self.name} - # The meta-information is some additional information about the specific - # map that we are interested in (?) - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.map - # This returns a tuple containing the object, if it exists, and some - # information about how it's stored in the database. - available = msg_store.query(TopologicalNode._type, query, query_meta) - if len(available) == 1: - for edge in available[0][0].edges: - if edge.edge_id == edge_id: - edge.action = new_action or edge.action - edge.top_vel = new_top_vel or edge.top_vel - - msg_store.update(available[0][0], query_meta, query, upsert=True) - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - - @deprecated - def update_node_name(self, node_name, new_name): - msg_store = MessageStoreProxy(collection='topological_maps') - # The query retrieves the node name with the given name from the given pointset. - query = {"name": node_name, "pointset": self.name} - # The meta-information is some additional information about the specific - # map that we are interested in (?) - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.map - # This returns a tuple containing the object, if it exists, and some - # information about how it's stored in the database. - available = msg_store.query(TopologicalNode._type, query, query_meta) - - if len(available) == 1: - available[0][0].name = new_name - # Also need to update all edges which involve the renamed node - allnodes_query = {"pointset": self.name} - allnodes_query_meta = {} - allnodes_query_meta["pointset"] = self.name - allnodes_query_meta["map"] = self.map - # this produces a list of tuples, each with [0] as the node, [1] as database info - allnodes_available = msg_store.query(TopologicalNode._type, {}, allnodes_query_meta) - - # Check the edges of each node for a reference to the node to be - # renamed, and change the edge id if there is one. Enumerate the - # values so that we can edit the objects in place to send them back - # to the database. - for node_ind, node_tpl in enumerate(allnodes_available): - for edge_ind, edge in enumerate(node_tpl[0].edges): - # change names of the edges in other nodes, and update their values in the database - if node_tpl[0].name != node_name and node_name in edge.edge_id: - allnodes_available[node_ind][0].edges[edge_ind].edge_id = edge.edge_id.replace(node_name, new_name) - # must also update the name of the node this edge goes to - allnodes_available[node_ind][0].edges[edge_ind].node = new_name - curnode_query = {"name": node_tpl[0].name, "pointset": self.name} - msg_store.update(allnodes_available[node_ind][0], allnodes_query_meta, curnode_query, upsert=True) - - # update all edge ids for this node - for edge_ind, edge in enumerate(available[0][0].edges): - available[0][0].edges[edge_ind].edge_id = edge.edge_id.replace(node_name, new_name) - - msg_store.update(available[0][0], query_meta, query, upsert=True) - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - - @deprecated - def add_edge(self, or_waypoint, de_waypoint, action): - #print 'removing edge: '+edge_name - rospy.loginfo('Adding Edge from '+or_waypoint+' to '+de_waypoint+' using '+action) - node_name = or_waypoint - #nodeindx = self._get_node_index(edged[0]) - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name": node_name, "pointset": self.name} - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.map - available = msg_store.query(TopologicalNode._type, query, query_meta) - if len(available) == 1: - found =False - for i in available[0][0].edges: - #print i.node - if i.node == de_waypoint: - found=True - break - if not found: - edge = Edge() - edge.edge_id = "{0}_{1}".format(or_waypoint, de_waypoint) - edge.node = de_waypoint - edge.action = action - edge.top_vel = 0.55 - edge.map_2d = available[0][0].map - available[0][0].edges.append(edge) - msg_store.update(available[0][0], query_meta, query, upsert=True) - else: - rospy.logerr("Edge already exist: Try deleting it first") - else: - rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") - rospy.logerr("Available data: "+str(available)) - - @deprecated - def remove_node(self, node_name): - rospy.loginfo('Removing Node: '+node_name) - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name": node_name, "pointset": self.name} - query_meta = {} - query_meta["pointset"] = self.name - query_meta["map"] = self.map - - available = msg_store.query(TopologicalNode._type, query, query_meta) - - node_found = False - if len(available) == 1: - node_found = True - rm_id = str(available[0][1]['_id']) - print(rm_id) - else: - rospy.logerr("Node not found "+str(len(available))+" waypoints found after query") - #rospy.logerr("Available data: "+str(available)) - - - if node_found: - query_meta = {} - query_meta["pointset"] = self.name - edges_to_rm = [] - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - for i in message_list: - for j in i[0].edges: - if j.node == node_name: - edge_rm = i[0].name+'_'+node_name - edges_to_rm.append(edge_rm) - - for k in edges_to_rm: - print('remove: '+k) - self.remove_edge(k) - msg_store.delete(rm_id) - - @deprecated - def add_node(self, name, dist, pos, std_action): - rospy.loginfo('Creating Node: '+name) - msg_store = MessageStoreProxy(collection='topological_maps') - - found = False - for i in self.nodes: - if i.name == name: - found = True - - if found: - rospy.logerr("Node already exists, try another name") - else: - rospy.loginfo('Adding node: '+name) - - meta = {} - meta["map"] = self.map - meta["pointset"] = self.name - meta["node"] = name - - node = TopologicalNode() - node.name = name - node.map = self.map - node.pointset = self.name - node.pose = pos - vertices=[(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69), (-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69), (0.287, -0.69), (0.69, -0.287)] - for j in vertices: - v = Vertex() - v.x = float(j[0]) - v.y = float(j[1]) - node.verts.append(v) - - - cx = node.pose.position.x - cy = node.pose.position.y - close_nodes = [] - for i in self.nodes: - ndist = i._get_distance(cx, cy) - if ndist < dist: - if i.name != 'ChargingPoint': - close_nodes.append(i.name) - - for i in close_nodes: - edge = Edge() - edge.node = i - edge.action = std_action - node.edges.append(edge) - - msg_store.insert(node,meta) - - for i in close_nodes: - self.add_edge(i, name, std_action) - - # need to reload the map when a node is added, for consistency - self.loadMap(self.name) - - @deprecated - def delete_map(self): - - rospy.loginfo('Deleting map: '+self.name) - msg_store = MessageStoreProxy(collection='topological_maps') - - query_meta = {} - query_meta["pointset"] = self.name - - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - for i in message_list: - rm_id = str(i[1]['_id']) - msg_store.delete(rm_id) - - @deprecated - def map_from_msg(self, nodes): - #self.topol_map = msg.pointset - points = [] - for i in nodes: - self.map = i.map - b = topological_node(i.name) - edges = [] - for j in i.edges: - data = {} - data["node"]=j.node - data["action"]=j.action - edges.append(data) - b.edges = edges - verts = [] - for j in i.verts: - data = [j.x,j.y] - verts.append(data) - b._insert_vertices(verts) - c=i.pose - waypoint=[str(c.position.x), str(c.position.y), str(c.position.z), str(c.orientation.x), str(c.orientation.y), str(c.orientation.z), str(c.orientation.w)] - b.waypoint = waypoint - b._get_coords() - points.append(b) - - return points - - @deprecated - def loadMap(self, point_set): - msg_store = MessageStoreProxy(collection='topological_maps') - - query_meta = {} - query_meta["pointset"] = point_set - - available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 - - if available <= 0: - rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") - rospy.logerr("Available pointsets: "+str(available)) - raise Exception("Can't find waypoints.") - - else: - query_meta = {} - query_meta["pointset"] = point_set - - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - - points = [] - for i in message_list: - self.map = i[0].map - b = topological_node(i[0].name) - edges = [] - for j in i[0].edges: - data = {} - data["node"]=j.node - data["action"]=j.action - edges.append(data) - b.edges = edges - - verts = [] - for j in i[0].verts: - data = [j.x,j.y] - verts.append(data) - b._insert_vertices(verts) - - c=i[0].pose - waypoint=[str(c.position.x), str(c.position.y), str(c.position.z), str(c.orientation.x), str(c.orientation.y), str(c.orientation.z), str(c.orientation.w)] - b.waypoint = waypoint - b._get_coords() - - points.append(b) - - return points diff --git a/topological_navigation/topological_navigation/topological_node.py b/topological_navigation/topological_navigation/topological_node.py deleted file mode 100644 index cfe6b649..00000000 --- a/topological_navigation/topological_navigation/topological_node.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python -import math -from geometry_msgs.msg import Pose - -def findInList(name,List): - found = name in List - if found : - pos = List.index(name) - else : - pos = -1 - return pos - - -def update_to_expand(to_expand, new_nodes, maptree, father) : - for i in new_nodes : - found_el=False - for j in to_expand : - if i == j.name : - found_el=True - if not found_el : - for k in maptree : - if i == k.name : - new_el=k - new_el._set_Father(father) - to_expand.append(new_el) - return to_expand - - -def get_node(name, maptree) : - for i in maptree : - if i.name == name : - return i - return None - - -class topological_node(object): - def __init__(self, name): - self.name = name - self.expanded=False - self.father='none' - self.px=0.0 - self.py=0.0 - self.influence_radius=1.5 - - def _insert_waypoint(self, waypoint): - self.waypoint=waypoint - self._get_coords() - - def _get_coords(self): - self.px=float(self.waypoint[0]) - self.py=float(self.waypoint[1]) - - def _get_distance(self, cx, cy): - dist=math.hypot((cx-self.px),(cy-self.py)) - return dist - - def _insert_edges(self, edges): - self.edges=edges - - def _insert_vertices(self, vertices): - self.vertices=vertices - a=0 - for i in self.vertices : - dist=math.hypot(i[0],i[1]) - if dist > a : - a=dist - self.influence_radius=a - - def _get_Children(self) : - self.expanded=True - a=['none'] - for k in self.edges : - a.append(k['node']) - a.pop(0) - return a - - def _get_action(self, node) : - for k in self.edges : - if k['node'] == node : - return k['action'] - - def _set_Father(self,father) : - self.father=father - - def _get_pose(self): - p = Pose() - p.position.x=float(self.waypoint[0]) - p.position.y=float(self.waypoint[1]) - p.position.z=float(self.waypoint[2]) - p.orientation.x=float(self.waypoint[3]) - p.orientation.y=float(self.waypoint[4]) - p.orientation.z=float(self.waypoint[5]) - p.orientation.w=float(self.waypoint[6]) - return p \ No newline at end of file diff --git a/topological_navigation/topological_navigation/topomap_marker.py b/topological_navigation/topological_navigation/topomap_marker.py deleted file mode 100644 index cf5949d3..00000000 --- a/topological_navigation/topological_navigation/topomap_marker.py +++ /dev/null @@ -1,198 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy - -from std_msgs.msg import String -from visualization_msgs.msg import * -from geometry_msgs.msg import Point - -from topological_navigation_msgs.msg import TopologicalMap -import topological_navigation.tmap_utils as tmap_utils - - -class TopologicalVis(Node): - - def __init__(self): - super().__init__('topological_map_markers') - qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - - # Marker Publisher - self.map_markers = MarkerArray() - self.topmap_pub = self.create_publisher(MarkerArray, '~/vis', 2) - self.topmap_pub.publish(self.map_markers) - - # Rescaller Subscriber - self.scale = 1 - self.rescale_sub = self.create_subscription(String, '~/rescale', self.rescale_callback, qos) - - # Map Subscriber - self.map = None - self.map_sub = self.create_subscription(TopologicalMap, '/topological_map', self.map_callback, qos) - self.get_logger().info('map visualiser init complete') - - - def rescale_callback(self, msg): - self.get_logger().info('new scale recieved') - self.get_logger().info(f'scale: {msg.data}') - self.scale = float(msg.data) - self.map_callback(self.map) - - def map_callback(self, msg): - self.get_logger().info('map callback triggered') - self.get_logger().info(f'name: {msg.name}') - self.get_logger().info(f'map: {msg.map}') - self.get_logger().info(f'pointset: {msg.pointset}') - self.get_logger().info(f'last_updated: {msg.last_updated}') - self.get_logger().info(f'total nodes: {len(msg.nodes)}') - - # Message - self.map = msg - - # Array - self.map_markers = MarkerArray() - self.map_markers.markers=[] - - # Legend - actions = list(set(sum([[edge.action for edge in node.edges] for node in self.map.nodes],[]))) - for i, action in enumerate(actions): - self.map_markers.markers.append(self.get_legend_marker(action, i)) - - # Map - for node in self.map.nodes: - - # Nodes - self.map_markers.markers.append(self.get_node_marker(node)) # Node - self.map_markers.markers.append(self.get_name_marker(node)) # Name - self.map_markers.markers.append(self.get_zone_marker(node)) # Zone - - # Edges - for edge in node.edges: - marker = self.get_edge_marker(node, edge, actions.index(edge.action)) - if marker: - self.map_markers.markers.append(marker) - - # Publish - for i, marker in enumerate(self.map_markers.markers): - marker.id = i - self.topmap_pub.publish(self.map_markers) - self.get_logger().info('new map visual published') - - - def get_legend_marker(self, action, col_id): - col=self.get_colour(col_id) - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.TEXT_VIEW_FACING - marker.text=action - marker.pose.position.x= 1.0+(0.12*(self.scale*col_id)) - marker.pose.position.y= 0.0 - marker.pose.position.z= 0.2 - marker.pose.orientation.w= 1.0 - marker.scale.z = self.scale * 0.1 - marker.color.a = 0.5 - marker.color.r = float(col[0]) - marker.color.g = float(col[1]) - marker.color.b = float(col[2]) - marker.ns='/legend' - return marker - - def get_name_marker(self, node): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.TEXT_VIEW_FACING - marker.text= node.name - marker.pose = node.pose - marker.scale.z = self.scale * 0.12 - marker.color.a = 0.9 - marker.color.r = 0.3 - marker.color.g = 0.3 - marker.color.b = 0.3 - marker.ns='/names' - return marker - - def get_edge_marker(self, node, edge, col_id): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.LINE_LIST - V1=Point() - V2=Point() - V1= node.pose.position - to_node=tmap_utils.get_node(self.map, edge.node) - col=self.get_colour(col_id) - if to_node: - V2= to_node.pose.position - marker.pose.orientation.w= 1.0 - marker.scale.x = self.scale * 0.1 - marker.color.a = 0.5 - marker.color.r = float(col[0]) - marker.color.g = float(col[1]) - marker.color.b = float(col[2]) - marker.points.append(V1) - marker.points.append(V2) - marker.ns='/edges' - return marker - else: - rospy.logwarn("No node %s found" %edge.node) - return None - - def get_node_marker(self, node): - marker = Marker() - marker.header.frame_id = "map" - marker.type = Marker.SPHERE - marker.scale.x = self.scale * 0.2 - marker.scale.y = self.scale * 0.2 - marker.scale.z = self.scale * 0.2 - marker.color.a = 0.4 - marker.color.r = 0.2 - marker.color.g = 0.2 - marker.color.b = 0.7 - marker.pose = node.pose - marker.pose.position.z = marker.pose.position.z+0.1 - marker.ns='/nodes' - return marker - - - def get_zone_marker(self, node): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.LINE_STRIP - marker.pose.orientation.w= 1.0 - marker.scale.x = self.scale * 0.1 - marker.color.a = 0.8 - marker.color.r = 0.7 - marker.color.g = 0.1 - marker.color.b = 0.2 - - for j in node.verts : - Vert = Point() - Vert.z = 0.05 - Vert.x = node.pose.position.x + j.x - Vert.y = node.pose.position.y + j.y - marker.points.append(Vert) - - Vert = Point() - Vert.z = 0.05 - Vert.x = node.pose.position.x + node.verts[0].x - Vert.y = node.pose.position.y + node.verts[0].y - marker.points.append(Vert) - marker.ns='/zones' - return marker - - def get_colour(self, number): - pallete=[[1,1,1],[0,0,0],[1,0,0],[0,1,0],[0,0,1],[1,1,0],[1,0,1],[0,1,1]] - return pallete[number] if number < len(pallete) else pallete[0] - - -def main(args=None): - rclpy.init(args=args) - - TV = TopologicalVis() - rclpy.spin(TV) - - TV.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/topological_navigation/topological_navigation/topomap_marker2.py b/topological_navigation/topological_navigation/topomap_marker2.py deleted file mode 100644 index ad556b9d..00000000 --- a/topological_navigation/topological_navigation/topomap_marker2.py +++ /dev/null @@ -1,231 +0,0 @@ -#!/usr/bin/env python3 - -import yaml -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy - -from std_msgs.msg import String -from visualization_msgs.msg import * -from geometry_msgs.msg import Point - -#from topological_navigation_msgs.msg import TopologicalMap -import topological_navigation.tmap_utils as tmap_utils - - - -# this ensures that all the poses and translates -# are float-type and not int-type as there is an -# assertion in ros2 messages (vector3, pose etc.) -# for float-type [x,y,z,w] keys. -class CustomSafeLoader(yaml.SafeLoader): - def construct_mapping(self, node, deep=False): - mapping = super().construct_mapping(node, deep=deep) - - # this can be extended to test the validity of the tmap2 - # as well at load time (or add missing keys) - for key in ['x', 'y', 'z', 'w']: - if key in mapping and isinstance(mapping[key], int): - mapping[key] = float(mapping[key]) - - return mapping - -class TopologicalVis(Node): - - def __init__(self): - super().__init__('topological_map_markers') - qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - - # Marker Publisher - self.map_markers = MarkerArray() - self.topmap_pub = self.create_publisher(MarkerArray, '~/vis', qos_profile=qos) - self.topmap_pub.publish(self.map_markers) - - # Rescaller Subscriber - self.scale = 1 - self.rescale_sub = self.create_subscription(String, '~/rescale', self.rescale_callback, qos_profile=qos) - - # Map Subscriber - self.map = None - self.map_sub = self.create_subscription(String, '/topological_map_2', self.map_callback, qos_profile=qos) - self.get_logger().info('map visualiser init complete') - - - def rescale_callback(self, msg): - self.get_logger().info('new scale recieved') - self.get_logger().info(f'scale: {msg.data}') - self.scale = float(msg.data) - self.map_callback(self.map) - - def map_callback(self, msg): - # Save Message for Republishing with scale - self.map = msg - - # Load Yaml - # self.tmap = yaml.safe_load(msg.data) - self.tmap = yaml.load(msg.data, Loader = CustomSafeLoader) - - # Display Meta Information - self.get_logger().info(' ') - self.get_logger().info('-----') - self.get_logger().info(' ') - self.get_logger().info('map callback triggered') - self.get_logger().info(f'name: {self.tmap["name"]}') - self.get_logger().info(f'map: {self.tmap["metric_map"]}') - self.get_logger().info(f'pointset: {self.tmap["pointset"]}') - self.get_logger().info(f'last_updated: {self.tmap["meta"]["last_updated"]}') - self.get_logger().info(f'total nodes: {len(self.tmap["nodes"])}') - - # Array - self.map_markers = MarkerArray() - self.map_markers.markers=[] - - # Legend - actions = list(set(sum([[edge['action'] for edge in node['node']['edges']] for node in self.tmap['nodes']],[]))) - for i, action in enumerate(actions): - self.map_markers.markers.append(self.get_legend_marker(action, i)) - - # Map - for node in self.tmap['nodes']: - - # Nodes - self.map_markers.markers.append(self.get_node_marker(node)) # Node - self.map_markers.markers.append(self.get_name_marker(node)) # Name - self.map_markers.markers.append(self.get_zone_marker(node)) # Zone - - # Edges - for edge in node['node']['edges']: - marker = self.get_edge_marker(node, edge, actions.index(edge['action'])) - if marker: - self.map_markers.markers.append(marker) - - # Publish - for i, marker in enumerate(self.map_markers.markers): - marker.id = i - self.topmap_pub.publish(self.map_markers) - self.get_logger().info('new map visual published') - - - def get_legend_marker(self, action, col_id): - col=self.get_colour(col_id) - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.TEXT_VIEW_FACING - marker.text=action - marker.pose.position.x= 1.0+(0.12*(self.scale*col_id)) - marker.pose.position.y= 0.0 - marker.pose.position.z= 0.2 - marker.pose.orientation.w= 1.0 - marker.scale.z = self.scale * 0.1 - marker.color.a = 0.5 - marker.color.r = float(col[0]) - marker.color.g = float(col[1]) - marker.color.b = float(col[2]) - marker.ns='/legend' - return marker - - def get_name_marker(self, node): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.TEXT_VIEW_FACING - marker.text= node['node']['name'] - marker.pose.position.x = node['node']['pose']['position']['x'] - marker.pose.position.y = node['node']['pose']['position']['y'] - marker.pose.position.z = node['node']['pose']['position']['z'] - marker.scale.z = self.scale * 0.12 - marker.color.a = 0.9 - marker.color.r = 0.3 - marker.color.g = 0.3 - marker.color.b = 0.3 - marker.ns='/names' - return marker - - def get_edge_marker(self, node, edge, col_id): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.LINE_LIST - V1 = Point() - V1.x = node['node']['pose']['position']['x'] - V1.y = node['node']['pose']['position']['y'] - V1.z = node['node']['pose']['position']['z'] - to_node = tmap_utils.get_node_from_tmap2(self.tmap, edge['node']) - col = self.get_colour(col_id) - if to_node: - V2 = Point() - V2.x = to_node['node']['pose']['position']['x'] - V2.y = to_node['node']['pose']['position']['y'] - V2.z = to_node['node']['pose']['position']['z'] - marker.pose.orientation.w = 1.0 - marker.scale.x = self.scale * 0.1 - marker.color.a = 0.5 - marker.color.r = float(col[0]) - marker.color.g = float(col[1]) - marker.color.b = float(col[2]) - marker.points.append(V1) - marker.points.append(V2) - marker.ns='/edges' - return marker - else: - self.get_logger().warn("No node %s found" %edge['node']) - return None - - def get_node_marker(self, node): - marker = Marker() - marker.header.frame_id = "map" - marker.type = Marker.SPHERE - marker.scale.x = self.scale * 0.2 - marker.scale.y = self.scale * 0.2 - marker.scale.z = self.scale * 0.2 - marker.color.a = 0.4 - marker.color.r = 0.2 - marker.color.g = 0.2 - marker.color.b = 0.7 - marker.pose.position.x = node['node']['pose']['position']['x'] - marker.pose.position.y = node['node']['pose']['position']['y'] - marker.pose.position.z = node['node']['pose']['position']['z'] + 0.2 - marker.ns='/nodes' - return marker - - - def get_zone_marker(self, node): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.LINE_STRIP - marker.pose.orientation.w = 1.0 - marker.scale.x = self.scale * 0.1 - marker.color.a = 0.8 - marker.color.r = 0.7 - marker.color.g = 0.1 - marker.color.b = 0.2 - - for j in node['node']['verts']: - vert = Point() - vert.z = node['node']['pose']['position']['z'] - vert.x = node['node']['pose']['position']['x'] + j['x'] - vert.y = node['node']['pose']['position']['y'] + j['y'] - marker.points.append(vert) - - vert = Point() - vert.z = node['node']['pose']['position']['z'] - vert.x = node['node']['pose']['position']['x'] + node['node']['verts'][0]['x'] - vert.y = node['node']['pose']['position']['y'] + node['node']['verts'][0]['y'] - marker.points.append(vert) - marker.ns='/zones' - return marker - - def get_colour(self, number): - pallete=[[1,1,1],[0,0,0],[1,0,0],[0,1,0],[0,0,1],[1,1,0],[1,0,1],[0,1,1]] - return pallete[number] if number < len(pallete) else pallete[0] - - -def main(args=None): - rclpy.init(args=args) - - TV = TopologicalVis() - rclpy.spin(TV) - - TV.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/topological_navigation/topological_navigation/scripts/validate_map.py b/topological_navigation/topological_navigation/validate_map.py similarity index 100% rename from topological_navigation/topological_navigation/scripts/validate_map.py rename to topological_navigation/topological_navigation/validate_map.py diff --git a/topological_navigation/topological_navigation/vertex_controller.py b/topological_navigation/topological_navigation/vertex_controller.py deleted file mode 100644 index fe61c589..00000000 --- a/topological_navigation/topological_navigation/vertex_controller.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy - -from threading import Timer -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Point - -from visualization_msgs.msg import * -from interactive_markers.interactive_marker_server import * - -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation_msgs.msg import TopologicalMap - -from topological_navigation.topological_map import * - - -class VertexControllers(object): - - def __init__(self) : - #map_name = rospy.get_param('/topological_map_name', 'top_map') - self.timer = Timer(1.0, self.timer_callback) - self._vertex_server = InteractiveMarkerServer("topological_map_zones") - self.map_update = rospy.Publisher('/update_map', std_msgs.msg.Time, queue_size=10) - rospy.Subscriber('/topological_map', TopologicalMap, self.MapCallback) - - - def update_map(self, msg) : - print("updating vertex controllers...") - self.topo_map = topological_map(msg.name, msg=msg) - self._vertex_server.clear() - self._vertex_server.applyChanges() - - for node in self.topo_map.nodes : - node._get_coords() - count=0 - for i in node.vertices : - Vert = Point() - Vert.z = 0.05 - Vert.x = node.px + i[0] - Vert.y = node.py + i[1] - vertname = "%s-%d" %(node.name, count) - Pos = Pose() - Pos.position = Vert - self._vertex_marker(vertname, Pos, vertname) - count+=1 - - - """ - MapCallback - - This function receives the Topological Map - """ - def MapCallback(self, msg) : - self.update_map(msg) - - - def _vertex_marker(self, marker_name, pose, marker_description="vertex marker"): - # create an interactive marker for our server - marker = InteractiveMarker() - marker.header.frame_id = "map" - marker.name = marker_name - marker.description = marker_description - - # the marker in the middle - box_marker = Marker() - box_marker.type = Marker.SPHERE - box_marker.scale.x = 0.25 - box_marker.scale.y = 0.25 - box_marker.scale.z = 0.25 - box_marker.color.r = 0.5 - box_marker.color.g = 0.0 - box_marker.color.b = 0.5 - box_marker.color.a = 0.8 - - # create a non-interactive control which contains the box - box_control = InteractiveMarkerControl() - box_control.always_visible = True - #box_control.always_visible = False - box_control.markers.append( box_marker ) - marker.controls.append( box_control ) - - # move x - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 1 - control.orientation.z = 0 - control.always_visible = False -# control.name = "move_x" -# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - control.name = "move_plane" - control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE - marker.controls.append(control) - - - self._vertex_server.insert(marker, self._vertex_feedback) - self._vertex_server.applyChanges() - - if pose is not None: - pose.position.z=pose.position.z+0.15 - self._vertex_server.setPose( marker.name, pose ) - self._vertex_server.applyChanges() - - - def _vertex_feedback(self, feedback): - self.info = feedback - self.timer.cancel() - del self.timer - self.timer = Timer(1.0, self.timer_callback) - self.timer.start() - - - def timer_callback(self) : - vertex_name = self.info.marker_name.rsplit('-', 1) - node_name = vertex_name[0] - vertex_index = int(vertex_name[1]) - self.topo_map.update_node_vertex(node_name, vertex_index, self.info.pose) - self.map_update.publish(rospy.Time.now()) - - - def clear(): - self._vertex_server.clear() - self._vertex_server.applyChanges() diff --git a/topological_navigation_msgs/CMakeLists.txt b/topological_navigation_msgs/CMakeLists.txt index b5ea5913..18ce3bfd 100644 --- a/topological_navigation_msgs/CMakeLists.txt +++ b/topological_navigation_msgs/CMakeLists.txt @@ -58,44 +58,14 @@ set(msg_files # Declare the custom service files set(srv_files "srv/WriteTopologicalMap.srv" - "srv/UpdateEdgeConfig.srv" - "srv/UpdateRestrictions.srv" - "srv/UpdateEdge.srv" - "srv/UpdateAction.srv" - "srv/RestrictMap.srv" "srv/EvaluateNode.srv" "srv/EvaluateEdge.srv" - "srv/AddDatum.srv" - "srv/GetTaggedNodes.srv" - "srv/GetTags.srv" - "srv/GetNodeTags.srv" - "srv/NodeMetadata.srv" - "srv/GetEdgesBetweenNodes.srv" - "srv/AddNode.srv" - "srv/RmvNode.srv" - "srv/AddEdge.srv" - "srv/AddEdgeRviz.srv" - "srv/AddContent.srv" - "srv/UpdateNodeName.srv" - "srv/UpdateNodeTolerance.srv" - "srv/ModifyTag.srv" - "srv/AddTag.srv" - "srv/GetTopologicalMap.srv" - "srv/UpdateEdgeLegacy.srv" "srv/LocalisePose.srv" "srv/GetRouteTo.srv" "srv/GetRouteBetween.srv" - "srv/EstimateTravelTime.srv" - "srv/PredictEdgeState.srv" - "srv/UpdateFailPolicy.srv" - "srv/ReconfAtEdges.srv" "srv/LoadTopoNavTestScenario.srv" "srv/RunTopoNavTestScenario.srv" - "srv/SetInfluenceZone.srv" - "srv/AddEdgeArray.srv" - "srv/AddNodeArray.srv" - "srv/SetInfluenceZoneArray.srv" - "srv/UpdateEdgeConfigArray.srv" + "srv/ReconfAtEdges.srv" ) diff --git a/topological_navigation_msgs/srv/AddContent.srv b/topological_navigation_msgs/srv/AddContent.srv deleted file mode 100644 index 5c3a29bd..00000000 --- a/topological_navigation_msgs/srv/AddContent.srv +++ /dev/null @@ -1,5 +0,0 @@ -string node -string content ---- -bool success -string content \ No newline at end of file diff --git a/topological_navigation_msgs/srv/AddDatum.srv b/topological_navigation_msgs/srv/AddDatum.srv deleted file mode 100644 index 9d8aad99..00000000 --- a/topological_navigation_msgs/srv/AddDatum.srv +++ /dev/null @@ -1,4 +0,0 @@ -float64 latitude -float64 longitude ---- -bool success diff --git a/topological_navigation_msgs/srv/AddEdge.srv b/topological_navigation_msgs/srv/AddEdge.srv deleted file mode 100644 index 9faaf93f..00000000 --- a/topological_navigation_msgs/srv/AddEdge.srv +++ /dev/null @@ -1,7 +0,0 @@ -string origin -string destination -string action -string action_type -string edge_id ---- -bool success diff --git a/topological_navigation_msgs/srv/AddEdgeArray.srv b/topological_navigation_msgs/srv/AddEdgeArray.srv deleted file mode 100644 index 8d56cf40..00000000 --- a/topological_navigation_msgs/srv/AddEdgeArray.srv +++ /dev/null @@ -1,3 +0,0 @@ -topological_navigation_msgs/AddEdgeReq[] data ---- -bool success diff --git a/topological_navigation_msgs/srv/AddEdgeRviz.srv b/topological_navigation_msgs/srv/AddEdgeRviz.srv deleted file mode 100644 index 3665264f..00000000 --- a/topological_navigation_msgs/srv/AddEdgeRviz.srv +++ /dev/null @@ -1,18 +0,0 @@ -# This service allows you to add an edge by providing two poses on the map, and -# will add an edge to the topological map based on the closest nodes to those -# clicked locations. - -# Pose of where the first mouse click happened -geometry_msgs/Pose first -# Pose of where the second mouse click happened -geometry_msgs/Pose second - -# If there are nodes within this distance of a click location (for either the -# start or end node), the edge will not be created -float32 max_distance - -# If true, add two edges - one from first->second, one from second->first -bool bidirectional ---- -bool success -string message \ No newline at end of file diff --git a/topological_navigation_msgs/srv/AddNode.srv b/topological_navigation_msgs/srv/AddNode.srv deleted file mode 100644 index e39a5616..00000000 --- a/topological_navigation_msgs/srv/AddNode.srv +++ /dev/null @@ -1,6 +0,0 @@ -string name -geometry_msgs/Pose pose -# Set to true to add nodes which are in close proximity (currently hardcoded to 8m) -bool add_close_nodes ---- -bool success diff --git a/topological_navigation_msgs/srv/AddNodeArray.srv b/topological_navigation_msgs/srv/AddNodeArray.srv deleted file mode 100644 index 3356a11c..00000000 --- a/topological_navigation_msgs/srv/AddNodeArray.srv +++ /dev/null @@ -1,3 +0,0 @@ -topological_navigation_msgs/AddNodeReq[] data ---- -bool success diff --git a/topological_navigation_msgs/srv/AddTag.srv b/topological_navigation_msgs/srv/AddTag.srv deleted file mode 100644 index effb9b3a..00000000 --- a/topological_navigation_msgs/srv/AddTag.srv +++ /dev/null @@ -1,5 +0,0 @@ -string tag -string[] node ---- -bool success -string meta \ No newline at end of file diff --git a/topological_navigation_msgs/srv/EstimateTravelTime.srv b/topological_navigation_msgs/srv/EstimateTravelTime.srv deleted file mode 100644 index 28566e98..00000000 --- a/topological_navigation_msgs/srv/EstimateTravelTime.srv +++ /dev/null @@ -1,4 +0,0 @@ -string start -string target ---- -builtin_interfaces/Duration travel_time diff --git a/topological_navigation_msgs/srv/GetEdgesBetweenNodes.srv b/topological_navigation_msgs/srv/GetEdgesBetweenNodes.srv deleted file mode 100644 index 0992ee37..00000000 --- a/topological_navigation_msgs/srv/GetEdgesBetweenNodes.srv +++ /dev/null @@ -1,5 +0,0 @@ -string nodea -string nodeb ---- -string[] ids_a_to_b -string[] ids_b_to_a \ No newline at end of file diff --git a/topological_navigation_msgs/srv/GetNodeTags.srv b/topological_navigation_msgs/srv/GetNodeTags.srv deleted file mode 100644 index 1451dc0f..00000000 --- a/topological_navigation_msgs/srv/GetNodeTags.srv +++ /dev/null @@ -1,4 +0,0 @@ -string node_name ---- -bool success -string[] tags \ No newline at end of file diff --git a/topological_navigation_msgs/srv/GetTaggedNodes.srv b/topological_navigation_msgs/srv/GetTaggedNodes.srv deleted file mode 100644 index 92c7ac2b..00000000 --- a/topological_navigation_msgs/srv/GetTaggedNodes.srv +++ /dev/null @@ -1,3 +0,0 @@ -string tag ---- -string[] nodes diff --git a/topological_navigation_msgs/srv/GetTags.srv b/topological_navigation_msgs/srv/GetTags.srv deleted file mode 100644 index 3ad97207..00000000 --- a/topological_navigation_msgs/srv/GetTags.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -string[] tags diff --git a/topological_navigation_msgs/srv/GetTopologicalMap.srv b/topological_navigation_msgs/srv/GetTopologicalMap.srv deleted file mode 100644 index f0cd0b04..00000000 --- a/topological_navigation_msgs/srv/GetTopologicalMap.srv +++ /dev/null @@ -1,3 +0,0 @@ -string pointset #Pointset to retrieve ---- -topological_navigation_msgs/TopologicalMap map diff --git a/topological_navigation_msgs/srv/ModifyTag.srv b/topological_navigation_msgs/srv/ModifyTag.srv deleted file mode 100644 index d34a705a..00000000 --- a/topological_navigation_msgs/srv/ModifyTag.srv +++ /dev/null @@ -1,6 +0,0 @@ -string tag -string new_tag -string[] node ---- -bool success -string meta \ No newline at end of file diff --git a/topological_navigation_msgs/srv/NodeMetadata.srv b/topological_navigation_msgs/srv/NodeMetadata.srv deleted file mode 100644 index e29524fe..00000000 --- a/topological_navigation_msgs/srv/NodeMetadata.srv +++ /dev/null @@ -1,9 +0,0 @@ -#string map_name -string pointset -string meta_category ---- -string[] name -string[] description -string[] goto_node -string[] node_type -bool[] at_node diff --git a/topological_navigation_msgs/srv/PredictEdgeState.srv b/topological_navigation_msgs/srv/PredictEdgeState.srv deleted file mode 100644 index 8c52ac2b..00000000 --- a/topological_navigation_msgs/srv/PredictEdgeState.srv +++ /dev/null @@ -1,5 +0,0 @@ -builtin_interfaces/Time epoch ---- -string[] edge_ids -float64[] probs -builtin_interfaces/Duration[] durations diff --git a/topological_navigation_msgs/srv/RestrictMap.srv b/topological_navigation_msgs/srv/RestrictMap.srv deleted file mode 100644 index aff76b8d..00000000 --- a/topological_navigation_msgs/srv/RestrictMap.srv +++ /dev/null @@ -1,6 +0,0 @@ -string state # the state of the robot, e.g. '{"robot": "tall", "task": "uv"}' - ---- - -bool success -string restricted_tmap # the map without the nodes/edges which does not satisfy the restrictions according to the state \ No newline at end of file diff --git a/topological_navigation_msgs/srv/RmvNode.srv b/topological_navigation_msgs/srv/RmvNode.srv deleted file mode 100644 index dc5ebb75..00000000 --- a/topological_navigation_msgs/srv/RmvNode.srv +++ /dev/null @@ -1,3 +0,0 @@ -string name ---- -bool success \ No newline at end of file diff --git a/topological_navigation_msgs/srv/SetInfluenceZone.srv b/topological_navigation_msgs/srv/SetInfluenceZone.srv deleted file mode 100644 index 1324716e..00000000 --- a/topological_navigation_msgs/srv/SetInfluenceZone.srv +++ /dev/null @@ -1,5 +0,0 @@ -string name -float64[] vertices_x -float64[] vertices_y ---- -bool success diff --git a/topological_navigation_msgs/srv/SetInfluenceZoneArray.srv b/topological_navigation_msgs/srv/SetInfluenceZoneArray.srv deleted file mode 100644 index bf3160cf..00000000 --- a/topological_navigation_msgs/srv/SetInfluenceZoneArray.srv +++ /dev/null @@ -1,3 +0,0 @@ -topological_navigation_msgs/SetInfluenceZoneReq[] data ---- -bool success diff --git a/topological_navigation_msgs/srv/UpdateAction.srv b/topological_navigation_msgs/srv/UpdateAction.srv deleted file mode 100644 index 0a8da7e1..00000000 --- a/topological_navigation_msgs/srv/UpdateAction.srv +++ /dev/null @@ -1,5 +0,0 @@ -string action_name # the action for which you are setting the type and goal for every edge in map -string action_type # e.g. move_base_msgs/MoveBaseGoal -string goal # json string setting the args of the goal ---- -bool success diff --git a/topological_navigation_msgs/srv/UpdateEdge.srv b/topological_navigation_msgs/srv/UpdateEdge.srv deleted file mode 100644 index 02ed091d..00000000 --- a/topological_navigation_msgs/srv/UpdateEdge.srv +++ /dev/null @@ -1,8 +0,0 @@ -string edge_id -string action_name -string action_type # e.g. move_base_msgs/MoveBaseGoal -string goal # json string setting the args of the goal -string fail_policy -bool not_fluid # if true then the robot gets to the exact pose of the edge's origin node before it is traversed ---- -bool success diff --git a/topological_navigation_msgs/srv/UpdateEdgeConfig.srv b/topological_navigation_msgs/srv/UpdateEdgeConfig.srv deleted file mode 100644 index 0418f595..00000000 --- a/topological_navigation_msgs/srv/UpdateEdgeConfig.srv +++ /dev/null @@ -1,9 +0,0 @@ -string edge_id -string namespace_name -string name -string value # python uses its eval function to determine the type unless it is a string -bool value_is_string -bool not_reset # if false then the param is reconfigured back to its original value after the edge is traversed ---- -bool success -string message diff --git a/topological_navigation_msgs/srv/UpdateEdgeConfigArray.srv b/topological_navigation_msgs/srv/UpdateEdgeConfigArray.srv deleted file mode 100644 index d7e166e3..00000000 --- a/topological_navigation_msgs/srv/UpdateEdgeConfigArray.srv +++ /dev/null @@ -1,3 +0,0 @@ -topological_navigation_msgs/UpdateEdgeConfigReq[] data ---- -bool success diff --git a/topological_navigation_msgs/srv/UpdateEdgeLegacy.srv b/topological_navigation_msgs/srv/UpdateEdgeLegacy.srv deleted file mode 100644 index 10d17f49..00000000 --- a/topological_navigation_msgs/srv/UpdateEdgeLegacy.srv +++ /dev/null @@ -1,6 +0,0 @@ -string edge_id -string action -float32 top_vel ---- -bool success -string message \ No newline at end of file diff --git a/topological_navigation_msgs/srv/UpdateFailPolicy.srv b/topological_navigation_msgs/srv/UpdateFailPolicy.srv deleted file mode 100644 index 79149368..00000000 --- a/topological_navigation_msgs/srv/UpdateFailPolicy.srv +++ /dev/null @@ -1,11 +0,0 @@ -string fail_policy ---- -bool success - - -# "retry_N" with N the number of retries, tries again to execute the same action -# "wait_S_N" with S the number of seconds and N the number of waits, wait action -# "replan_N" with N the number of replans, generates a new plan which does not pass through the same edge where the fail occurred -# "fail" fails the entire plan - -# can combine fail policy actions e.g "replan_2, fail" diff --git a/topological_navigation_msgs/srv/UpdateNodeName.srv b/topological_navigation_msgs/srv/UpdateNodeName.srv deleted file mode 100644 index 2762bee9..00000000 --- a/topological_navigation_msgs/srv/UpdateNodeName.srv +++ /dev/null @@ -1,5 +0,0 @@ -string node_name -string new_name ---- -bool success -string message \ No newline at end of file diff --git a/topological_navigation_msgs/srv/UpdateNodeTolerance.srv b/topological_navigation_msgs/srv/UpdateNodeTolerance.srv deleted file mode 100644 index 88e708cb..00000000 --- a/topological_navigation_msgs/srv/UpdateNodeTolerance.srv +++ /dev/null @@ -1,6 +0,0 @@ -string node_name -float64 xy_tolerance -float64 yaw_tolerance ---- -bool success -string message \ No newline at end of file diff --git a/topological_navigation_msgs/srv/UpdateRestrictions.srv b/topological_navigation_msgs/srv/UpdateRestrictions.srv deleted file mode 100644 index af0dbb6f..00000000 --- a/topological_navigation_msgs/srv/UpdateRestrictions.srv +++ /dev/null @@ -1,7 +0,0 @@ -string name # node name or edge id -string restrictions_planning # boolean sentence -string restrictions_runtime # boolean sentence -bool update_edges # if updating node restrictions then apply planning restrictions to edges involving the node ---- -bool success -string message diff --git a/topological_navigation_visual/launch/topological_map_visualiser.launch.py b/topological_navigation_visual/launch/topological_map_visualiser.launch.py new file mode 100644 index 00000000..b26743de --- /dev/null +++ b/topological_navigation_visual/launch/topological_map_visualiser.launch.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +"""Launch file for the unified topological map visualiser. + +Usage +----- +Live visualisation (subscribes to /topological_map_2):: + + ros2 launch topological_navigation_visual topological_map_visualiser.launch.py + +Interactive editing from file:: + + ros2 launch topological_navigation_visual topological_map_visualiser.launch.py \ + map_file:=/path/to/map.tmap2.yaml +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'map_file', default_value='', + description='Path to a .tmap2.yaml file. ' + 'Leave empty to subscribe to /topological_map_2.', + ), + DeclareLaunchArgument( + 'auto_save', default_value='false', + description='Periodically save the map (every 30 s).', + ), + DeclareLaunchArgument( + 'marker_scale', default_value='0.5', + description='Scale factor for markers in RViz.', + ), + DeclareLaunchArgument( + 'edit_mode', default_value='true', + description='Enable interactive drag-and-drop editing.', + ), + DeclareLaunchArgument( + 'nav_action_name', default_value='/topological_navigation', + description='GotoNode action server name for click-to-navigate.', + ), + Node( + package='topological_navigation_visual', + executable='topological_map_visualiser.py', + name='topological_map_visualiser', + output='screen', + parameters=[{ + 'map_file': LaunchConfiguration('map_file'), + 'auto_save': LaunchConfiguration('auto_save'), + 'marker_scale': LaunchConfiguration('marker_scale'), + 'edit_mode': LaunchConfiguration('edit_mode'), + 'nav_action_name': LaunchConfiguration('nav_action_name'), + }], + ), + ]) diff --git a/topological_utils/package.xml b/topological_navigation_visual/package.xml similarity index 52% rename from topological_utils/package.xml rename to topological_navigation_visual/package.xml index 996a408e..527002e4 100644 --- a/topological_utils/package.xml +++ b/topological_navigation_visual/package.xml @@ -1,17 +1,12 @@ - topological_utils - 3.0.5 - The ros2 topological_utils package - + topological_navigation_visual + 1.0.0 + Visualization and interactive editing tools for topological maps Marc Hanheide - James R Heselden - - James R Heselden - Jaime Pulido Fentanes - Gautham P Das + Ibrahim Hroob Apache License 2.0 @@ -20,8 +15,15 @@ ament_pep257 python3-pytest + geometry_msgs + std_msgs + std_srvs + visualization_msgs + interactive_markers topological_navigation_msgs - + topological_navigation + tf_transformations + python3-numpy ament_python diff --git a/topological_rviz_tools/COLCON_IGNORE b/topological_navigation_visual/resource/topological_navigation_visual similarity index 100% rename from topological_rviz_tools/COLCON_IGNORE rename to topological_navigation_visual/resource/topological_navigation_visual diff --git a/topological_navigation_visual/setup.cfg b/topological_navigation_visual/setup.cfg new file mode 100644 index 00000000..efaa4692 --- /dev/null +++ b/topological_navigation_visual/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/topological_navigation_visual +[install] +install_scripts=$base/lib/topological_navigation_visual diff --git a/topological_navigation_visual/setup.py b/topological_navigation_visual/setup.py new file mode 100644 index 00000000..85a7667a --- /dev/null +++ b/topological_navigation_visual/setup.py @@ -0,0 +1,37 @@ +from setuptools import find_packages +from setuptools import setup +from glob import glob + +package_name = 'topological_navigation_visual' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch/', glob('launch/*', recursive=True)), + ('share/' + package_name + '/config/', glob('config/*', recursive=True)), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Ibrahim Hroob', + maintainer_email='ihroob@lincoln.ac.uk', + description='Visualization and interactive editing tools for topological maps', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + # Unified map visualization + interactive editor + 'topological_map_visualiser.py = topological_navigation_visual.scripts.topological_map_visualiser:main', + + # Route and occupancy visualization + 'topological_visual.py = topological_navigation_visual.scripts.topological_visual:main', + + # Policy visualization + 'policy_marker.py = topological_navigation_visual.policy_marker:main', + ], + }, +) diff --git a/topological_utils/__init__.py b/topological_navigation_visual/test/__init__.py similarity index 100% rename from topological_utils/__init__.py rename to topological_navigation_visual/test/__init__.py diff --git a/topological_utils/resource/__init__.py b/topological_navigation_visual/topological_navigation_visual/__init__.py similarity index 100% rename from topological_utils/resource/__init__.py rename to topological_navigation_visual/topological_navigation_visual/__init__.py diff --git a/topological_navigation_visual/topological_navigation_visual/policy_marker.py b/topological_navigation_visual/topological_navigation_visual/policy_marker.py new file mode 100644 index 00000000..808c453f --- /dev/null +++ b/topological_navigation_visual/topological_navigation_visual/policy_marker.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +"""Policy visualisation node. + +Subscribes to ``/topological_map_2`` for the current map and to +``mdp_plan_exec/current_policy_mode`` for the active policy. Publishes +arrow markers on ``~/vis`` to show policy edges. +""" + +import math + +import rclpy +import yaml +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy + +from geometry_msgs.msg import Pose +from std_msgs.msg import String +from visualization_msgs.msg import Marker, MarkerArray +from tf_transformations import quaternion_from_euler + +from topological_navigation.tmap_utils import CustomSafeLoader +from topological_navigation.networkx_utils import build_graph_from_tmap +from topological_navigation.tmap_utils import get_edge_from_id_tmap2 + +from topological_navigation_msgs.msg import NavRoute + + +class PoliciesVis(Node): + """Visualise the current topological policy as arrows in RViz.""" + + def __init__(self): + super().__init__('topological_policy_markers') + self.lnodes = None + self._graph = None + self.policy = MarkerArray() + + self.marker_pub = self.create_publisher(MarkerArray, '~/vis', 10) + self.marker_pub.publish(self.policy) + + qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ) + self.create_subscription( + String, '/topological_map_2', self._map_cb, qos + ) + self.create_subscription( + NavRoute, 'mdp_plan_exec/current_policy_mode', + self._policy_cb, qos, + ) + self.get_logger().info('Policy visualiser started') + + def _map_cb(self, msg: String): + self.lnodes = yaml.load(msg.data, Loader=CustomSafeLoader) + self._graph = build_graph_from_tmap( + self.lnodes, logger=self.get_logger(), + ) + self.get_logger().info('Map received for policy visualisation') + + def _policy_cb(self, msg: NavRoute): + if self.lnodes is None or self._graph is None: + return + + self.policy = MarkerArray() + added_sources = [] + + for i in range(len(msg.source)): + source = msg.source[i] + edge_id = msg.edge_id[i] + + if source not in self._graph: + continue + ori_attrs = self._graph.nodes[source] + ori_pose = { + 'position': { + 'x': ori_attrs['x'], + 'y': ori_attrs['y'], + 'z': ori_attrs['z'], + }, + } + + edge_data = get_edge_from_id_tmap2( + self.lnodes, source, edge_id + ) + if edge_data is None: + continue + + tgt_name = edge_data['node'] + if tgt_name not in self._graph: + continue + tgt_attrs = self._graph.nodes[tgt_name] + tgt_pose = { + 'position': { + 'x': tgt_attrs['x'], + 'y': tgt_attrs['y'], + 'z': tgt_attrs['z'], + }, + } + + added_sources.append(source) + colour = ( + [0.1, 0.1, 0.9] + if edge_data['node'] in added_sources + else [0.9, 0.1, 0.1] + ) + self.policy.markers.append( + self._arrow( + ori_pose, tgt_pose, colour + ) + ) + + for idx, m in enumerate(self.policy.markers): + m.id = idx + self.marker_pub.publish(self.policy) + + @staticmethod + def _arrow(pose1_dict, pose2_dict, colour) -> Marker: + m = Marker() + m.header.frame_id = 'map' + m.type = Marker.ARROW + + p1x = float(pose1_dict['position']['x']) + p1y = float(pose1_dict['position']['y']) + p1z = float(pose1_dict['position']['z']) + p2x = float(pose2_dict['position']['x']) + p2y = float(pose2_dict['position']['y']) + + angle = math.atan2(p2y - p1y, p2x - p1x) + qat = quaternion_from_euler(0, 0, angle) + + pose = Pose() + pose.position.x = p1x + pose.position.y = p1y + pose.position.z = p1z + pose.orientation.w = qat[3] + pose.orientation.x = qat[0] + pose.orientation.y = qat[1] + pose.orientation.z = qat[2] + + r = math.hypot(p2y - p1y, p2x - p1x) + m.scale.x = r + m.scale.y = 0.15 + m.scale.z = 0.15 + m.color.a = 0.95 + m.color.r = float(colour[0]) + m.color.g = float(colour[1]) + m.color.b = float(colour[2]) + m.pose = pose + return m + + +def main(args=None): + rclpy.init(args=args) + node = PoliciesVis() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/topological_utils/topological_utils/__init__.py b/topological_navigation_visual/topological_navigation_visual/scripts/__init__.py similarity index 100% rename from topological_utils/topological_utils/__init__.py rename to topological_navigation_visual/topological_navigation_visual/scripts/__init__.py diff --git a/topological_navigation_visual/topological_navigation_visual/scripts/topological_map_visualiser.py b/topological_navigation_visual/topological_navigation_visual/scripts/topological_map_visualiser.py new file mode 100644 index 00000000..5dd1ea12 --- /dev/null +++ b/topological_navigation_visual/topological_navigation_visual/scripts/topological_map_visualiser.py @@ -0,0 +1,1141 @@ +#!/usr/bin/env python3 +"""Unified topological map visualiser with interactive editing. + +Provides a single ROS 2 node that: +- Subscribes to ``/topological_map_2`` and renders nodes, edges, zones, and + labels as RViz ``MarkerArray`` messages. +- Optionally loads a map directly from a YAML file for editing (``map_file`` + parameter). +- Creates interactive markers for every node, allowing the user to drag + positions (X/Y) and rotate yaw in RViz. +- Saves the modified map back to YAML and re-publishes it on + ``/topological_map_2`` so that downstream nodes pick up changes + immediately. + +Usage +----- +**Live visualisation only** (subscribes to map topic):: + + ros2 run topological_navigation_visual topological_map_visualiser.py + +**Interactive editing from file**:: + + ros2 run topological_navigation_visual topological_map_visualiser.py \\ + --ros-args -p map_file:=/path/to/map.tmap2.yaml + +**Save** (from another terminal):: + + ros2 service call /topological_map_visualiser/save_map std_srvs/srv/Trigger + +""" + +import math +import os +import sys +import tempfile +from copy import deepcopy + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.qos import QoSProfile, DurabilityPolicy + +import yaml +import tf_transformations + +from geometry_msgs.msg import Point, Pose +from std_msgs.msg import String +from std_srvs.srv import Trigger +from visualization_msgs.msg import ( + InteractiveMarker, + InteractiveMarkerControl, + InteractiveMarkerFeedback, + Marker, + MarkerArray, +) +from rclpy.action import ActionClient +from interactive_markers import InteractiveMarkerServer, MenuHandler + +from topological_navigation.tmap_utils import CustomSafeLoader +from topological_navigation.navigation_graph import plan_route +from topological_navigation.networkx_utils import build_graph_from_tmap +from topological_navigation_msgs.action import GotoNode + +# ────────────────────────────────────────────────────────────────── +# Colour palette +# ────────────────────────────────────────────────────────────────── +_PALETTE = [ + [0.2, 0.2, 0.7], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + [0.0, 1.0, 0.0], + [1.0, 1.0, 0.0], + [1.0, 0.0, 1.0], + [0.0, 1.0, 1.0], + [1.0, 1.0, 1.0], +] + + +def _colour(index: int): + """Return an RGB list from the palette, wrapping around.""" + return _PALETTE[index % len(_PALETTE)] + + +def _node2pose(pose_dict) -> Pose: + """Convert a tmap2 pose dict to a ``geometry_msgs/Pose``.""" + p = Pose() + p.position.x = float(pose_dict['position']['x']) + p.position.y = float(pose_dict['position']['y']) + p.position.z = float(pose_dict['position']['z']) + p.orientation.w = float(pose_dict['orientation']['w']) + p.orientation.x = float(pose_dict['orientation']['x']) + p.orientation.y = float(pose_dict['orientation']['y']) + p.orientation.z = float(pose_dict['orientation']['z']) + return p + + +def _get_node(nodes_list, name): + """Look up a node dict by name from the tmap2 node list.""" + for entry in nodes_list: + if entry['node']['name'] == name: + return entry['node'] + return None + + +# ══════════════════════════════════════════════════════════════════ +# Main visualiser node +# ══════════════════════════════════════════════════════════════════ +class TopologicalMapVisualiser(Node): + """Unified map visualiser with optional interactive editing.""" + + def __init__(self): + super().__init__('topological_map_visualiser') + + # ── Parameters ────────────────────────────────────────── + self.declare_parameter('map_file', '') + self.declare_parameter('auto_save', False) + self.declare_parameter('marker_scale', 0.5) + self.declare_parameter('edit_mode', True) + self.declare_parameter( + 'nav_action_name', '/topological_navigation', + ) + + self.map_file: str = self.get_parameter('map_file').value + self.auto_save: bool = self.get_parameter('auto_save').value + self.marker_scale: float = self.get_parameter('marker_scale').value + self.edit_mode: bool = self.get_parameter('edit_mode').value + nav_action: str = self.get_parameter('nav_action_name').value + + # ── State ──────────────────────────────────────────────── + self.tmap = None + self._graph = None + self._map_dirty = False + self._navigating_to: str | None = None + self._current_node: str = 'none' + self._closest_node: str = 'none' + self._route_nodes: list = [] # ordered node names on active route + self._pending_route_target: str | None = None + self._route_retry_timer = None + self._initial_vis_timer = None + + # ── Debounced republish after drag ──────────────────────── + self._republish_timer = None + self._temp_map_dir = os.path.join( + tempfile.gettempdir(), 'topological_maps', + ) + os.makedirs(self._temp_map_dir, exist_ok=True) + + # ── QoS ────────────────────────────────────────────────── + self._latching_qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ) + self._cb_group = ReentrantCallbackGroup() + + # ── Publishers ─────────────────────────────────────────── + self.map_marker_pub = self.create_publisher( + MarkerArray, + 'topological_map_visualisation', + qos_profile=self._latching_qos, + ) + self.map_topic_pub = self.create_publisher( + String, + '/topological_map_2', + qos_profile=self._latching_qos, + ) + self.route_marker_pub = self.create_publisher( + MarkerArray, + 'topological_route_visualisation', + qos_profile=self._latching_qos, + ) + + # ── Interactive-marker server (for dragging nodes) ────── + self._im_server = InteractiveMarkerServer( + self, 'topological_map_editor' + ) + + # ── GotoNode action client ────────────────────────────── + self._goto_client = ActionClient( + self, + GotoNode, + nav_action, + callback_group=self._cb_group, + ) + self._goto_goal_handle = None + + # ── Menu handler for right-click context menu ──────────── + self._menu_handler = MenuHandler() + self._menu_nav_id = self._menu_handler.insert( + 'Navigate Here', callback=self._menu_navigate_cb, + ) + self._menu_cancel_id = self._menu_handler.insert( + 'Cancel Navigation', callback=self._menu_cancel_cb, + ) + + # ── Subscribe to localisation topics ───────────────────── + self.create_subscription( + String, + '/current_node', + self._current_node_cb, + 10, + callback_group=self._cb_group, + ) + self.create_subscription( + String, + '/closest_node', + self._closest_node_cb, + 10, + callback_group=self._cb_group, + ) + + # ── Load map from file or subscribe to topic ───────────── + if self.map_file: + self._load_map_from_file() + self._publish_map_topic() + self._rebuild_visualisation() + # Delayed re-publish so late-subscribing RViz gets markers + self._initial_vis_timer = self.create_timer( + 2.0, self._delayed_initial_vis, + callback_group=self._cb_group, + ) + else: + self.get_logger().info( + 'No map_file parameter — subscribing to /topological_map_2' + ) + + # Always subscribe so we pick up external updates too + self.create_subscription( + String, + '/topological_map_2', + self._map_topic_cb, + qos_profile=self._latching_qos, + callback_group=self._cb_group, + ) + + # ── Save service ───────────────────────────────────────── + self.create_service( + Trigger, + f'{self.get_name()}/save_map', + self._save_map_service, + callback_group=self._cb_group, + ) + + # ── Auto-save timer ────────────────────────────────────── + if self.auto_save and self.map_file: + self.create_timer(30.0, self._auto_save_cb) + + self.get_logger().info('Topological map visualiser started') + if self.map_file: + self.get_logger().info(f' map_file : {self.map_file}') + self.get_logger().info(f' edit_mode: {self.edit_mode}') + self.get_logger().info(f' auto_save: {self.auto_save}') + self.get_logger().info( + 'Call //save_map service to persist changes' + ) + + # ────────────────────────────────────────────────────────────── + # Map loading / saving + # ────────────────────────────────────────────────────────────── + def _load_map_from_file(self): + """Load a tmap2 YAML file into ``self.tmap``.""" + try: + with open(self.map_file, 'r') as fh: + self.tmap = yaml.load(fh, Loader=CustomSafeLoader) + self._graph = build_graph_from_tmap( + self.tmap, logger=self.get_logger(), + ) + self.get_logger().info( + f'Loaded map with {len(self.tmap.get("nodes", []))} nodes ' + f'from {self.map_file}' + ) + except Exception as exc: + self.get_logger().error(f'Failed to load map file: {exc}') + sys.exit(1) + + def save_map(self): + """Save current map to the YAML file (with backup).""" + if not self.map_file: + self.get_logger().warn('No map_file set — cannot save') + return False + try: + backup = self.map_file + '.backup' + if os.path.exists(self.map_file): + if os.path.exists(backup): + os.remove(backup) + os.rename(self.map_file, backup) + + with open(self.map_file, 'w') as fh: + yaml.dump( + self.tmap, + fh, + default_flow_style=False, + sort_keys=False, + ) + self.get_logger().info(f'Map saved to {self.map_file}') + self._map_dirty = False + self._publish_map_topic() + return True + + except Exception as exc: + self.get_logger().error(f'Save failed: {exc}') + backup = self.map_file + '.backup' + if os.path.exists(backup): + os.rename(backup, self.map_file) + return False + + # ── Callbacks ──────────────────────────────────────────────── + def _current_node_cb(self, msg: String): + """Track the robot's current topological node.""" + self._current_node = msg.data + + def _closest_node_cb(self, msg: String): + """Track the robot's closest topological node.""" + self._closest_node = msg.data + + def _map_topic_cb(self, msg: String): + """Handle updates from the ``/topological_map_2`` topic.""" + incoming = yaml.load(msg.data, Loader=CustomSafeLoader) + if incoming == self.tmap: + return # no change, avoid flicker + self.tmap = incoming + self._graph = build_graph_from_tmap( + self.tmap, logger=self.get_logger(), + ) + self.get_logger().info('Received updated map from topic') + self._rebuild_visualisation() + # Delayed re-publish so late-subscribing RViz gets markers + if self._initial_vis_timer is not None: + self.destroy_timer(self._initial_vis_timer) + self._initial_vis_timer = self.create_timer( + 2.0, self._delayed_initial_vis, + callback_group=self._cb_group, + ) + + def _save_map_service(self, request, response): + ok = self.save_map() + response.success = ok + response.message = ( + f'Map saved to {self.map_file}' if ok else 'Save failed' + ) + return response + + def _auto_save_cb(self): + if self._map_dirty: + self.get_logger().info('Auto-saving map…') + self.save_map() + + def _delayed_initial_vis(self): + """One-shot republish so late-subscribing RViz gets markers.""" + if self._initial_vis_timer is not None: + self.destroy_timer(self._initial_vis_timer) + self._initial_vis_timer = None + if self.tmap is not None: + self._rebuild_static_markers() + self.get_logger().debug( + 'Deferred map visualisation republished' + ) + + # ────────────────────────────────────────────────────────────── + # GotoNode navigation helpers + # ────────────────────────────────────────────────────────────── + def _menu_navigate_cb(self, feedback: InteractiveMarkerFeedback): + """Context-menu callback: send GotoNode goal for this node.""" + node_name = feedback.marker_name + self.get_logger().info( + f'Navigate request → {node_name}' + ) + # Compute and highlight route before sending goal + self._compute_and_highlight_route(node_name) + self._send_goto_goal(node_name) + + def _menu_cancel_cb(self, feedback: InteractiveMarkerFeedback): + """Context-menu callback: cancel the active GotoNode goal.""" + self._cancel_navigation() + + def _send_goto_goal(self, target: str): + """Send a ``GotoNode`` goal to the topological navigation server.""" + if not self._goto_client.wait_for_server(timeout_sec=2.0): + self.get_logger().error( + 'GotoNode action server not available — is navigation2 ' + 'running?' + ) + return + goal = GotoNode.Goal() + goal.target = target + goal.no_orientation = False + self.get_logger().info(f'Sending GotoNode goal: target={target}') + + future = self._goto_client.send_goal_async( + goal, feedback_callback=self._goto_feedback_cb, + ) + future.add_done_callback(self._goto_response_cb) + self._navigating_to = target + + def _goto_response_cb(self, future): + """Handle the goal acceptance / rejection response.""" + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().warn('GotoNode goal was rejected') + self._navigating_to = None + return + self.get_logger().info('GotoNode goal accepted') + self._goto_goal_handle = goal_handle + result_future = goal_handle.get_result_async() + result_future.add_done_callback(self._goto_result_cb) + + def _goto_result_cb(self, future): + """Handle the final result of a GotoNode action.""" + result = future.result().result + status = 'SUCCESS' if result.success else 'FAILURE' + self.get_logger().info( + f'GotoNode result: {status} (target={self._navigating_to})' + ) + self._navigating_to = None + self._goto_goal_handle = None + self._clear_route_highlight() + + def _goto_feedback_cb(self, feedback_msg): + """Log GotoNode action feedback.""" + fb = feedback_msg.feedback + self.get_logger().debug(f'GotoNode feedback: route={fb.route}') + + def _cancel_navigation(self): + """Cancel the currently active GotoNode goal, if any.""" + if self._goto_goal_handle is not None: + self.get_logger().info('Cancelling active GotoNode goal') + self._goto_goal_handle.cancel_goal_async() + self._navigating_to = None + self._goto_goal_handle = None + self._clear_route_highlight() + else: + self.get_logger().info('No active navigation goal to cancel') + + # ────────────────────────────────────────────────────────────── + # Route highlighting + # ────────────────────────────────────────────────────────────── + def _get_source_node(self) -> str: + """Return the best available source node for route search.""" + if self._current_node and self._current_node != 'none': + return self._current_node + if self._closest_node and self._closest_node != 'none': + return self._closest_node + return 'none' + + def _compute_and_highlight_route(self, target: str): + """Compute route from current node to target and publish markers.""" + source = self._get_source_node() + if source == 'none': + self.get_logger().info( + 'Source node unknown — will retry route highlight' + ) + self._pending_route_target = target + if self._route_retry_timer is None: + self._route_retry_timer = self.create_timer( + 0.5, self._retry_route_highlight, + callback_group=self._cb_group, + ) + return + if self._graph is None: + if self.tmap is not None: + self._graph = build_graph_from_tmap( + self.tmap, logger=self.get_logger(), + ) + else: + self.get_logger().warn( + 'Cannot highlight route — no map loaded' + ) + return + + route_nodes = plan_route( + self._graph, source, target, + logger=self.get_logger(), + ) + if not route_nodes or len(route_nodes) < 2: + self.get_logger().warn( + f'No route found from {source} to {target}' + ) + self._route_nodes = [] + self._clear_route_highlight() + return + + self._route_nodes = route_nodes + self.get_logger().info( + f'Route highlighted: {" → ".join(self._route_nodes)}' + ) + self._publish_route_markers() + + def _retry_route_highlight(self): + """Timer callback: retry route highlighting once source is known.""" + if self._pending_route_target is None or self._navigating_to is None: + # Navigation finished or cancelled before source appeared + self._pending_route_target = None + if self._route_retry_timer is not None: + self.destroy_timer(self._route_retry_timer) + self._route_retry_timer = None + return + + source = self._get_source_node() + if source == 'none': + return # Still unknown — timer will fire again + + target = self._pending_route_target + self._pending_route_target = None + if self._route_retry_timer is not None: + self.destroy_timer(self._route_retry_timer) + self._route_retry_timer = None + + self._compute_and_highlight_route(target) + + def _publish_route_markers(self): + """Create and publish route highlight markers.""" + if not self._route_nodes or self.tmap is None: + return + + nodes = self.tmap.get('nodes', []) + marker_array = MarkerArray() + idn = 0 + scale = self.marker_scale + + # Highlight nodes on the route + for node_name in self._route_nodes: + node_data = _get_node(nodes, node_name) + if node_data is None: + continue + m = Marker() + m.id = idn + m.header.frame_id = node_data.get('parent_frame', 'map') + m.type = Marker.SPHERE + m.scale.x = scale * 0.7 + m.scale.y = scale * 0.7 + m.scale.z = scale * 0.7 + m.color.a = 0.9 + m.color.r = 0.0 + m.color.g = 1.0 + m.color.b = 0.0 + m.pose = _node2pose(node_data['pose']) + m.pose.position.z += 0.15 + m.ns = '/route_nodes' + marker_array.markers.append(m) + idn += 1 + + # Highlight edges along the route + for i in range(len(self._route_nodes) - 1): + src_name = self._route_nodes[i] + dst_name = self._route_nodes[i + 1] + src_data = _get_node(nodes, src_name) + dst_data = _get_node(nodes, dst_name) + if src_data is None or dst_data is None: + continue + + m = Marker() + m.id = idn + m.header.frame_id = src_data.get('parent_frame', 'map') + m.type = Marker.LINE_STRIP + m.pose.orientation.w = 1.0 + m.scale.x = scale * 0.12 + m.color.a = 0.9 + m.color.r = 0.0 + m.color.g = 1.0 + m.color.b = 0.0 + + v1 = _node2pose(src_data['pose']).position + v1.z += 0.15 + v2 = _node2pose(dst_data['pose']).position + v2.z += 0.15 + m.points.append(v1) + m.points.append(v2) + m.ns = '/route_edges' + marker_array.markers.append(m) + idn += 1 + + # Source node highlight (cyan) + src_data = _get_node(nodes, self._route_nodes[0]) + if src_data is not None: + m = Marker() + m.id = idn + m.header.frame_id = src_data.get('parent_frame', 'map') + m.type = Marker.SPHERE + m.scale.x = scale * 0.9 + m.scale.y = scale * 0.9 + m.scale.z = scale * 0.9 + m.color.a = 0.7 + m.color.r = 0.0 + m.color.g = 1.0 + m.color.b = 1.0 + m.pose = _node2pose(src_data['pose']) + m.pose.position.z += 0.2 + m.ns = '/route_endpoints' + marker_array.markers.append(m) + idn += 1 + + # Target node highlight (yellow) + tgt_data = _get_node(nodes, self._route_nodes[-1]) + if tgt_data is not None: + m = Marker() + m.id = idn + m.header.frame_id = tgt_data.get('parent_frame', 'map') + m.type = Marker.SPHERE + m.scale.x = scale * 0.9 + m.scale.y = scale * 0.9 + m.scale.z = scale * 0.9 + m.color.a = 0.7 + m.color.r = 1.0 + m.color.g = 1.0 + m.color.b = 0.0 + m.pose = _node2pose(tgt_data['pose']) + m.pose.position.z += 0.2 + m.ns = '/route_endpoints' + marker_array.markers.append(m) + idn += 1 + + # Route label + if tgt_data is not None: + m = Marker() + m.id = idn + m.header.frame_id = tgt_data.get('parent_frame', 'map') + m.type = Marker.TEXT_VIEW_FACING + m.text = f'Route: {self._route_nodes[0]} → {self._route_nodes[-1]}' + m.pose = _node2pose(tgt_data['pose']) + m.pose.position.z += 0.6 + m.scale.z = scale * 0.35 + m.color.a = 1.0 + m.color.r = 0.0 + m.color.g = 1.0 + m.color.b = 0.0 + m.ns = '/route_label' + marker_array.markers.append(m) + idn += 1 + + self.route_marker_pub.publish(marker_array) + + def _clear_route_highlight(self): + """Remove route highlight markers from RViz.""" + self._route_nodes = [] + self._pending_route_target = None + if self._route_retry_timer is not None: + self.destroy_timer(self._route_retry_timer) + self._route_retry_timer = None + # Publish a DELETE_ALL marker to clear the route topic + marker_array = MarkerArray() + m = Marker() + m.action = Marker.DELETEALL + marker_array.markers.append(m) + self.route_marker_pub.publish(marker_array) + + # ────────────────────────────────────────────────────────────── + # Publishing helpers + # ────────────────────────────────────────────────────────────── + def _publish_map_topic(self): + """Publish the in-memory map to ``/topological_map_2``.""" + if self.tmap is None: + return + msg = String() + msg.data = yaml.dump( + self.tmap, default_flow_style=False, sort_keys=False + ) + self.map_topic_pub.publish(msg) + self.get_logger().info('Published map to /topological_map_2') + + # ────────────────────────────────────────────────────────────── + # Build / rebuild all visualisation markers + # ────────────────────────────────────────────────────────────── + def _rebuild_visualisation(self): + """Re-create marker array + interactive markers from ``self.tmap``.""" + if self.tmap is None: + return + + nodes = self.tmap.get('nodes', []) + marker_array = MarkerArray() + actions_seen: list = [] + idn = 0 + + for entry in nodes: + node = entry['node'] + + # Collect unique edge actions for the legend + for edge in node.get('edges', []): + act = edge.get('action', '') + if act and act not in actions_seen: + actions_seen.append(act) + + # Node sphere + marker_array.markers.append(self._mk_node(node, idn)) + idn += 1 + # Name label + marker_array.markers.append(self._mk_name(node, idn)) + idn += 1 + # Influence zone + if node.get('verts'): + marker_array.markers.append(self._mk_zone(node, idn)) + idn += 1 + # Edges + for edge in node.get('edges', []): + m = self._mk_edge(node, edge, actions_seen) + if m is not None: + m.id = idn + marker_array.markers.append(m) + idn += 1 + + # Legend + for row, action_name in enumerate(actions_seen): + marker_array.markers.append( + self._mk_legend(action_name, row, actions_seen, idn) + ) + idn += 1 + + self.map_marker_pub.publish(marker_array) + + # Interactive editor markers + if self.edit_mode: + self._rebuild_interactive_markers(nodes) + + self.get_logger().info( + f'Visualisation published ({len(nodes)} nodes, ' + f'{idn} markers)' + ) + + # ────────────────────────────────────────────────────────────── + # Interactive marker layer + # ────────────────────────────────────────────────────────────── + def _rebuild_interactive_markers(self, nodes): + """Create / update interactive markers for every node.""" + # Clear existing markers + self._im_server.clear() + + for entry in nodes: + node = entry['node'] + self._create_edit_marker(node) + + # Apply the right-click context menu to every marker, then + # re-register type-specific callbacks. MenuHandler.apply() + # overwrites the default (catch-all) callback, so BUTTON_CLICK + # and POSE_UPDATE would be silently dropped without this step. + for entry in nodes: + name = entry['node']['name'] + self._menu_handler.apply(self._im_server, name) + self._im_server.setCallback( + name, self._im_feedback, + InteractiveMarkerFeedback.BUTTON_CLICK, + ) + self._im_server.setCallback( + name, self._im_feedback, + InteractiveMarkerFeedback.POSE_UPDATE, + ) + + self._im_server.applyChanges() + + def _create_edit_marker(self, node: dict): + """Insert one interactive marker for *node*.""" + scale = self.marker_scale + + im = InteractiveMarker() + im.header.frame_id = node.get('parent_frame', 'map') + im.name = node['name'] + im.description = '' + + pose = _node2pose(node['pose']) + im.pose = pose + + # ── Visible sphere ────────────────────────────────────── + sphere = Marker() + sphere.type = Marker.SPHERE + sphere.scale.x = scale + sphere.scale.y = scale + sphere.scale.z = scale + sphere.color.r = 0.0 + sphere.color.g = 0.5 + sphere.color.b = 1.0 + sphere.color.a = 0.8 + + vis_ctrl = InteractiveMarkerControl() + vis_ctrl.always_visible = True + vis_ctrl.markers.append(sphere) + vis_ctrl.interaction_mode = InteractiveMarkerControl.BUTTON + vis_ctrl.orientation.w = 1.0 + vis_ctrl.orientation.y = 1.0 # XY plane + im.controls.append(vis_ctrl) + + # ── Move X ────────────────────────────────────────────── + ctrl = InteractiveMarkerControl() + ctrl.orientation.w = 1.0 + ctrl.orientation.x = 1.0 + ctrl.name = 'move_x' + ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS + im.controls.append(ctrl) + + # ── Move Y ────────────────────────────────────────────── + ctrl = InteractiveMarkerControl() + ctrl.orientation.w = 1.0 + ctrl.orientation.z = 1.0 + ctrl.name = 'move_y' + ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS + im.controls.append(ctrl) + + # ── Rotate Z (yaw) ───────────────────────────────────── + ctrl = InteractiveMarkerControl() + ctrl.orientation.w = 1.0 + ctrl.orientation.y = 1.0 + ctrl.name = 'rotate_z' + ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS + im.controls.append(ctrl) + + # ── Arrow showing orientation ─────────────────────────── + arrow = Marker() + arrow.type = Marker.ARROW + arrow.scale.x = scale * 1.5 + arrow.scale.y = scale * 0.2 + arrow.scale.z = scale * 0.2 + arrow.color.r = 1.0 + arrow.color.a = 1.0 + + arrow_ctrl = InteractiveMarkerControl() + arrow_ctrl.always_visible = True + arrow_ctrl.markers.append(arrow) + im.controls.append(arrow_ctrl) + + # ── Text label ────────────────────────────────────────── + txt = Marker() + txt.type = Marker.TEXT_VIEW_FACING + txt.text = node['name'] + txt.scale.z = scale * 0.5 + txt.color.r = 1.0 + txt.color.g = 1.0 + txt.color.b = 1.0 + txt.color.a = 1.0 + txt.pose.position.z = scale * 0.8 + + txt_ctrl = InteractiveMarkerControl() + txt_ctrl.always_visible = True + txt_ctrl.markers.append(txt) + im.controls.append(txt_ctrl) + + self._im_server.insert(im) + self._im_server.setCallback(im.name, self._im_feedback) + + def _im_feedback(self, feedback: InteractiveMarkerFeedback): + """Handle interactive-marker drag / rotate / click events.""" + # Let MenuHandler process right-click menu selections + if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: + return + + # Left-click on node sphere → navigate to that node + if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: + node_name = feedback.marker_name + self.get_logger().info( + f'Node clicked → navigating to {node_name}' + ) + # Cancel any active navigation before sending new goal + if self._goto_goal_handle is not None: + self._cancel_navigation() + self._compute_and_highlight_route(node_name) + self._send_goto_goal(node_name) + self._im_server.applyChanges() + return + + if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE: + self._im_server.applyChanges() + return + + name = feedback.marker_name + for entry in self.tmap.get('nodes', []): + if entry['node']['name'] != name: + continue + + node = entry['node'] + node['pose']['position']['x'] = float( + feedback.pose.position.x + ) + node['pose']['position']['y'] = float( + feedback.pose.position.y + ) + node['pose']['position']['z'] = 0.0 + + euler = tf_transformations.euler_from_quaternion([ + feedback.pose.orientation.x, + feedback.pose.orientation.y, + feedback.pose.orientation.z, + feedback.pose.orientation.w, + ]) + yaw = euler[2] + quat = tf_transformations.quaternion_from_euler(0.0, 0.0, yaw) + node['pose']['orientation']['x'] = float(quat[0]) + node['pose']['orientation']['y'] = float(quat[1]) + node['pose']['orientation']['z'] = float(quat[2]) + node['pose']['orientation']['w'] = float(quat[3]) + + self._map_dirty = True + yaw_deg = math.degrees(yaw) + self.get_logger().debug( + f'{name}: pos=({feedback.pose.position.x:.2f}, ' + f'{feedback.pose.position.y:.2f}), yaw={yaw_deg:.1f}°' + ) + break + + # Refresh the static markers (edges, zones) to reflect the move + self._rebuild_static_markers() + self._im_server.applyChanges() + + # Schedule a debounced republish so downstream nodes + # (navigation, localisation) pick up the change. + self._schedule_republish() + + def _schedule_republish(self): + """Debounce map republish: wait 0.5 s after the last drag event.""" + if self._republish_timer is not None: + self._republish_timer.cancel() + self.destroy_timer(self._republish_timer) + self._republish_timer = self.create_timer( + 0.5, self._deferred_republish, + callback_group=self._cb_group, + ) + + def _deferred_republish(self): + """Republish map, rebuild graph, and save to temp folder.""" + # One-shot: cancel the timer immediately + if self._republish_timer is not None: + self._republish_timer.cancel() + self.destroy_timer(self._republish_timer) + self._republish_timer = None + + # Rebuild the NetworkX graph so route planning uses new positions + self._graph = build_graph_from_tmap( + self.tmap, logger=self.get_logger(), + ) + + # Publish updated map to topic so all subscribers get it + self._publish_map_topic() + self.get_logger().info( + 'Map republished after node position update' + ) + + # Save to temp folder + self._save_to_temp() + + def _save_to_temp(self): + """Save the current map to a temp folder for recovery.""" + if self.tmap is None: + return + map_name = self.tmap.get('pointset', 'unknown_map') + temp_path = os.path.join( + self._temp_map_dir, + f'{map_name}.tmap2.yaml', + ) + try: + with open(temp_path, 'w') as fh: + yaml.dump( + self.tmap, + fh, + default_flow_style=False, + sort_keys=False, + ) + self.get_logger().info(f'Map saved to {temp_path}') + except Exception as exc: + self.get_logger().error(f'Failed to save temp map: {exc}') + + def _rebuild_static_markers(self): + """Re-publish static MarkerArray only (no interactive markers).""" + if self.tmap is None: + return + + nodes = self.tmap.get('nodes', []) + marker_array = MarkerArray() + actions_seen: list = [] + idn = 0 + + for entry in nodes: + node = entry['node'] + for edge in node.get('edges', []): + act = edge.get('action', '') + if act and act not in actions_seen: + actions_seen.append(act) + + marker_array.markers.append(self._mk_node(node, idn)) + idn += 1 + marker_array.markers.append(self._mk_name(node, idn)) + idn += 1 + if node.get('verts'): + marker_array.markers.append(self._mk_zone(node, idn)) + idn += 1 + for edge in node.get('edges', []): + m = self._mk_edge(node, edge, actions_seen) + if m is not None: + m.id = idn + marker_array.markers.append(m) + idn += 1 + + for row, action_name in enumerate(actions_seen): + marker_array.markers.append( + self._mk_legend(action_name, row, actions_seen, idn) + ) + idn += 1 + + self.map_marker_pub.publish(marker_array) + + # ────────────────────────────────────────────────────────────── + # Marker factory helpers + # ────────────────────────────────────────────────────────────── + def _mk_node(self, node: dict, idn: int) -> Marker: + m = Marker() + m.id = idn + m.header.frame_id = node.get('parent_frame', 'map') + m.type = Marker.SPHERE + s = self.marker_scale + m.scale.x = s * 0.4 + m.scale.y = s * 0.4 + m.scale.z = s * 0.4 + m.color.a = 0.4 + m.color.r = 0.2 + m.color.g = 0.2 + m.color.b = 0.7 + m.pose = _node2pose(node['pose']) + m.pose.position.z += 0.1 + m.ns = '/nodes' + return m + + def _mk_name(self, node: dict, idn: int) -> Marker: + m = Marker() + m.id = idn + m.header.frame_id = node.get('parent_frame', 'map') + m.type = Marker.TEXT_VIEW_FACING + m.text = node['name'] + m.pose = _node2pose(node['pose']) + m.pose.position.z += 0.25 + m.scale.z = self.marker_scale * 0.24 + m.color.a = 0.9 + m.color.r = 0.3 + m.color.g = 0.3 + m.color.b = 0.3 + m.ns = '/names' + return m + + def _mk_zone(self, node: dict, idn: int) -> Marker: + m = Marker() + m.id = idn + m.header.frame_id = node.get('parent_frame', 'map') + m.type = Marker.LINE_STRIP + m.pose.orientation.w = 1.0 + m.scale.x = self.marker_scale * 0.06 + m.color.a = 0.8 + m.color.r = 0.7 + m.color.g = 0.1 + m.color.b = 0.2 + + px = float(node['pose']['position']['x']) + py = float(node['pose']['position']['y']) + pz = float(node['pose']['position']['z']) + + for v in node['verts']: + pt = Point() + pt.x = px + float(v['x']) + pt.y = py + float(v['y']) + pt.z = pz + m.points.append(pt) + + # Close the polygon + first = node['verts'][0] + pt = Point() + pt.x = px + float(first['x']) + pt.y = py + float(first['y']) + pt.z = pz + m.points.append(pt) + m.ns = '/zones' + return m + + def _mk_edge( + self, node: dict, edge: dict, actions: list + ) -> Marker | None: + to_node = _get_node(self.tmap['nodes'], edge['node']) + if to_node is None: + self.get_logger().warn( + f"Edge target node '{edge['node']}' not found" + ) + return None + + action = edge.get('action', '') + col_idx = actions.index(action) if action in actions else 0 + col = _colour(col_idx) + + m = Marker() + m.header.frame_id = node.get('parent_frame', 'map') + m.type = Marker.LINE_LIST + + v1 = _node2pose(node['pose']).position + v1.z += 0.1 + v2 = _node2pose(to_node['pose']).position + v2.z += 0.1 + + m.pose.orientation.w = 1.0 + m.scale.x = self.marker_scale * 0.06 + m.color.a = 0.5 + m.color.r = float(col[0]) + m.color.g = float(col[1]) + m.color.b = float(col[2]) + m.points.append(v1) + m.points.append(v2) + m.ns = '/edges' + return m + + def _mk_legend( + self, action: str, row: int, actions: list, idn: int + ) -> Marker: + col_idx = actions.index(action) if action in actions else 0 + col = _colour(col_idx) + m = Marker() + m.id = idn + m.header.frame_id = 'map' + m.type = Marker.TEXT_VIEW_FACING + m.text = action + m.pose.position.x = 1.0 + m.pose.position.y = 0.18 * row + m.pose.position.z = 0.2 + m.pose.orientation.w = 1.0 + m.scale.z = self.marker_scale * 0.3 + m.color.a = 1.0 + m.color.r = float(col[0]) + m.color.g = float(col[1]) + m.color.b = float(col[2]) + m.ns = '/legend' + return m + + +# ══════════════════════════════════════════════════════════════════ +# Entry-point +# ══════════════════════════════════════════════════════════════════ +def main(args=None): + rclpy.init(args=args) + node = TopologicalMapVisualiser() + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.get_logger().info('Shutting down topological map visualiser') + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/topological_navigation_visual/topological_navigation_visual/scripts/topological_visual.py b/topological_navigation_visual/topological_navigation_visual/scripts/topological_visual.py new file mode 100644 index 00000000..facb0bae --- /dev/null +++ b/topological_navigation_visual/topological_navigation_visual/scripts/topological_visual.py @@ -0,0 +1,249 @@ +#!/usr/bin/env python3 +"""Route and occupancy visualisation nodes. + +Provides two ROS 2 nodes that run together in a single process: + +- **RouteVisualiserNode** — subscribes to ``topological_navigation/Route`` + and publishes arrow markers between consecutive waypoints on + ``topological_route_visualisation``. +- **OccupancyVisualiserNode** — subscribes to + ``/topological_navigation/occupied_node`` and publishes sphere markers + on ``/topological_navigation/visual/occupied_node``. + +Both require a ``tmap`` parameter pointing to a ``.tmap2.yaml`` file. +""" + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSProfile, DurabilityPolicy + +import yaml +import numpy as np +import tf_transformations + +from geometry_msgs.msg import Point +from visualization_msgs.msg import Marker, MarkerArray +from topological_navigation_msgs.msg import ( + TopologicalOccupiedNode, + TopologicalRoute, +) + + +# ────────────────────────────────────────────────────────────────── +# Utils +# ────────────────────────────────────────────────────────────────── +def load_waypoints_from_tmap(file_path): + """Load and pre-transform waypoint vertices into the map frame.""" + try: + with open(file_path, 'r') as fh: + tmap_data = yaml.safe_load(fh) + + waypoints = {} + for entry in tmap_data.get('nodes', []): + try: + node = entry['node'] + name = node['name'] + pos = node['pose']['position'] + ori = node['pose']['orientation'] + + t_mat = tf_transformations.translation_matrix( + [pos['x'], pos['y'], pos['z']] + ) + r_mat = tf_transformations.quaternion_matrix( + [ori['x'], ori['y'], ori['z'], ori['w']] + ) + transform = np.dot(t_mat, r_mat) + + verts = [] + for v in node.get('verts', []): + local = np.array([v['x'], v['y'], 0.0, 1.0]) + world = np.dot(transform, local) + verts.append((world[0], world[1])) + + waypoints[name] = { + 'position': (pos['x'], pos['y'], pos['z']), + 'orientation': ( + ori['x'], ori['y'], ori['z'], ori['w'] + ), + 'verts': verts, + } + except KeyError: + continue + return waypoints + + except Exception: + return {} + + +# ══════════════════════════════════════════════════════════════════ +# Route visualiser +# ══════════════════════════════════════════════════════════════════ +class RouteVisualiserNode(Node): + """Subscribes to a route topic and publishes arrow markers.""" + + def __init__(self): + super().__init__('route_visualiser') + self.declare_parameter('tmap', rclpy.Parameter.Type.STRING) + tmap_path = ( + self.get_parameter('tmap').get_parameter_value().string_value + ) + self.tmap = load_waypoints_from_tmap(tmap_path) + + qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ) + self.pub = self.create_publisher( + MarkerArray, 'topological_route_visualisation', qos_profile=qos + ) + self.create_subscription( + TopologicalRoute, + 'topological_navigation/Route', + self._route_cb, + qos_profile=qos, + ) + self.get_logger().info('Route visualiser started') + + def _route_cb(self, msg: TopologicalRoute): + self._clear() + arr = MarkerArray() + idn = 0 + if self.tmap: + for i in range(1, len(msg.nodes)): + arr.markers.append( + self._arrow(msg.nodes[i - 1], msg.nodes[i], idn) + ) + idn += 1 + self.pub.publish(arr) + + def _clear(self): + arr = MarkerArray() + m = Marker() + m.action = Marker.DELETEALL + arr.markers.append(m) + self.pub.publish(arr) + + def _arrow(self, origin: str, end: str, idn: int) -> Marker: + m = Marker() + m.id = idn + m.header.frame_id = 'map' + m.type = Marker.ARROW + o = self.tmap[origin] + e = self.tmap[end] + v1 = Point( + x=o['position'][0], y=o['position'][1], z=o['position'][2] + 0.25 + ) + v2 = Point( + x=e['position'][0], y=e['position'][1], z=e['position'][2] + 0.25 + ) + m.pose.orientation.w = 1.0 + m.scale.x = 0.08 + m.scale.y = 0.12 + m.scale.z = 0.2 + m.color.a = 1.0 + m.color.r = 0.33 + m.color.g = 0.99 + m.color.b = 0.55 + m.points.append(v1) + m.points.append(v2) + m.ns = '/route_path' + return m + + +# ══════════════════════════════════════════════════════════════════ +# Occupancy visualiser +# ══════════════════════════════════════════════════════════════════ +class OccupancyVisualiserNode(Node): + """Subscribes to occupied-node topics and publishes sphere markers.""" + + def __init__(self): + super().__init__('occupancy_visualiser') + self.declare_parameter('tmap', rclpy.Parameter.Type.STRING) + tmap_path = ( + self.get_parameter('tmap').get_parameter_value().string_value + ) + self.tmap = load_waypoints_from_tmap(tmap_path) + self.last_count = 0 + + qos = QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ) + self.pub = self.create_publisher( + MarkerArray, + '/topological_navigation/visual/occupied_node', + qos, + ) + self.create_subscription( + TopologicalOccupiedNode, + '/topological_navigation/occupied_node', + self._cb, + 10, + ) + self.get_logger().info('Occupancy visualiser started') + + def _cb(self, msg: TopologicalOccupiedNode): + names = msg.nodes if msg.nodes else [] + arr = MarkerArray() + + for i, wp in enumerate(names): + data = self.tmap.get(wp) + if not data: + continue + m = Marker() + m.header.frame_id = 'map' + m.header.stamp = self.get_clock().now().to_msg() + m.ns = 'occupied_waypoints' + m.id = i + m.type = Marker.SPHERE + m.action = Marker.ADD + m.pose.position.x = data['position'][0] + m.pose.position.y = data['position'][1] + m.pose.position.z = 0.15 + m.pose.orientation.w = 1.0 + m.scale.x = 1.0 + m.scale.y = 1.0 + m.scale.z = 0.1 + m.color.r = 1.0 + m.color.a = 0.8 + arr.markers.append(m) + + # Delete stale markers + for i in range(len(names), self.last_count): + m = Marker() + m.header.frame_id = 'map' + m.ns = 'occupied_waypoints' + m.id = i + m.action = Marker.DELETE + arr.markers.append(m) + + self.pub.publish(arr) + self.last_count = len(names) + + +# ══════════════════════════════════════════════════════════════════ +# Entry-point +# ══════════════════════════════════════════════════════════════════ +def main(args=None): + rclpy.init(args=args) + + route_node = RouteVisualiserNode() + occupancy_node = OccupancyVisualiserNode() + + executor = MultiThreadedExecutor() + executor.add_node(route_node) + executor.add_node(occupancy_node) + + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + executor.shutdown() + route_node.destroy_node() + occupancy_node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/topological_rviz_tools/CHANGELOG.rst b/topological_rviz_tools/CHANGELOG.rst deleted file mode 100644 index caac4b24..00000000 --- a/topological_rviz_tools/CHANGELOG.rst +++ /dev/null @@ -1,182 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package topological_rviz_tools -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.4.0 (2022-01-25) ------------------- -* Merge pull request `#111 `_ from adambinch/remove_strands_dependencies - Removing strands navigation dependencies from topological navigation. -* update -* strands dependencies removed from topological_rviz_tools -* Merge branch 'master' of github.com:LCAS/topological_navigation into francescodelduchetto-toponav2-restrictions -* Merge branch 'master' of github.com:LCAS/topological_navigation into toponav2_launch -* Merge branch 'master' of github.com:LCAS/topological_navigation into faster_route_search2 -* Merge branch 'toponav2-devel-restrictions' of github.com:francescodelduchetto/topological_navigation into toponav2-devel -* Merge branch 'master' of https://github.com/adambinch/topological_navigation into adam-master -* Contributors: Adam Binch, adambinch, francescodelduchetto - -2.3.0 (2021-07-15) ------------------- -* Merge branch 'master' of github.com:LCAS/topological_navigation into tmap_to_tmap2 -* Merge pull request `#82 `_ from adambinch/fix_conflicts - Fix conflicts -* Merge branch 'master' of github.com:LCAS/topological_navigation into fix_conflicts - # Conflicts: - # topological_navigation/scripts/execute_policy_server.py - # topological_navigation/scripts/navigation.py -* Contributors: Adam Binch, adambinch - -* Merge branch 'master' of github.com:LCAS/topological_navigation into tmap_to_tmap2 -* Merge pull request `#82 `_ from adambinch/fix_conflicts - Fix conflicts -* Merge branch 'master' of github.com:LCAS/topological_navigation into fix_conflicts - # Conflicts: - # topological_navigation/scripts/execute_policy_server.py - # topological_navigation/scripts/navigation.py -* Contributors: Adam Binch, adambinch - -2.1.0 (2020-04-20) ------------------- -* Merge pull request `#8 `_ from heuristicus/move_topo_rviz_tools - Move topo rviz tools from strands_navigation -* added install for topmap interface -* 1.2.0 -* updated changelogs -* 1.1.0 -* changelogs -* 1.0.8 -* updated changelogs -* 1.0.7 -* updated changelogs -* added install for extra dirs (`#365 `_) -* Standalone easier to run on strands robots - Would have to run with rviz=false and then run rviz on the main PC if the - database is on on of the side PCs. With this change can run the database on the - side PC and then run this with launch_db:=false to not have to run rviz - separately -* 1.0.6 -* updated changelogs -* 1.0.5 -* updated changelogs -* update of absolute/relative topic names for multi-robot setup -* more descriptive names for topics displayed in rviz -* Can now place nodes with RMB to stop automatic edge creation - Fix deletion dialogue, edges and tags were swapped -* Adding waiting for services -* Update topological_edge_tool.cpp -* Adding waiting for the add_node service -* Update strands_rviz_topmap.launch -* 1.0.4 -* updated changelogs -* set version to 1.0.3 as the rest of repository -* add standalone flag for when navigation is running -* Topmap editor (`#344 `_) - * Initial commit - * initial slightly modified clone of plantflag tutorial - * Add slightly modified clone of rviz views panel - * Plugin and tool now load properly - * Shuffling things around, taking code from existing rviz property - Looks like properties are the best way to look at things from rviz. Kind of a - rudimentary approximation of messages. Not sure of the best way to transfer data - back to the map once we want to change it, but that will come once we can view - the map data in the panel. - * finally have compiling base classes to modify - * renamed classes - * fix function call parens - * panel displays some default values - * More work on getting nodes and edges represented in the panel - * Rearrange property orderings - Property ordering is - nodecontroller - ^ - | - nodeproperty (per node) - ^ ^ - / \ - poseproperty edgecontroller - ^ - | - edgeproperty (per edge) - Controllers are intended as a way of adding nodes and edges from the topological - map while keeping everything contained somewhat nicely in its own class - * Finally linked to topmap topic - Still not able to see edges. They are being added, but not displayed in the - panel. Something to do with the edge controller? - * Now properly displays edges in each node - EdgeController was missing the base class initialisation from its constructor. - * slots for node updates connected - * Start on python interface node to allow access to topmap modification - Add custom rviz config to use for topmap modification, plus launch file - * update launch to allow proper viewing of topmap - * Now correctly refreshes on change, constructors filled in - Very inefficient - repopulates all properties when anything in the map changes. - Constructors all use delete. This should be changed later to not use pointers. - Currently has a segfault when a pose property is modified, think it's because - the current node is deleted and there is still something else referencing it. - * First attempt at not replacing all properties when one is modified - Connected slot in node property to node controller to notify on modification of - xy threshold and yaw etc, need to do the same for pose and edge properties and - see if knowing which node was changed can help fix the problem. - * finally able to modify poses without crash - * fix only partial deletion of properties on new map - * fix weird space-colon - * Easier translational movement of waypoints, generic node field updater - Moving the waypoints that are displayed in the topological map in rviz is now - easier - just uses 2D planar motion as opposed to multiple handles for the x and - y dimensions. - Added a function which calls into the database to update any property of a node. - * linked name changing - * Prevent possibility of node name duplication - Also added resets to the previous value of the property when a service call - fails, so that the properties reflect the actual values. - * Now possible to add and remove nodes via rviz - * can now use the edge tool to add edges - * Fixed not loading map after update, correctly updates edges on node rename - This should really not be the file being used - it seems like the one in util is - used to change things and as such is more up to date. - * Adding nodes now done via tool - Click tool, then click on map to add node. Add shortcuts for edge tool (e), and - node tool (n). - * Fix edge property name, bidirectional with left click - Also fix node tool disabling - * use dummy navigation - * remove unnecessary if - * Show arrow when creating edge, disallow edges to self - * rename package and namespace - * updated launch file and rviz config - * update import in interface script, add db path to args - * update function for edge action and top_vel - * allow edge property editing for action and top_vel - * add localise by topic to node prop (read only) - * add deprecation warnings to topological_map.py - should use manager.py instead - * start on work to make manager services more useful for modifying map - * initial work on tags for nodes - still needs work in the manager to retrieve tags for specific nodes - * add callback for getting tags for a specific node - * hide tag controller property if node has no tags - * partial switch to the using manager, updating and adding tags - * tag addition and modification, move to manager.py in progress - started moving topological map update to the panel rather than node controller - so we can decide whether to update or not more easily - * fix message fields and add messages to generation - * redirect most calls to manager rather than interface - Removed/moved messages to strands navigation msgs so that the manager can - perform all required tasks - Map updates triggered in the topological map panel as opposed to at the node - level - * move to subdirectory in preparation for PR - * small script to insert empty map into a database - * more sensible paths - * add edge removal service - * allow removal of tags and edges from panel - * add confirmation dialog for remove button - * add readme - * little more in the readme - * nodes in panel sorted, fix occasional segfault due iterate/delete - * change callbacks so that functions can be called without service - * update edge and update tolerance now pass both params - * Readme mentions standalone flag - * add note about using tools in arbitrary rviz session - * try and stop compilation issue with AddEdge not being found - * add dependency on the project messages to library generation -* Contributors: Bruno Lacerda, Jaime Pulido Fentanes, Jenkins, LCAS build farm, Lenka Mudrova, Marc Hanheide, Michal Staniaszek, Nick Hawes diff --git a/topological_rviz_tools/CMakeLists.txt b/topological_rviz_tools/CMakeLists.txt deleted file mode 100644 index a2b48f9a..00000000 --- a/topological_rviz_tools/CMakeLists.txt +++ /dev/null @@ -1,128 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(topological_rviz_tools) - - -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -Wall -Wno-deprecated-declarations") - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(topological_navigation_msgs REQUIRED) - -# Define dependencies for RViz Plugins -find_package(rviz_common REQUIRED) -find_package(rviz_default_plugins REQUIRED) -find_package(rviz_ogre_vendor REQUIRED) -find_package(rviz_rendering REQUIRED) -find_package(Qt5 REQUIRED COMPONENTS Widgets Core) -find_package(pluginlib REQUIRED) - -add_definitions(-DQT_NO_KEYWORDS) -set(CMAKE_AUTOMOC ON) - -include_directories( - include - ${rclcpp_INCLUDE_DIRS} - ${rviz_common_INCLUDE_DIRS} - ${Qt5Core_INCLUDE_DIRS} - ${Qt5Widgets_INCLUDE_DIRS} -) - -set(CMAKE_INCLUDE_CURRENT_DIR ON) - -set(topological_rviz_tools_headers_to_moc - include/${PROJECT_NAME}/edge_controller.hpp - include/${PROJECT_NAME}/node_controller.hpp - include/${PROJECT_NAME}/node_property.hpp - include/${PROJECT_NAME}/pose_property.hpp - include/${PROJECT_NAME}/tag_controller.hpp - include/${PROJECT_NAME}/tag_property.hpp - include/${PROJECT_NAME}/topmap_manager.hpp - include/${PROJECT_NAME}/topological_edge_tool.hpp - include/${PROJECT_NAME}/topological_map_panel.hpp - include/${PROJECT_NAME}/topological_node_tool.hpp -) - -foreach(header "${vision_msgs_rviz_plugins_headers_to_moc}") - qt5_wrap_cpp(topological_rviz_tools_headers_to_moc "${header}") -endforeach() - -set(topological_rviz_tools_source_files - src/topological_edge_tool.cpp - src/topological_node_tool.cpp - src/topological_map_panel.cpp - src/node_controller.cpp - src/topmap_manager.cpp - src/node_property.cpp - src/edge_property.cpp - src/pose_property.cpp - src/edge_controller.cpp - src/tag_controller.cpp - src/tag_property.cpp -) - -add_library(${PROJECT_NAME} SHARED ${topological_rviz_tools_source_files} - ${topological_rviz_tools_headers_to_moc}) - -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - ${Qt5Widgets_INCLUDE_DIRS} ) - -target_link_libraries(${PROJECT_NAME} PUBLIC - rviz_ogre_vendor::OgreMain - rviz_ogre_vendor::OgreOverlay - rviz_common::rviz_common -) - -target_compile_definitions(${PROJECT_NAME} - PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") - - -ament_target_dependencies(${PROJECT_NAME} - PUBLIC - rclcpp - rclcpp_components - rviz_common - rviz_default_plugins - rviz_ogre_vendor - rviz_rendering - topological_navigation_msgs - pluginlib) - -pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) - -ament_export_include_directories("include/${PROJECT_NAME}") - - -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) - -ament_export_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_components - rviz_common - rviz_default_plugins - rviz_ogre_vendor - rviz_rendering - topological_navigation_msgs - pluginlib) - -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - ) - - install( - DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" - DESTINATION "share/${PROJECT_NAME}" - ) - - install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} - ) - - -ament_package() \ No newline at end of file diff --git a/topological_rviz_tools/LICENSE b/topological_rviz_tools/LICENSE deleted file mode 100644 index 261eeb9e..00000000 --- a/topological_rviz_tools/LICENSE +++ /dev/null @@ -1,201 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/topological_rviz_tools/README.md b/topological_rviz_tools/README.md deleted file mode 100644 index 6cfdf5ca..00000000 --- a/topological_rviz_tools/README.md +++ /dev/null @@ -1,79 +0,0 @@ -# topological_rviz_tools -Rviz tool for creating a STRANDS topological map - -## Usage - -This rviz toolset can be launched using - -```sh -roslaunch topological_rviz_tools strands_rviz_topmap.launch map:=/path/to/map.yaml topmap:=topmap_pointset db_path:=/path/to/db standalone:=true -``` - -`map` specifies the `yaml` file for the map you are using, and `topmap` the -corresponding pointset that exists in the database. `db_path` is used to point -to the database you want to use. If `standalone` is true, everything needed will -be run automatically. If false, it is assumed that other parts of the strands -system are running (navigation and mongodb_store in particular), and only run -a few additional things. - -You can also add the node tool, edge tool and topological map panel to rviz -individually by using the buttons for adding tools or panels. You will need to -run the above launch file with `standalone=false` for this to work correctly. - -When you launch with a database which contains a topological map, you should see -something like the following: - -![Annotated rviz_tools image](images/00_annotated.png) - -You can move nodes around by clicking the arrow at the centre of topological -nodes and dragging. The ring around the node allows you to change the -orientation of the node. You can delete edges using the red arrows in the middle -of edges. - -The following sections give a little more detail about to tools and panel -labelled in the image above. - -### 1. The edge tool - -Use this tool to create edges between nodes. Left click to select the start point of -the edge, then click again to create an edge. The edge will be created between -the nodes closest to the two clicks, but only if there are nodes within some -distance of the clicks. Left clicking will create a bidirectional edge, whereas -right clicking will create an edge only from the node closest to the first click -to the second one. This tool stays on until you press escape. - -The shortcut is `e`. - -### 2. The node tool - -This tool allows you to add nodes to the topological map. Click the tool and -then click on the map to add a node in that location. Edges will automatically -be added between the new node and any nodes in close proximity. - -The shortcut is `n`. - -### 3. Add tag button - -This button allows you to add tags to nodes. You can select multiple nodes, and -the tag you enter in the dialog box will be added to all of them. - -### 4. Remove button - -With this button, you can remove edges, tags, and nodes from the topological -map. You can select multiple elements and they will all be removed at once. - -### 5. Topological map panel - -You can see all the elements of the topological map here. You can edit the -following elements: - -- Node name -- Node pose -- Node tags -- Node yaw tolerance -- Node xy tolerance -- Edge action -- Edge velocity - -Ctrl-click allows you to select multiple distinct elements. Shift-click will -select elements between the previously selected element and the current one. diff --git a/topological_rviz_tools/conf/topological_rviz_tools.rviz b/topological_rviz_tools/conf/topological_rviz_tools.rviz deleted file mode 100644 index 4d1b1bf7..00000000 --- a/topological_rviz_tools/conf/topological_rviz_tools.rviz +++ /dev/null @@ -1,166 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 1365 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: topological_rviz_tools/TopologicalMap - Expanded: ~ - Name: TopologicalMap - Splitter Ratio: 0.5 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 0.699999988 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /map - Unreliable: false - Use Timestamp: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /edge_tool_marker - Name: Edge Tool markers - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: true - Name: Delete edges - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /topological_map_edges/update - Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: true - Name: Move Nodes - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /topological_map_markers/update - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /topological_map_visualisation - Name: Edges and node influence - Namespaces: - /edges: true - /legend: true - /nodes: true - /zones: true - Queue Size: 100 - Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: false - Name: Edit Influence Zones - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /topological_map_zones/update - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: topological_rviz_tools/TopmapEdge - - Class: topological_rviz_tools/TopmapNode - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 76.5415802 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 17.6023445 - Y: 18.1025562 - Z: 1.19333327 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 1.56979632 - Target Frame: - Value: Orbit (rviz) - Yaw: 1.58040023 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1752 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000026500000607fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000040000006070000010000fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000019a00000607fc0200000004fb0000001c0054006f0070006f006c006f0067006900630061006c004d00610070010000004000000607000000aa00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000378000000e800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000abe00000058fc0100000002fb0000000800540069006d0065010000000000000abe0000057800fffffffb0000000800540069006d00650100000000000004500000000000000000000006a70000060700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - TopologicalMap: - collapsed: false - Views: - collapsed: false - Width: 2750 - X: 130 - Y: 48 diff --git a/topological_rviz_tools/images/00_annotated.png b/topological_rviz_tools/images/00_annotated.png deleted file mode 100644 index 876b483a..00000000 Binary files a/topological_rviz_tools/images/00_annotated.png and /dev/null differ diff --git a/topological_rviz_tools/images/00_annotated.xcf b/topological_rviz_tools/images/00_annotated.xcf deleted file mode 100644 index 2146e5b1..00000000 Binary files a/topological_rviz_tools/images/00_annotated.xcf and /dev/null differ diff --git a/topological_rviz_tools/include/topological_rviz_tools/edge_controller.hpp b/topological_rviz_tools/include/topological_rviz_tools/edge_controller.hpp deleted file mode 100644 index 5b5f9edb..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/edge_controller.hpp +++ /dev/null @@ -1,79 +0,0 @@ -#ifndef TOPMAP_EDGE_CONTROLLER_H -#define TOPMAP_EDGE_CONTROLLER_H - -#include - -#include -#include -#include -#include - -#include "rviz_common/config.hpp" -#include "rviz_common/display_context.hpp" -#include "rviz_common/frame_manager_iface.hpp" -#include "rviz_common/load_resource.hpp" -#include "rviz_rendering/render_system.hpp" -#include "rviz_common/properties/bool_property.hpp" -#include "rviz_common/properties/enum_property.hpp" -#include "rviz_common/properties/float_property.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/render_panel.hpp" -#include "rviz_common/interaction/selection_manager.hpp" -#include "rviz_common/viewport_mouse_event.hpp" -#include "rviz_common/window_manager_interface.hpp" -#include "topological_navigation_msgs/msg/edge.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "edge_property.hpp" - -class QKeyEvent; - -namespace topological_rviz_tools { -class EdgeController: public rviz_common::properties::Property -{ -Q_OBJECT -public: - EdgeController(const QString& name = QString(), - const std::vector& default_values = std::vector(), - const QString& description = QString(), - rviz_common::properties::Property* parent = 0, - const char *changed_slot = 0, - QObject* receiver = 0); - virtual ~EdgeController(); - - void initialize(); - - /** @brief Subclasses should call this whenever a change is made which would change the results of toString(). */ - void emitConfigChanged(); - - // required by something that initialises this class - QString formatClassId(const QString& class_id); - - /** @brief Return the class identifier which was used to create this - * instance. This version just returns whatever was set with - * setClassId(). */ - virtual QString getClassId() const { return class_id_; } - - /** @brief Set the class identifier used to create this instance. - * Typically this will be set by the factory object which created it. */ - virtual void setClassId( const QString& class_id ) { class_id_ = class_id; } - - virtual void load(const rviz_common::Config& config); - virtual void save(rviz_common::Config config) const; - bool addEdge(const topological_navigation_msgs::msg::Edge& edge); -Q_SIGNALS: - void configChanged(); - -protected: - /** @brief Do subclass-specific initialization. Called by - * EdgeController::initialize after context_ and camera_ are set. - * Default implementation does nothing. */ - virtual void onInitialize() {} -private: - QString class_id_; - std::vector edges_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; -}; - -} // end namespace topological_rviz_tools - -#endif // TOPMAP_EDGE_CONTROLLER_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/edge_property.hpp b/topological_rviz_tools/include/topological_rviz_tools/edge_property.hpp deleted file mode 100644 index ef91b883..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/edge_property.hpp +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef EDGE_PROPERTY_H -#define EDGE_PROPERTY_H - -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/properties/string_property.hpp" -#include "rviz_common/properties/float_property.hpp" -#include "topological_navigation_msgs/msg/edge.hpp" -#include "topological_navigation_msgs/srv/update_edge_legacy.hpp" - -namespace topological_rviz_tools -{ - -/** @brief Property specialized to provide getter for booleans. */ -class EdgeProperty: public rviz_common::properties::Property -{ -Q_OBJECT -public: - EdgeProperty(const QString& name = QString(), - const topological_navigation_msgs::msg::Edge& default_value = topological_navigation_msgs::msg::Edge(), - const QString& description = QString(), - Property* parent = 0, - const char *changed_slot = 0, - QObject* receiver = 0); - - virtual ~EdgeProperty(); - - std::string getEdgeId() { return edge_id_->getString().toStdString(); } -public Q_SLOTS: - void updateAction(); - void updateTopvel(); - -Q_SIGNALS: - void edgeModified(); - -private: - const topological_navigation_msgs::msg::Edge& edge_; - - // keep track of changing values to ensure that they are redisplayed correctly - // when we fail to update. - std::string action_value_; - float topvel_value_; - - bool reset_value_; - ros::ServiceClient edgeUpdate_; - - rviz_common::properties::StringProperty* edge_id_; - rviz_common::properties::StringProperty* node_; - rviz_common::properties::StringProperty* action_; - rviz_common::properties::StringProperty* map_2d_; - rviz_common::properties::FloatProperty* inflation_radius_; - rviz_common::properties::FloatProperty* top_vel_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; -}; - -} // end namespace topological_rviz_tools - -#endif // EDGE_PROPERTY_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/node_controller.hpp b/topological_rviz_tools/include/topological_rviz_tools/node_controller.hpp deleted file mode 100644 index 139a8a47..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/node_controller.hpp +++ /dev/null @@ -1,108 +0,0 @@ -#ifndef TOPMAP_NODE_CONTROLLER_H -#define TOPMAP_NODE_CONTROLLER_H - -#include -#include -#include - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "rviz_common/config.hpp" -#include "rviz_common/display_context.hpp" -#include "rviz_common/frame_manager_iface.hpp" -#include "rviz_common/load_resource.hpp" -#include "rviz_rendering/render_system.hpp" -#include "rviz_common/properties/bool_property.hpp" -#include "rviz_common/properties/enum_property.hpp" -#include "rviz_common/properties/float_property.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/render_panel.hpp" -#include "rviz_common/interaction/selection_manager.hpp" -#include "rviz_common/viewport_mouse_event.hpp" -#include "rviz_common/window_manager_interface.hpp" - -#include "topological_navigation_msgs/msg/topological_map.hpp" -#include "topological_navigation_msgs/msg/topological_node.hpp" - -#include "node_property.hpp" - -class QKeyEvent; - -namespace topological_rviz_tools { -class NodeController: public rviz_common::properties::Property -{ -Q_OBJECT -public: - NodeController(); - virtual ~NodeController(); - - /** @brief Do all setup that can't be done in the constructor. - * - * - * Calls onInitialize() just before returning. */ - void initialize(); - - /** @brief Subclasses should call this whenever a change is made which would change the results of toString(). */ - void emitConfigChanged(); - - virtual void load(const rviz_common::Config& config); - virtual void save(rviz_common::Config config) const; - - // required by something that initialises this class - QString formatClassId(const QString& class_id); - - /** @brief Return the class identifier which was used to create this - * instance. This version just returns whatever was set with - * setClassId(). */ - virtual QString getClassId() const { return class_id_; } - - /** @brief Set the class identifier used to create this instance. - * Typically this will be set by the factory object which created it. */ - virtual void setClassId( const QString& class_id ) { class_id_ = class_id; } - -Q_SIGNALS: - void configChanged(); - void childModified(); - -private Q_SLOTS: - void updateModifiedNode(Property* node); - -protected: - /** @brief Do subclass-specific initialization. Called by - * NodeController::initialize after context_ and camera_ are set. - * Default implementation does nothing. */ - virtual void onInitialize() {} - - void addModifiedChild(rviz_common::properties::Property* modifiedChild){ modifiedChildren_.push_back(modifiedChild); } - -private: - void topmapCallback(const topological_navigation_msgs::TopologicalMap::ConstPtr& msg); - - QString class_id_; - ros::Subscriber top_sub_; - std::vector modifiedChildren_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; - - /* bool sortNodes(topological_navigation_msgs::msg::TopologicalNode a, topological_navigation_msgs::msg::TopologicalNode b) { return a.name.compare(b.name) < 0; } */ - - struct NodeSorter { - - bool operator() (topological_navigation_msgs::msg::TopologicalNode a, - topological_navigation_msgs::msg::TopologicalNode b) { - std::string an = a.name; - std::string bn = b.name; - std::transform(an.begin(), an.end(), an.begin(), ::tolower); - std::transform(bn.begin(), bn.end(), bn.begin(), ::tolower); - return an.compare(bn) < 0; - } - } nodeSort; -}; - -} // end namespace topological_rviz_tools - -#endif // TOPMAP_NODE_CONTROLLER_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/node_property.hpp b/topological_rviz_tools/include/topological_rviz_tools/node_property.hpp deleted file mode 100644 index a8134fca..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/node_property.hpp +++ /dev/null @@ -1,75 +0,0 @@ -#ifndef NODE_PROPERTY_H -#define NODE_PROPERTY_H - -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/properties/string_property.hpp" -#include "rviz_common/properties/float_property.hpp" -#include "topological_navigation_msgs/msg/topological_node.hpp" -#include "topological_navigation_msgs/srv/get_node_tags.hpp" -#include "topological_navigation_msgs/srv/update_node_name.hpp" -#include "topological_navigation_msgs/srv/update_node_tolerance.hpp" -#include "pose_property.hpp" -#include "edge_controller.hpp" -#include "tag_controller.hpp" - -namespace topological_rviz_tools -{ - -class TagController; - -/** @brief Property specialized to provide getter for booleans. */ -class NodeProperty: public rviz_common::properties::Property -{ -Q_OBJECT -public: - NodeProperty(const QString& name = QString(), - const topological_navigation_msgs::msg::TopologicalNode& default_value = topological_navigation_msgs::msg::TopologicalNode(), - const QString& description = QString(), - Property* parent = 0, - const char *changed_slot = 0, - QObject* receiver = 0); - - virtual ~NodeProperty(); - - std::string getNodeName() { return name_; } - TagController* getTagController() { return tag_controller_; } -public Q_SLOTS: - void updateYawTolerance(); - void updateXYTolerance(); - void updateNodeName(); - void nodePropertyUpdated(); - -Q_SIGNALS: -void nodeModified(Property* node); - -private: - const topological_navigation_msgs::msg::TopologicalNode& node_; - - ros::ServiceClient nameUpdate_; - ros::ServiceClient toleranceUpdate_; - - rviz_common::properties::StringProperty* node_name_; - rviz_common::properties::StringProperty* map_; - rviz_common::properties::StringProperty* pointset_; - rviz_common::properties::StringProperty* localise_; - rviz_common::properties::FloatProperty* yaw_tolerance_; - rviz_common::properties::FloatProperty* xy_tolerance_; - // Store the name so that we can refer to it to change the node name in the - // map - once it changes in the property we won't know its previous value - // otherwise. - std::string name_; - // Also store the editable values, in case the service call fails. We then - // reset the property value to its original value. - float xy_tol_value_; - float yaw_tol_value_; - bool reset_value_; - PoseProperty* pose_; - EdgeController* edge_controller_; - TagController* tag_controller_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; -}; - -} // end namespace topological_rviz_tools - -#endif // NODE_PROPERTY_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/pose_property.hpp b/topological_rviz_tools/include/topological_rviz_tools/pose_property.hpp deleted file mode 100644 index e7831a45..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/pose_property.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef POSE_PROPERTY_H -#define POSE_PROPERTY_H - -#include "rclcpp/rclcpp.hpp" -#include "topological_navigation_msgs/srv/add_node.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/properties/float_property.hpp" -#include "rviz_common/properties/string_property.hpp" - -namespace topological_rviz_tools -{ - -/** @brief Property specialized to provide getter for booleans. */ -class PoseProperty: public rviz_common::properties::Property -{ - -public: - PoseProperty(const QString& name = QString(), - const geometry_msgs::msg::Pose& default_value = geometry_msgs::msg::Pose(), - const QString& description = QString(), - rviz_common::properties::Property* parent = 0, - const char *changed_slot = 0, - QObject* receiver = 0); - - virtual ~PoseProperty(); - -public Q_SLOTS: - void positionUpdated(); - -Q_SIGNALS: - void poseModified(); - -private: - const geometry_msgs::msg::Pose& pose_; - rviz_common::properties::StringProperty* orientation_; - rviz_common::properties::FloatProperty* orientation_w_; - rviz_common::properties::FloatProperty* orientation_x_; - rviz_common::properties::FloatProperty* orientation_y_; - rviz_common::properties::FloatProperty* orientation_z_; - rviz_common::properties::StringProperty* position_; - rviz_common::properties::FloatProperty* position_x_; - rviz_common::properties::FloatProperty* position_y_; - rviz_common::properties::FloatProperty* position_z_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; - - ros::ServiceClient poseUpdate_; -}; - -} // end namespace topological_rviz_tools - -#endif // POSE_PROPERTY_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/tag_controller.hpp b/topological_rviz_tools/include/topological_rviz_tools/tag_controller.hpp deleted file mode 100644 index 46366e05..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/tag_controller.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef TAG_CONTROLLER_H -#define TAG_CONTROLLER_H - -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/properties/string_property.hpp" -#include "tag_property.hpp" -#include "node_property.hpp" - -namespace topological_rviz_tools -{ - -class NodeProperty; - -/** @brief Property specialized to provide getter for booleans. */ -class TagController: public rviz_common::properties::Property -{ - -public: - TagController(const QString& name = QString(), - const std::vector& default_value = std::vector(), - const QString& description = QString(), - NodeProperty* parent = 0, - const char *changed_slot = 0, - QObject* receiver = 0); - - virtual ~TagController(); - /** @brief Do all setup that can't be done in the constructor. - * - * - * Calls onInitialize() just before returning. */ - void initialize(); - - /** @brief Subclasses should call this whenever a change is made which would change the results of toString(). */ - void emitConfigChanged(); - - virtual void load(const rviz_common::Config& config); - virtual void save(rviz_common::Config config) const; - - // required by something that initialises this class - QString formatClassId(const QString& class_id); - - /** @brief Return the class identifier which was used to create this - * instance. This version just returns whatever was set with - * setClassId(). */ - virtual QString getClassId() const { return class_id_; } - - /** @brief Set the class identifier used to create this instance. - * Typically this will be set by the factory object which created it. */ - virtual void setClassId( const QString& class_id ) { class_id_ = class_id; } - -Q_SIGNALS: - void configChanged(); - -private: - - QString class_id_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; -}; - -} // end namespace topological_rviz_tools - -#endif // TAG_CONTROLLER_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/tag_property.hpp b/topological_rviz_tools/include/topological_rviz_tools/tag_property.hpp deleted file mode 100644 index a97717e8..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/tag_property.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef TAG_PROPERTY_H -#define TAG_PROPERTY_H - -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/properties/string_property.hpp" -#include "topological_navigation_msgs/srv/modify_tag.hpp" - -namespace topological_rviz_tools -{ - -class TagProperty: public rviz_common::properties::StringProperty -{ - -public: - TagProperty(const QString& name = QString(), - const QString& default_value = QString(), - const QString& description = QString(), - const QString& node_name = QString(), - Property* parent = 0, - const char *changed_slot = 0, - QObject* receiver = 0); - - - virtual ~TagProperty(); - void addTag(const QString& tag); - -public Q_SLOTS: - void updateTag(); - -Q_SIGNALS: - void tagModified(); -private: - ros::ServiceClient tagUpdate_; - std::string tag_value_; // keep value so it's not lost if we fail to update - bool reset_value_; - std::string node_name_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; -}; - -} // end namespace topological_rviz_tools - -#endif // TAG_PROPERTY_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/topmap_manager.hpp b/topological_rviz_tools/include/topological_rviz_tools/topmap_manager.hpp deleted file mode 100644 index 63a6349d..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/topmap_manager.hpp +++ /dev/null @@ -1,115 +0,0 @@ -#ifndef TOPMAP_MANAGER_H -#define TOPMAP_MANAGER_H - -#include "node_controller.hpp" -#include "rclcpp/rclcpp.hpp" - -#include -#include - -#include -#include -#include - -#include "rviz_common/display_context.hpp" -#include "rviz_common/factory/pluginlib_factory.hpp" -#include "rviz_common/properties/enum_property.hpp" -#include "rviz_common/properties/property.hpp" -#include "rviz_common/properties/property_tree_model.hpp" -#include "rviz_common/render_panel.hpp" - -namespace topological_rviz_tools -{ - -class TopmapManager: public QObject -{ - -public: - TopmapManager(rviz_common::DisplayContext* context); - ~TopmapManager(); - - void initialize(); - - void update(float wall_dt, float ros_dt); - - /** @brief Return the current NodeController in use for the main - * RenderWindow. */ - NodeController* getController() const; - - NodeProperty* getCurrent() const; - - NodeController* create(const QString& type); - - int getNumViews() const; - - NodeController* getViewAt(int index) const; - - /** @brief Remove the given NodeController from the list and return - * it. If it is not in the list, NULL is returned and nothing - * changes. */ - NodeController* take(NodeController* view); - - /** @brief Remove the NodeController at the given index from the - * list and return it. If the index is not valid, NULL is returned - * and nothing changes. */ - NodeController* takeAt(int index); - - rviz_common::properties::PropertyTreeModel* getPropertyModel() { return property_model_; } - - void load(const rviz_common::Config& config); - void save(rviz_common::Config config) const; - - /** @brief Make a copy of @a view_to_copy and install that as the new current NodeController. */ - void setCurrentFrom(NodeProperty* view_to_copy); - - /** @brief Return a copy of source, made by saving source to - * a Config and instantiating and loading a new one from that. */ - NodeProperty* copy(NodeProperty* source); - - rviz_common::PluginlibFactory* getFactory() const { return factory_; } - - /** @brief Set the 3D view widget whose view will be controlled by - * NodeController instances from by this TopmapManager. */ - void setRenderPanel(rviz_common::RenderPanel* render_panel); - - /** @brief Return the 3D view widget managed by this TopmapManager. */ - rviz_common::RenderPanel* getRenderPanel() const { return render_panel_; } - -public Q_SLOTS: - - /** @brief Make a copy of the current NodeController and add it to the end of the list of saved views. */ - void copyCurrentToList(); - - /** @brief Create a new view controller of the given type and set it - * up to mimic and replace the previous current view. */ - void setCurrentNodeControllerType(const QString& new_class_id); - -Q_SIGNALS: - void configChanged(); - - /** @brief Emitted just after the current view controller changes. */ - void currentChanged(); - -private Q_SLOTS: - void onCurrentDestroyed(QObject* obj); - -private: - /** @brief Set @a new_current as current. - * @param mimic_view If true, call new_current->mimic(previous), if false call new_current->transitionFrom(previous). - * - * This calls mimic() or transitionFrom() on the new controller, - * deletes the previous controller (if one existed), and tells the - * RenderPanel about the new controller. */ - void setCurrent(NodeProperty* new_current, bool mimic_view); - - rviz_common::DisplayContext* context_; - NodeController* root_property_; - rviz_common::properties::PropertyTreeModel* property_model_; - rviz_common::PluginlibFactory* factory_; - NodeProperty* current_; - rviz_common::RenderPanel* render_panel_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; -}; - -} -#endif diff --git a/topological_rviz_tools/include/topological_rviz_tools/topological_edge_tool.hpp b/topological_rviz_tools/include/topological_rviz_tools/topological_edge_tool.hpp deleted file mode 100644 index e41de45f..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/topological_edge_tool.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef TOPMAP_EDGE_TOOL_H -#define TOPMAP_EDGE_TOOL_H - -#include -#include "rviz_common/tool.hpp" -#include -#include -#include "topological_navigation_msgs/srv/add_edge_rviz.hpp" -#include "std_msgs/msg/header.hpp" - -namespace rviz -{ -class VectorProperty; -class VisualizationManager; -class ViewportMouseEvent; -} - -namespace topological_rviz_tools -{ - -// BEGIN_TUTORIAL -// Here we declare our new subclass of rviz::Tool. Every tool -// which can be added to the tool bar is a subclass of -// rviz::Tool. -class TopmapEdgeTool: public rviz::Tool -{ -public: - TopmapEdgeTool(); - ~TopmapEdgeTool(); - - virtual void onInitialize(); - - virtual void activate(); - virtual void deactivate(); - - virtual int processMouseEvent(rviz::ViewportMouseEvent& event); -private: - rclcpp::Publisher::SharedPtr markerPub_; - ros::Publisher update_map_; - ros::ServiceClient addEdgeSrv_; - bool noClick_; // true if nothing clicked yet - geometry_msgs::msg::Pose firstClick_; - visualization_msgs::msg::Marker edgeMarker_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; - -}; -} // end namespace topological_rviz_tools - -#endif // TOPMAP_EDGE_TOOL_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/topological_map_panel.hpp b/topological_rviz_tools/include/topological_rviz_tools/topological_map_panel.hpp deleted file mode 100644 index 14bf55a2..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/topological_map_panel.hpp +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef TOPMAP_PANEL_H -#define TOPMAP_PANEL_H - -#include - -#include "rviz_common/panel.hpp" -#include "topmap_manager.hpp" -#include "tag_property.hpp" -#include "edge_property.hpp" -#include "node_property.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "rviz_common/properties/property_tree_widget.hpp" -#include "std_msgs/msg/header.hpp" - -#include "topological_navigation_msgs/srv/add_tag.hpp" -#include "topological_navigation_msgs/srv/add_edge.hpp" -#include "topological_navigation_msgs/srv/rmv_node.hpp" - -class QComboBox; -class QMessageBox; -class QModelIndex; -class QPushButton; -class QInputDialog; - -namespace topological_rviz_tools { -/** - * @brief Panel for choosing the view controller and saving and restoring - * viewpoints. - */ -class TopologicalMapPanel: public rviz::Panel -{ -public: - TopologicalMapPanel(QWidget* parent = 0); - virtual ~TopologicalMapPanel() {} - - /** @brief Overridden from TopologicalMapPanel. Just calls setMan() with vis_manager_->getTopmapManager(). */ - virtual void onInitialize(); - - /** @brief Set the TopmapManager which this panel should display and edit. - * - * If this TopologicalMapPanel is to be used with a TopmapManager other than - * the one in the TopmapManager sent in through - * TopologicalMapPanel::initialize(), either TopologicalMapPanelel::initialize() must not be - * called or setTopmapManager() must be called after - * TopologicalMapPanel::initialize(). */ - void setTopmapManager(TopmapManager* topmap_man); - - /** @brief Returns the current TopmapManager. */ - TopmapManager* getTopmapManager() const { return topmap_man_; } - - /** @brief Load configuration data, specifically the PropertyTreeWidget view settings. */ - virtual void load(const rviz_common::Config& config); - - /** @brief Save configuration data, specifically the PropertyTreeWidget view settings. */ - virtual void save(rviz_common::Config config) const; - -private Q_SLOTS: - void onDeleteClicked(); - void onAddTagClicked(); - void renameSelected(); - void onCurrentChanged(); - void updateTopMap(); -private: - ros::ServiceClient delNodeSrv_; - ros::ServiceClient delTagSrv_; - ros::ServiceClient delEdgeSrv_; - ros::ServiceClient addTagSrv_; - ros::Publisher update_map_; - - TopmapManager* topmap_man_; - rviz::PropertyTreeWidget* properties_view_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; -}; - -} // namespace topological_rviz_tools - -#endif // TOPMAP_PANEL_H diff --git a/topological_rviz_tools/include/topological_rviz_tools/topological_node_tool.hpp b/topological_rviz_tools/include/topological_rviz_tools/topological_node_tool.hpp deleted file mode 100644 index 1f51d39b..00000000 --- a/topological_rviz_tools/include/topological_rviz_tools/topological_node_tool.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef TOPMAP_NODE_TOOL_H -#define TOPMAP_NODE_TOOL_H - -#include -#include "rviz_common/tool.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "std_msgs/msg/header.hpp" -#include "topological_navigation_msgs/srv/add_node.hpp" - -namespace rviz -{ - class VectorProperty; - class VisualizationManager; - class ViewportMouseEvent; -} - -namespace topological_rviz_tools -{ - -// BEGIN_TUTORIAL -// Here we declare our new subclass of rviz::Tool. Every tool -// which can be added to the tool bar is a subclass of -// rviz::Tool. -class TopmapNodeTool: public rviz::Tool -{ -public: - TopmapNodeTool(); - ~TopmapNodeTool(); - - virtual void onInitialize(); - - virtual void activate(); - virtual void deactivate(); - - virtual int processMouseEvent(rviz::ViewportMouseEvent& event); -private: - ros::ServiceClient addNodeSrv_; - ros::Publisher update_map_; - rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; -}; -} // end namespace topological_rviz_tools - -#endif // TOPMAP_NODE_TOOL_H diff --git a/topological_rviz_tools/launch/strands_rviz_topmap.launch b/topological_rviz_tools/launch/strands_rviz_topmap.launch deleted file mode 100644 index 37e21d73..00000000 --- a/topological_rviz_tools/launch/strands_rviz_topmap.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_rviz_tools/package.xml b/topological_rviz_tools/package.xml deleted file mode 100644 index e4a42e13..00000000 --- a/topological_rviz_tools/package.xml +++ /dev/null @@ -1,42 +0,0 @@ - - - - topological_rviz_tools - 3.0.0 - The ros2 rviz-based topological map construction package - - - Marc Hanheide - Marc Hanheide - Michal Staniaszek - - Apache-2.0 - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - ament_cmake - - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - - std_msgs - diagnostic_msgs - geometry_msgs - topological_navigation_msgs - rclcpp - libogre-dev - rviz_common - rviz_default_plugins - rviz_ogre_vendor - rviz_rendering - - - ament_cmake - - - - diff --git a/topological_rviz_tools/plugin_description.xml b/topological_rviz_tools/plugin_description.xml deleted file mode 100644 index 0af4defe..00000000 --- a/topological_rviz_tools/plugin_description.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - Tool for manipulating a topological map - - - - - Tool for manipulating edges in the topological map - - - - - Tool for adding nodes in the topological map - - - - diff --git a/topological_rviz_tools/scripts/python_topmap_interface.py b/topological_rviz_tools/scripts/python_topmap_interface.py deleted file mode 100755 index 537dd51b..00000000 --- a/topological_rviz_tools/scripts/python_topmap_interface.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python - -import rospy -import math -import operator -from std_msgs.msg import Time -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation_msgs.srv import * -from geometry_msgs.msg import Pose - -class TopmapInterface(object): - """Creates some topics which can be used by c++ code in the rviz portion of this - package to call into python functions to modify the topological map - - """ - - def __init__(self): - """ - name: the name of the map as recorded in the database. Use - topological_util/list_maps to see which ones are available - - """ - rospy.init_node("topmap_interface") - self.name = rospy.get_param('~map_name') - self.topmap = None - self.new_nodes = 0 - - self.topmap_sub = rospy.Subscriber("/topological_map", TopologicalMap, self.topmap_cb) - self.add_edge_srv = rospy.Service("~add_edge", topological_navigation_msgs.srv.AddEdgeRviz, self.add_edge) - - self.manager_add_edge = rospy.ServiceProxy("/topological_map_manager/add_edges_between_nodes", topological_navigation_msgs.srv.AddEdge) - - rospy.spin() - - def add_edge(self, req): - def tuple_dist(pose_tuple): - return math.sqrt(pow(pose_tuple[0].position.x - pose_tuple[1].position.x, 2) - + pow(pose_tuple[0].position.y - pose_tuple[1].position.y, 2)) - - # Find the nodes closest to the start and end point of the line given in the request - poses = [node.pose for node in self.topmap.nodes] - - from_dists = map(tuple_dist, zip([req.first]*len(poses), poses)) - to_dists = map(tuple_dist, zip([req.second]*len(poses), poses)) - closest_from_ind, from_dist = min(enumerate(from_dists), key=operator.itemgetter(1)) - closest_to_ind, to_dist = min(enumerate(to_dists), key=operator.itemgetter(1)) - - if req.max_distance > 0 and (from_dist > req.max_distance or to_dist > req.max_distance): - return AddEdgeResponse(True, "Click locations were not close enough to a node") - - from_name = self.topmap.nodes[closest_from_ind].name - to_name = self.topmap.nodes[closest_to_ind].name - - if from_name == to_name: - return AddEdgeResponse(True, "Can't have an edge from a node to itself.") - - message = "Added edge from {0} to {1}".format(from_name, to_name) - - self.manager_add_edge(origin=from_name, destination=to_name, action="move_base") - if req.bidirectional: - self.manager_add_edge(origin=to_name, destination=from_name, action="move_base") - message += " (bidirectional)" - - return topological_navigation_msgs.srv.AddEdgeRvizResponse(True, message) - - def topmap_cb(self, msg): - rospy.loginfo("Topological map was updated via callback.") - self.topmap = msg - -if __name__ == '__main__': - TopmapInterface() diff --git a/topological_rviz_tools/setup.py b/topological_rviz_tools/setup.py deleted file mode 100644 index 095f0c1e..00000000 --- a/topological_rviz_tools/setup.py +++ /dev/null @@ -1,28 +0,0 @@ -from setuptools import setup -package_name = 'topological_rviz_tools' - -setup( - name=package_name, - version='3.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=[''], - zip_safe=True, - maintainer='Michal Staniaszek', - maintainer_email='staniasm@cs.bham.ac.uk', - description='The ros2 rviz-based topological map construction package', - license='BSD', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'python_topmap_interface.py = topological_rviz_tools.scripts.python_topmap_interface.py' - ], - }, - -) - - diff --git a/topological_rviz_tools/src/edge_controller.cpp b/topological_rviz_tools/src/edge_controller.cpp deleted file mode 100644 index e87c1f23..00000000 --- a/topological_rviz_tools/src/edge_controller.cpp +++ /dev/null @@ -1,82 +0,0 @@ - -#include - -namespace topological_rviz_tools -{ -EdgeController::EdgeController(const QString& name, - const std::vector& default_values, - const QString& description, - rviz_common::properties::Property* parent, - const char *changed_slot, - QObject* receiver) - : rviz_common::properties::Property(name, "", description, parent, changed_slot, receiver) -{ - for (int i = 0; i < default_values.size(); i++) { - // RCLCPP_INFO("ADDING EDGE %s", default_values[i].edge_id.c_str()); - EdgeProperty* newEdge = new EdgeProperty("Edge", default_values[i], ""); - addChild(newEdge); - connect(newEdge, SIGNAL(edgeModified()), parent, SLOT(nodePropertyUpdated())); - } -} - -void EdgeController::initialize() -{ - - std::stringstream ss; - static int count = 0; - ss << "EdgeControllerCamera" << count++; - - // Do subclass initialization. - onInitialize(); -} - -EdgeController::~EdgeController() -{ - for (;numChildren() != 0;) { - delete takeChildAt(0); - } -} - -QString EdgeController::formatClassId(const QString& class_id) -{ - QStringList id_parts = class_id.split("/"); - if(id_parts.size() != 2) - { - // Should never happen with pluginlib class ids, which are - // formatted like "package_name/class_name". Not worth crashing - // over though. - return class_id; - } - else - { - return id_parts[ 1 ] + " (" + id_parts[ 0 ] + ")"; - } -} - - -void EdgeController::emitConfigChanged() -{ - Q_EMIT configChanged(); -} - -void EdgeController::load(const rviz_common::Config& config) -{ - // // Load the name by hand. - // QString name; - // if(config.mapGetString("Name", &name)) - // { - // setName(name); - // } - // // Load all sub-properties the same way the base class does. - // rviz_common::properties::Property::load(config); -} - -void EdgeController::save(rviz_common::Config config) const -{ - // config.mapSetValue("Class", getClassId()); - // config.mapSetValue("Name", getName()); - - // rviz_common::properties::Property::save(config); -} - -} // end namespace topological_rviz_tools diff --git a/topological_rviz_tools/src/edge_property.cpp b/topological_rviz_tools/src/edge_property.cpp deleted file mode 100644 index 3f345b91..00000000 --- a/topological_rviz_tools/src/edge_property.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include "topological_rviz_tools/edge_property.hpp" - -namespace topological_rviz_tools -{ - -EdgeProperty::EdgeProperty(const QString& name, - const topological_navigation_msgs::msg::Edge& default_value, - const QString& description, - Property* parent, - const char *changed_slot, - QObject* receiver) - : rviz_common::properties::Property(name, default_value.edge_id.c_str(), description, parent, changed_slot, receiver) - , edge_(default_value) - , action_value_(default_value.action) - , topvel_value_(default_value.top_vel) - , reset_value_(false) -{ - ros::NodeHandle nh; - edgeUpdate_ = nh.serviceClient("/topological_map_manager/update_edge", true); - setReadOnly(true); - edge_id_ = new rviz_common::properties::StringProperty("Edge ID", edge_.edge_id.c_str(), "", this); - edge_id_->setReadOnly(true); - node_ = new rviz_common::properties::StringProperty("Node", edge_.node.c_str(), "", this); - node_->setReadOnly(true); - action_ = new rviz_common::properties::StringProperty("Action", edge_.action.c_str(), "", this, SLOT(updateAction()), this); - map_2d_ = new rviz_common::properties::StringProperty("Map 2D", edge_.map_2d.c_str(), "", this); - map_2d_->setReadOnly(true); - top_vel_ = new rviz_common::properties::FloatProperty("Top vel", edge_.top_vel, "", this, SLOT(updateTopvel()), this); - inflation_radius_ = new rviz_common::properties::FloatProperty("Inflation radius", edge_.inflation_radius, "", this); - inflation_radius_->setReadOnly(true); -} - -void EdgeProperty::updateTopvel(){ - if (reset_value_){ // this function gets called when we reset a value when the service call fails, so ignore that. - reset_value_ = false; - return; - } - - topological_navigation_msgs::UpdateEdgeLegacy srv; - srv.request.edge_id = edge_id_->getStdString().c_str(); - srv.request.top_vel = top_vel_->getFloat(); - srv.request.action = action_->getStdString().c_str(); - - if (edgeUpdate_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully updated edge %s topvel to %f", edge_id_->getStdString().c_str(), srv.request.top_vel); - Q_EMIT edgeModified(); - topvel_value_ = top_vel_->getFloat(); - } else { - RCLCPP_INFO(logger_,"Failed to update xy tolerance of %s: %s", edge_id_->getStdString().c_str(), srv.response.message.c_str()); - reset_value_ = true; - top_vel_->setValue(topvel_value_); - } - } else { - RCLCPP_WARN(logger_,"Failed to get response from service to update xy tolerance for node %s", edge_id_->getStdString().c_str()); - reset_value_ = true; - top_vel_->setValue(topvel_value_); - } - -} - -void EdgeProperty::updateAction(){ - if (reset_value_){ // this function gets called when we reset a value when the service call fails, so ignore that. - reset_value_ = false; - return; - } - - topological_navigation_msgs::UpdateEdgeLegacy srv; - srv.request.edge_id = edge_id_->getStdString().c_str(); - srv.request.top_vel = top_vel_->getFloat(); - srv.request.action = action_->getStdString().c_str(); - - if (edgeUpdate_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_,"Successfully updated edge %s action to %s", edge_id_->getStdString().c_str(), srv.request.action.c_str()); - Q_EMIT edgeModified(); - action_value_ = action_->getStdString(); - } else { - RCLCPP_INFO(logger_,"Failed to update edge action of %s: %s", edge_id_->getStdString().c_str(), srv.response.message.c_str()); - reset_value_ = true; - action_->setValue(QString::fromStdString(action_value_)); - } - } else { - RCLCPP_WARN(logger_,"Failed to get response from service to update action for edge %s", edge_id_->getStdString().c_str()); - reset_value_ = true; - action_->setValue(QString::fromStdString(action_value_)); - } -} - -EdgeProperty::~EdgeProperty() -{ - delete edge_id_; - delete node_; - delete action_; - delete map_2d_; - delete top_vel_; - delete inflation_radius_; -} - -} // end namespace topological_rviz_tools diff --git a/topological_rviz_tools/src/node_controller.cpp b/topological_rviz_tools/src/node_controller.cpp deleted file mode 100644 index 46f58b32..00000000 --- a/topological_rviz_tools/src/node_controller.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "topological_rviz_tools/node_controller.hpp" - - -namespace topological_rviz_tools -{ -NodeController::NodeController() - : rviz_common::properties::Property() -{ - ros::NodeHandle nh_; - top_sub_ = nh_.subscribe("topological_map", 1, &NodeController::topmapCallback, this); -} - -void NodeController::initialize() -{ - - std::stringstream ss; - static int count = 0; - ss << "NodeControllerCamera" << count++; - - // Do subclass initialization. - onInitialize(); -} - -NodeController::~NodeController() -{ -} - -void NodeController::topmapCallback(const topological_navigation_msgs::TopologicalMap::ConstPtr& msg){ - RCLCPP_INFO(logger_, "Updating topological map"); - - // deep copy (because of const), and then sort the array so we display in - // alphabetical order of node names - std::vector nodes = msg->nodes; - std::sort(nodes.begin(), nodes.end(), nodeSort); - - RCLCPP_INFO(logger_, "%s", nodes[nodes.size()-1].name.c_str()); - - // If we're the ones who made the change, then we only replace the property - // for the specific nodes that we changed, otherwise replace everything. - if (modifiedChildren_.size() == 0) { - // Use takechildat to remove, because removeChildren doesn't change the - // child states, and can cause issues when the new properties are added. - for (;numChildren() != 0;) { - delete takeChildAt(0); - } - - for (int i = 0; i < nodes.size(); i++) { - NodeProperty* newProp = new NodeProperty("Node", nodes[i], ""); - addChild(newProp); - connect(newProp, SIGNAL(nodeModified(Property*)), this, SLOT(updateModifiedNode(Property*))); - } - } else { - std::vector > toDelete; - for (int msg_ind = 0; msg_ind < nodes.size(); msg_ind++) { - // could reduce checks here by removing children, but probably not worth the trouble - for (int mod_ind = 0; mod_ind < modifiedChildren_.size(); mod_ind++) { - if (std::string(nodes[msg_ind].name).compare(modifiedChildren_[mod_ind]->getValue().toString().toStdString().c_str()) == 0) { - toDelete.push_back(std::make_pair(mod_ind, msg_ind)); - } - } - } - - for (int i = 0; i < toDelete.size(); i++) { - // remove only the modified child from the child list - delete takeChild(modifiedChildren_[toDelete[i].first]); - - // RCLCPP_INFO("Adding node with name %s, which matches %s", msg->nodes[i].name.c_str(), nodeName.c_str()); - NodeProperty* newProp = new NodeProperty("Node", nodes[toDelete[i].second], ""); - addChild(newProp, toDelete[i].second); - connect(newProp, SIGNAL(nodeModified(Property*)), this, SLOT(updateModifiedNode(Property*))); - } - modifiedChildren_.clear(); - } -} - -void NodeController::updateModifiedNode(Property* node){ - RCLCPP_INFO(logger_, "Child was modified: %s", node->getValue().toString().toStdString().c_str()); - modifiedChildren_.push_back(node); - Q_EMIT childModified(); -} - -QString NodeController::formatClassId(const QString& class_id) -{ - QStringList id_parts = class_id.split("/"); - if(id_parts.size() != 2) - { - // Should never happen with pluginlib class ids, which are - // formatted like "package_name/class_name". Not worth crashing - // over though. - return class_id; - } - else - { - return id_parts[ 1 ] + " (" + id_parts[ 0 ] + ")"; - } -} - -void NodeController::load(const rviz_common::Config& config) -{ - // Load the name by hand. - QString name; - if(config.mapGetString("Name", &name)) - { - setName(name); - } - // Load all sub-properties the same way the base class does. - rviz_common::properties::Property::load(config); -} - -void NodeController::save(rviz_common::Config config) const -{ - config.mapSetValue("Class", getClassId()); - config.mapSetValue("Name", getName()); - - rviz_common::properties::Property::save(config); -} - -} // end namespace topological_rviz_tools diff --git a/topological_rviz_tools/src/node_property.cpp b/topological_rviz_tools/src/node_property.cpp deleted file mode 100644 index 9c9db2fb..00000000 --- a/topological_rviz_tools/src/node_property.cpp +++ /dev/null @@ -1,168 +0,0 @@ -#include "topological_rviz_tools/node_property.hpp" - -namespace topological_rviz_tools -{ - -NodeProperty::NodeProperty(const QString& name, - const topological_navigation_msgs::msg::TopologicalNode& default_value, - const QString& description, - Property* parent, - const char *changed_slot, - QObject* receiver) - : rviz_common::properties::Property(name, default_value.name.c_str(), description, parent, changed_slot, this) - , node_(default_value) - , name_(default_value.name) - , xy_tol_value_(default_value.xy_goal_tolerance) - , yaw_tol_value_(default_value.yaw_goal_tolerance) - , reset_value_(false) -{ - // manually connect the signals instead of using the constructor to do it. - // Can't seem to get the connection to work if passing in the slot in the - // constructor. - connect(this, SIGNAL(changed()), this, SLOT(updateNodeName())); - - ros::NodeHandle nh; - nameUpdate_ = nh.serviceClient("/topological_map_manager/update_node_name", true); - toleranceUpdate_ = nh.serviceClient("/topological_map_manager/update_node_tolerance", true); - - map_ = new rviz_common::properties::StringProperty("Map", node_.map.c_str(), "", this); - map_->setReadOnly(true); - - pointset_ = new rviz_common::properties::StringProperty("Pointset", node_.pointset.c_str(), "", this); - pointset_->setReadOnly(true); - - localise_ = new rviz_common::properties::StringProperty("Localise by topic", node_.localise_by_topic.c_str(), "", this); - localise_->setReadOnly(true); - - yaw_tolerance_ = new rviz_common::properties::FloatProperty("Yaw Tolerance", node_.yaw_goal_tolerance, - "The robot is facing the right direction if the" - " difference between the current yaw and the node's" - " orientation is less than this value.", - this, SLOT(updateYawTolerance()), this); - xy_tolerance_ = new rviz_common::properties::FloatProperty("XY Tolerance", node_.xy_goal_tolerance, - "The robot is at the goal if the difference" - " between its current position and the node's" - " position is less than this value.", - this, SLOT(updateXYTolerance()), this); - - ros::ServiceClient tagService_ = nh.serviceClient("/topological_map_manager/get_node_tags", true); - topological_navigation_msgs::GetNodeTags srv; - srv.request.node_name = name_.c_str(); - std::vector node_tags; - if (tagService_.call(srv)) { - if (srv.response.success) { - node_tags = srv.response.tags; - } else { - RCLCPP_WARN(logger_, "Failed to get tags for node %s", name_.c_str()); - } - } else { - RCLCPP_WARN(logger_, "Failed to get response from service to get tags for node %s", name_.c_str()); - } - tag_controller_ = new TagController("Tags", node_tags, "", this); - if (node_tags.size() == 0) { - tag_controller_->setHidden(true); - } - - pose_ = new PoseProperty("Pose", node_.pose, "", this); - edge_controller_ = new EdgeController("Edges", node_.edges, "", this); -} - -NodeProperty::~NodeProperty() -{ - delete map_; - delete pointset_; - delete yaw_tolerance_; - delete xy_tolerance_; - delete pose_; - delete edge_controller_; -} - -void NodeProperty::updateYawTolerance(){ - if (reset_value_){ // this function gets called when we reset a value when the service call fails, so ignore that. - reset_value_ = false; - return; - } - - topological_navigation_msgs::UpdateNodeTolerance srv; - srv.request.node_name = name_; - srv.request.yaw_tolerance = yaw_tolerance_->getFloat(); - srv.request.xy_tolerance = xy_tolerance_->getFloat(); - - if (toleranceUpdate_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully updated yaw tolerance for node %s to %f", name_.c_str(), srv.request.yaw_tolerance); - Q_EMIT nodeModified(this); - yaw_tol_value_ = yaw_tolerance_->getFloat(); - } else { - RCLCPP_INFO(logger_, "Failed to update yaw tolerance for node %s: %s", name_.c_str(), srv.response.message.c_str()); - reset_value_ = true; - yaw_tolerance_->setValue(yaw_tol_value_); - } - } else { - RCLCPP_WARN(logger_, "Failed to get response from service to update yaw tolerance for node %s", name_.c_str()); - reset_value_ = true; - yaw_tolerance_->setValue(yaw_tol_value_); - } -} - -void NodeProperty::updateXYTolerance(){ - if (reset_value_){ // this function gets called when we reset a value when the service call fails, so ignore that. - reset_value_ = false; - return; - } - - topological_navigation_msgs::UpdateNodeTolerance srv; - srv.request.node_name = name_; - srv.request.yaw_tolerance = yaw_tolerance_->getFloat(); - srv.request.xy_tolerance = xy_tolerance_->getFloat(); - - if (toleranceUpdate_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully updated tolerance for node %s to %f", name_.c_str(), srv.request.xy_tolerance); - Q_EMIT nodeModified(this); - xy_tol_value_ = xy_tolerance_->getFloat(); - } else { - RCLCPP_INFO(logger_, "Failed to update xy tolerance of %s: %s", name_.c_str(), srv.response.message.c_str()); - reset_value_ = true; - xy_tolerance_->setValue(xy_tol_value_); - } - } else { - RCLCPP_WARN(logger_, "Failed to get response from service to update xy tolerance for node %s", name_.c_str()); - reset_value_ = true; - xy_tolerance_->setValue(xy_tol_value_); - } -} - -void NodeProperty::updateNodeName(){ - if (reset_value_){ // this function gets called when we reset a value when the service call fails, so ignore that. - reset_value_ = false; - return; - } - - topological_navigation_msgs::UpdateNodeName srv; - srv.request.node_name = name_; - srv.request.new_name = this->getValue().toString().toStdString().c_str(); - - if (nameUpdate_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully updated node name %s to %s", name_.c_str(), srv.request.new_name.c_str()); - Q_EMIT nodeModified(this); - name_ = getValue().toString().toStdString(); - } else { - RCLCPP_INFO(logger_, "Failed to update node name of %s to %s: %s", name_.c_str(), srv.request.new_name.c_str(), srv.response.message.c_str()); - reset_value_ = true; - setValue(QString::fromStdString(name_)); - } - } else { - RCLCPP_WARN(logger_, "Failed to get response from service to update node %s", name_.c_str()); - reset_value_ = true; - setValue(QString::fromStdString(name_)); - } - name_ = this->getValue().toString().toStdString(); -} - -void NodeProperty::nodePropertyUpdated(){ - Q_EMIT nodeModified(this); -} - -} // end namespace topological_rviz_tools diff --git a/topological_rviz_tools/src/pose_property.cpp b/topological_rviz_tools/src/pose_property.cpp deleted file mode 100644 index 9ea77238..00000000 --- a/topological_rviz_tools/src/pose_property.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#include "topological_rviz_tools/pose_property.hpp" - -namespace topological_rviz_tools -{ - -PoseProperty::PoseProperty(const QString& name, - const geometry_msgs::msg::Pose& default_value, - const QString& description, - rviz_common::properties::Property* parent, - const char *changed_slot, - QObject* receiver) - // We set the default value sent to the base property to the empty string, - // rather than trying to put in the geometry msgs pose - : rviz_common::properties::Property(name, "", description, parent, changed_slot, receiver), - pose_(default_value) -{ - connect(this, SIGNAL(poseModified()), parent, SLOT(nodePropertyUpdated())); - setReadOnly(true); // can't change the name of this pose - - ros::NodeHandle nh; - // Use addnode because it has the fields we need, so we don't have to write a - // new message - poseUpdate_ = nh.serviceClient("/topological_map_manager/update_node_pose", true); - - orientation_ = new rviz_common::properties::StringProperty("Orientation", "", "", this); - orientation_w_ = new rviz_common::properties::FloatProperty("w", pose_.orientation.w, "", orientation_); - orientation_x_ = new rviz_common::properties::FloatProperty("x", pose_.orientation.x, "", orientation_); - orientation_y_ = new rviz_common::properties::FloatProperty("y", pose_.orientation.y, "", orientation_); - orientation_z_ = new rviz_common::properties::FloatProperty("z", pose_.orientation.z, "", orientation_); - // Don't allow messing around with the quaternion from here - can be done - // using the interactive marker. - orientation_->setReadOnly(true); - orientation_w_->setReadOnly(true); - orientation_x_->setReadOnly(true); - orientation_y_->setReadOnly(true); - orientation_z_->setReadOnly(true); - - position_ = new rviz_common::properties::StringProperty("Position", "", "", this); - position_x_ = new rviz_common::properties::FloatProperty("x", pose_.position.x, "", position_, SLOT(positionUpdated()), this); - position_y_ = new rviz_common::properties::FloatProperty("y", pose_.position.y, "", position_, SLOT(positionUpdated()), this); - position_z_ = new rviz_common::properties::FloatProperty("z", pose_.position.z, "", position_); - - // Don't allow modification of z position of the node - position_->setReadOnly(true); - position_z_->setReadOnly(true); -} - -PoseProperty::~PoseProperty() -{ - delete orientation_w_; - delete orientation_x_; - delete orientation_y_; - delete orientation_z_; - delete orientation_; - delete position_x_; - delete position_y_; - delete position_z_; - delete position_; -} - -void PoseProperty::positionUpdated() -{ - topological_navigation_msgs::AddNode srv; - srv.request.name = getParent()->getValue().toString().toStdString().c_str(); - srv.request.pose.position.x = position_x_->getFloat(); - srv.request.pose.position.y = position_y_->getFloat(); - srv.request.pose.position.z = position_z_->getFloat(); - srv.request.pose.orientation.x = orientation_x_->getFloat(); - srv.request.pose.orientation.y = orientation_y_->getFloat(); - srv.request.pose.orientation.z = orientation_z_->getFloat(); - srv.request.pose.orientation.w = orientation_w_->getFloat(); - - if (poseUpdate_.call(srv)) { - RCLCPP_INFO(logger_, "Successfully updated pose for node %s", srv.request.name.c_str()); - Q_EMIT poseModified(); - } else { - RCLCPP_WARN(logger_, "Failed to update pose for node %s", srv.request.name.c_str()); - } -} - -} // end namespace topological_rviz_tools diff --git a/topological_rviz_tools/src/tag_controller.cpp b/topological_rviz_tools/src/tag_controller.cpp deleted file mode 100644 index 50a22954..00000000 --- a/topological_rviz_tools/src/tag_controller.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include "topological_rviz_tools/tag_controller.hpp" - -namespace topological_rviz_tools -{ -TagController::TagController(const QString& name, - const std::vector& default_values, - const QString& description, - NodeProperty* parent, - const char *changed_slot, - QObject* receiver) - : rviz_common::properties::Property(name, "", description, parent, changed_slot, receiver) -{ - for (int i = 0; i < default_values.size(); i++) { - TagProperty* newTag = new TagProperty("Tag", QString(QString::fromStdString(default_values[i])), "", QString::fromStdString(parent->getNodeName())); - addChild(newTag); - connect(newTag, SIGNAL(tagModified()), parent, SLOT(nodePropertyUpdated())); - } -} - -void TagController::initialize() -{ - - std::stringstream ss; - static int count = 0; - ss << "TagController" << count++; -} - -TagController::~TagController() -{ - for (;numChildren() != 0;) { - delete takeChildAt(0); - } -} - -QString TagController::formatClassId(const QString& class_id) -{ - QStringList id_parts = class_id.split("/"); - if(id_parts.size() != 2) - { - // Should never happen with pluginlib class ids, which are - // formatted like "package_name/class_name". Not worth crashing - // over though. - return class_id; - } - else - { - return id_parts[ 1 ] + " (" + id_parts[ 0 ] + ")"; - } -} - -void TagController::emitConfigChanged() -{ - Q_EMIT configChanged(); -} - -void TagController::load(const rviz_common::Config& config) -{ - // // Load the name by hand. - // QString name; - // if(config.mapGetString("Name", &name)) - // { - // setName(name); - // } - // // Load all sub-properties the same way the base class does. - // rviz_common::properties::Property::load(config); -} - -void TagController::save(rviz_common::Config config) const -{ - // config.mapSetValue("Class", getClassId()); - // config.mapSetValue("Name", getName()); - - // rviz_common::properties::Property::save(config); -} - -} // end namespace topological_rviz_tools diff --git a/topological_rviz_tools/src/tag_property.cpp b/topological_rviz_tools/src/tag_property.cpp deleted file mode 100644 index 20229180..00000000 --- a/topological_rviz_tools/src/tag_property.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "topological_rviz_tools/tag_property.hpp" - -namespace topological_rviz_tools -{ - -TagProperty::TagProperty(const QString& name, - const QString& default_value, - const QString& description, - const QString& node_name, - Property* parent, - const char *changed_slot, - QObject* receiver) - : rviz_common::properties::StringProperty(name, default_value, description, parent, changed_slot, receiver) - , tag_value_(default_value.toStdString()) - , reset_value_(false) - , node_name_(node_name.toStdString()) -{ - connect(this, SIGNAL(changed()), this, SLOT(updateTag())); - ros::NodeHandle nh; - tagUpdate_ = nh.serviceClient("/topological_map_manager/modify_node_tags", true); -} - -void TagProperty::updateTag(){ - if (reset_value_){ // this function gets called when we reset a value when the service call fails, so ignore that. - reset_value_ = false; - return; - } - - topological_navigation_msgs::ModifyTag srv; - srv.request.tag = tag_value_.c_str(); - srv.request.new_tag = getString().toStdString().c_str(); - srv.request.node.push_back(node_name_); - - if (tagUpdate_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully updated tag %s to %s", srv.request.tag.c_str(), srv.request.new_tag.c_str()); - Q_EMIT tagModified(); - tag_value_ = getString().toStdString(); - } else { - RCLCPP_INFO(logger_, "Failed to update tag %s: %s", srv.request.tag.c_str(), srv.response.meta.c_str()); - reset_value_ = true; - setValue(QString::fromStdString(tag_value_)); - } - } else { - RCLCPP_WARN(logger_, "Failed to get response from service to update tag %s", srv.request.tag.c_str()); - reset_value_ = true; - setValue(QString::fromStdString(tag_value_)); - } -} - -TagProperty::~TagProperty() -{ -} - -} // end namespace topological_rviz_tools diff --git a/topological_rviz_tools/src/topmap_manager.cpp b/topological_rviz_tools/src/topmap_manager.cpp deleted file mode 100644 index 6fee8d11..00000000 --- a/topological_rviz_tools/src/topmap_manager.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include "topological_rviz_tools/topmap_manager.hpp" - -namespace topological_rviz_tools -{ -TopmapManager::TopmapManager(rviz_common::DisplayContext* context) - : context_(context) - , root_property_(new NodeController) - , property_model_(new rviz_common::properties::PropertyTreeModel(root_property_)) - , factory_(new rviz_common::PluginlibFactory("topological_rviz_tools", "topological_rviz_tools::NodeController")) - , current_(NULL) - , render_panel_(NULL) -{ - RCLCPP_INFO(logger_, "Initialising node manager"); - property_model_->setDragDropClass("node-controller"); - // connect(property_model_, SIGNAL(configChanged()), this, SIGNAL(configChanged())); - // add(new NodeController, -1); -} - -TopmapManager::~TopmapManager() -{ - delete property_model_; - delete factory_; - delete root_property_; -} - -void TopmapManager::initialize() -{ -} - -void TopmapManager::update(float wall_dt, float ros_dt) -{ -} - -NodeController* TopmapManager::create(const QString& class_id) -{ - // QString error; - // NodeController* view = factory_->make(class_id, &error); - // if(!view) - // { - // view = new FailedNodeController(class_id, error); - // } - // view->initialize(); - - // return view; -} - -NodeController* TopmapManager::getController() const -{ - return root_property_; -} - -NodeProperty* TopmapManager::getCurrent() const -{ - return current_; -} - -void TopmapManager::setCurrentFrom(NodeProperty* source_view) -{ - // if(source_view == NULL) - // { - // return; - // } - - // NodeController* previous = getCurrent(); - // if(source_view != previous) - // { - // NodeController* new_current = copy(source_view); - - // setCurrent(new_current, false); - // Q_EMIT configChanged(); - // } -} - -void TopmapManager::onCurrentDestroyed(QObject* obj) -{ - if(obj == current_) - { - current_ = NULL; - } -} - -void TopmapManager::setCurrent(NodeProperty* new_current, bool mimic_view) -{ - NodeProperty* previous = getCurrent(); - if(previous) - { - disconnect(previous, SIGNAL(destroyed(QObject*)), this, SLOT(onCurrentDestroyed(QObject*))); - } - new_current->setName("Current View"); - connect(new_current, SIGNAL(destroyed(QObject*)), this, SLOT(onCurrentDestroyed(QObject*))); - current_ = new_current; - // root_property_->addChildToFront(new_current); - delete previous; - - // if(render_panel_) - // { - // This setNodeController() can indirectly call - // TopmapManager::update(), so make sure getCurrent() will return the - // new one by this point. - // render_panel_->setViewController(new_current); - // } - Q_EMIT currentChanged(); -} - -void TopmapManager::setCurrentNodeControllerType(const QString& new_class_id) -{ - // setCurrent(create(new_class_id), true); -} - -void TopmapManager::copyCurrentToList() -{ - // NodeController* current = getCurrent(); - // if(current) - // { - // NodeController* new_copy = copy(current); - // new_copy->setName(factory_->getClassName(new_copy->getClassId())); - // root_property_->addChild(new_copy); - // } -} - -NodeController* TopmapManager::getViewAt(int index) const -{ - // if(index < 0) - // { - // index = 0; - // } - // return qobject_cast(root_property_->childAt(index + 1)); -} - -int TopmapManager::getNumViews() const -{ - // int count = root_property_->numChildren(); - // if(count <= 0) - // { - // return 0; - // } - // else - // { - // return count-1; - // } -} - -// void TopmapManager::add(NodeController* view, int index) -// { -// // if(index < 0) -// // { -// // index = root_property_->numChildren(); -// // } -// // else -// // { -// // index++; -// // } -// // property_model_->getRoot()->addChild(view, index); -// } - -NodeController* TopmapManager::take(NodeController* view) -{ - // for(int i = 0; i < getNumViews(); i++) - // { - // if(getViewAt(i) == view) - // { - // return qobject_cast(root_property_->takeChildAt(i + 1)); - // } - // } - // return NULL; -} - -NodeController* TopmapManager::takeAt(int index) -{ - // if(index < 0) - // { - // return NULL; - // } - // return qobject_cast(root_property_->takeChildAt(index + 1)); -} - -void TopmapManager::load(const rviz_common::Config& config) -{ - // rviz_common::Config current_config = config.mapGetChild("Current"); - // QString class_id; - // if(current_config.mapGetString("Class", &class_id)) - // { - // NodeController* new_current = create(class_id); - // new_current->load(current_config); - // setCurrent(new_current, false); - // } - - // rviz_common::Config saved_views_config = config.mapGetChild("Saved"); - // root_property_->removeChildren(1); - // int num_saved = saved_views_config.listLength(); - // for(int i = 0; i < num_saved; i++) - // { - // rviz_common::Config view_config = saved_views_config.listChildAt(i); - - // if(view_config.mapGetString("Class", &class_id)) - // { - // NodeController* view = create(class_id); - // view->load(view_config); - // add(view); - // } - // } -} - -void TopmapManager::save(rviz_common::Config config) const -{ - // getCurrent()->save(config.mapMakeChild("Current")); - - // rviz_common::Config saved_views_config = config.mapMakeChild("Saved"); - // for(int i = 0; i < getNumViews(); i++) - // { - // getViewAt(i)->save(saved_views_config.listAppendNew()); - // } -} - -NodeProperty* TopmapManager::copy(NodeProperty* source) -{ - // rviz_common::Config config; - // source->save(config); - - // NodeController* copy_of_source = create(source->getClassId()); - // copy_of_source->load(config); - - // return copy_of_source; - return NULL; -} - -} diff --git a/topological_rviz_tools/src/topological_edge_tool.cpp b/topological_rviz_tools/src/topological_edge_tool.cpp deleted file mode 100644 index 5c2a2495..00000000 --- a/topological_rviz_tools/src/topological_edge_tool.cpp +++ /dev/null @@ -1,180 +0,0 @@ -#include -#include -#include - -#include - - -#include -#include -#include -#include -#include - -#include "topological_rviz_tools/topological_edge_tool.hpp" - -namespace topological_rviz_tools -{ - -// BEGIN_TUTORIAL -// Construction and destruction -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -// -// The constructor must have no arguments, so we can't give the -// constructor the parameters it needs to fully initialize. -// -// Here we set the "shortcut_key_" member variable defined in the -// superclass to declare which key will activate the tool. -TopmapEdgeTool::TopmapEdgeTool() - : noClick_(true) -{ - shortcut_key_ = 'e'; - - // Initialise the marker with some defaults - edgeMarker_.header.frame_id = "map"; - edgeMarker_.ns = "edge_tool_markers"; - edgeMarker_.id = 0; - edgeMarker_.type = visualization_msgs::msg::Marker::ARROW; - edgeMarker_.scale.x = 0.1; - edgeMarker_.scale.y = 0.15; - edgeMarker_.scale.z = 0.2; - edgeMarker_.color.a = 1.0; // Don't forget to set the alpha! - edgeMarker_.color.r = 0.0; - edgeMarker_.color.g = 1.0; - edgeMarker_.color.b = 0.0; -} - -// The destructor destroys the Ogre scene nodes for the flags so they -// disappear from the 3D scene. The destructor for a Tool subclass is -// only called when the tool is removed from the toolbar with the "-" -// button. -TopmapEdgeTool::~TopmapEdgeTool() -{ -} - -// onInitialize() is called by the superclass after scene_manager_ and -// context_ are set. It should be called only once per instantiation. -// This is where most one-time initialization work should be done. -// onInitialize() is called during initial instantiation of the tool -// object. At this point the tool has not been activated yet, so any -// scene objects created should be invisible or disconnected from the -// scene at this point. -// -// In this case we load a mesh object with the shape and appearance of -// the flag, create an Ogre::SceneNode for the moving flag, and then -// set it invisible. -void TopmapEdgeTool::onInitialize() -{ - ros::NodeHandle nh; - ros::Rate r(10); - while(!ros::service::exists("/topmap_interface/add_edge", true)) - { - r.sleep(); - RCLCPP_INFO(logger_, "Waiting for add_edge service\n"); - } - addEdgeSrv_ = nh.serviceClient("/topmap_interface/add_edge", true); - markerPub_ = nh.advertise("edge_tool_marker", 0); - update_map_ = nh.advertise("/update_map", 5); -} - -// Activation and deactivation -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^ -// -// activate() is called when the tool is started by the user, either -// by clicking on its button in the toolbar or by pressing its hotkey. -void TopmapEdgeTool::activate() -{ -} - -// deactivate() is called when the tool is being turned off because -// another tool has been chosen. -void TopmapEdgeTool::deactivate() -{ - noClick_ = true; - edgeMarker_.action = visualization_msgs::msg::Marker::DELETE; - edgeMarker_.points.clear(); - edgeMarker_.header.stamp = ros::Time(); - markerPub_.publish(edgeMarker_); -} - -// Handling mouse events -// ^^^^^^^^^^^^^^^^^^^^^ -// -// processMouseEvent() is sort of the main function of a Tool, because -// mouse interactions are the point of Tools. -// -// We use the utility function rviz::getPointOnPlaneFromWindowXY() to -// see where on the ground plane the user's mouse is pointing. -int TopmapEdgeTool::processMouseEvent(rviz::ViewportMouseEvent& event) -{ - Ogre::Vector3 intersection; - Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); - if(rviz::getPointOnPlaneFromWindowXY(event.viewport, - ground_plane, - event.x, event.y, intersection)) - { - bool left = event.leftDown(); - bool right = event.rightDown(); - - geometry_msgs::msg::Pose event_pose = geometry_msgs::msg::Pose(); - event_pose.position.x = intersection.x; - event_pose.position.y = intersection.y; - event_pose.position.z = intersection.z; - - if (edgeMarker_.points.size() > 1) { - // Remove the second pose in points vector, with the previous pose of the - // mouse. - edgeMarker_.points.pop_back(); - } - - if (!noClick_){ - // If the first click happened, then we update the endpoint of the arrow each time the mouse moves. - edgeMarker_.points.push_back(event_pose.position); - edgeMarker_.header.stamp = ros::Time(); - edgeMarker_.action = visualization_msgs::msg::Marker::ADD; - markerPub_.publish(edgeMarker_); - } - - if (right || left){ - if (noClick_){ - firstClick_ = event_pose; - edgeMarker_.points.push_back(event_pose.position); - noClick_ = false; - } else { - // On the second click, send the edge to the service to be added to the - // map, and then reset the poses. - topological_navigation_msgs::AddEdgeRviz srv; - srv.request.first = firstClick_; - srv.request.second = event_pose; - srv.request.max_distance = 5.0; // be relatively accurate with clicks - // if left clicked, add bidirectional edge - srv.request.bidirectional = right ? false : true; - - if (addEdgeSrv_.call(srv)){ - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully added edge: %s", srv.response.message.c_str()); - std_msgs::Time t; - t.data = ros::Time::now(); - update_map_.publish(t); - } else { - RCLCPP_INFO(logger_, "Failed to add edge: %s", srv.response.message.c_str()); - } - } else { - RCLCPP_WARN(logger_, "Failed to add edge: %s", srv.response.message.c_str()); - } - edgeMarker_.action = visualization_msgs::msg::Marker::DELETE; - edgeMarker_.points.clear(); - edgeMarker_.header.stamp = ros::Time(); - markerPub_.publish(edgeMarker_); - noClick_ = true; - } - return Render; - } - } - - return Render; -} -} // end namespace topological_rviz_tools - -#include -PLUGINLIB_EXPORT_CLASS(topological_rviz_tools::TopmapEdgeTool,rviz::Tool) diff --git a/topological_rviz_tools/src/topological_map_panel.cpp b/topological_rviz_tools/src/topological_map_panel.cpp deleted file mode 100644 index 7ea6d445..00000000 --- a/topological_rviz_tools/src/topological_map_panel.cpp +++ /dev/null @@ -1,273 +0,0 @@ -#include "topological_rviz_tools/topological_map_panel.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace topological_rviz_tools -{ -TopologicalMapPanel::TopologicalMapPanel(QWidget* parent) - : rviz::Panel(parent) - , topmap_man_(NULL) -{ - properties_view_ = new rviz::PropertyTreeWidget(); - - ros::NodeHandle nh; - ros::Rate r(10); - while(!ros::service::exists("/topological_map_manager/remove_topological_node", true)) - { - r.sleep(); - RCLCPP_INFO(logger_, "Waiting for remove_topological_node service\n"); - } - while(!ros::service::exists("/topological_map_manager/add_tag_to_node", true)) - { - r.sleep(); - RCLCPP_INFO(logger_, "Waiting for add_tag_to_node service\n"); - } - - while(!ros::service::exists("/topological_map_manager/rm_tag_from_node", true)) - { - r.sleep(); - RCLCPP_INFO(logger_, "Waiting for rm_tag_from_node service\n"); - } - - while(!ros::service::exists("/topological_map_manager/remove_edge", true)) - { - r.sleep(); - RCLCPP_INFO(logger_, "Waiting for remove_edge service\n"); - } - delNodeSrv_ = nh.serviceClient("/topological_map_manager/remove_topological_node", true); - addTagSrv_ = nh.serviceClient("/topological_map_manager/add_tag_to_node", true); - delTagSrv_ = nh.serviceClient("/topological_map_manager/rm_tag_from_node", true); - delEdgeSrv_ = nh.serviceClient("/topological_map_manager/remove_edge", true); - update_map_ = nh.advertise("/update_map", 5); - - QPushButton* add_tag_button = new QPushButton("Add tag"); - QPushButton* remove_button = new QPushButton("Remove"); - - QHBoxLayout* button_layout = new QHBoxLayout; - button_layout->addWidget(add_tag_button); - button_layout->addWidget(remove_button); - button_layout->setContentsMargins(2, 0, 2, 2); - - QVBoxLayout* main_layout = new QVBoxLayout; - main_layout->setContentsMargins(0,0,0,0); - main_layout->addWidget(properties_view_); - main_layout->addLayout(button_layout); - setLayout(main_layout); - - connect(remove_button, SIGNAL(clicked()), this, SLOT(onDeleteClicked())); - connect(add_tag_button, SIGNAL(clicked()), this, SLOT(onAddTagClicked())); - // connect(properties_view_, SIGNAL(clicked(const QModelIndex&)), this, SLOT(setCurrentViewFromIndex(const QModelIndex&))); - // connect(properties_view_, SIGNAL(activated(const QModelIndex&)), this, SLOT(setCurrentViewFromIndex(const QModelIndex&))); -} - -void TopologicalMapPanel::onInitialize() -{ - RCLCPP_INFO(logger_, "Topmapmanel::OnInitialise"); - setTopmapManager(new TopmapManager(NULL)); - // connect topological map update caller to the nodecontroller so we can - // update on changes to the nodes - connect(topmap_man_->getController(), SIGNAL(childModified()), this, SLOT(updateTopMap())); -} - -void TopologicalMapPanel::setTopmapManager(TopmapManager* topmap_man) -{ - RCLCPP_INFO(logger_, "Setting model"); - properties_view_->setModel(topmap_man->getPropertyModel()); - topmap_man_ = topmap_man; - - // connect(camera_type_selector_, SIGNAL(activated(int)), this, SLOT(onTypeSelectorChanged(int))); - // connect(topmap_man_, SIGNAL(currentChanged()), this, SLOT(onCurrentChanged())); - // onCurrentChanged(); -} - -void TopologicalMapPanel::onDeleteClicked() -{ - QList nodes_to_delete = properties_view_->getSelectedObjects(); - QList tags_to_delete = properties_view_->getSelectedObjects(); - QList edges_to_delete = properties_view_->getSelectedObjects(); - - if (nodes_to_delete.size() + tags_to_delete.size() + edges_to_delete.size() == 0){ - return; - } - - NodeController* controller = topmap_man_->getController(); - - QMessageBox box; - char* buf = new char[100]; - sprintf(buf, "Delete %d nodes, %d edges and %d tags?", nodes_to_delete.size(), edges_to_delete.size(), tags_to_delete.size()); - std::string info_msg = buf; - delete buf; - - box.setText("You requested removal of objects."); - box.setInformativeText(QString::fromStdString(info_msg)); - box.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); - box.setDefaultButton(QMessageBox::Cancel); - int ret = box.exec(); - - if (ret == QMessageBox::Cancel){ - return; - } - - for(int i = 0; i < nodes_to_delete.size(); i++) { - topological_navigation_msgs::RmvNode srv; - srv.request.name = nodes_to_delete[i]->getValue().toString().toStdString().c_str(); - - if (delNodeSrv_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully removed node %s", srv.request.name.c_str()); - } else { - RCLCPP_INFO(logger_, "Failed to remove node %s", srv.request.name.c_str()); - } - } else { - RCLCPP_INFO(logger_, "Failed to get response from server to remove node %s", srv.request.name.c_str()); - } - } - - for(int i = 0; i < tags_to_delete.size(); i++) { - topological_navigation_msgs::AddTag srv; - srv.request.tag = tags_to_delete[i]->getString().toStdString().c_str(); - // go over all nodes in the controller and see which one this tag is from so - // we can extract the node name - std::string node_name; - for (int nd; nd < controller->numChildren(); nd++) { - if (controller->childAt(nd)->isAncestorOf(tags_to_delete[i])){ - srv.request.node.push_back(controller->childAt(nd)->getValue().toString().toStdString()); - break; - } - } - - if (delTagSrv_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully removed tag %s from node %s", srv.request.tag.c_str(), srv.request.node[0].c_str()); - } else { - RCLCPP_INFO(logger_, "Failed to remove tag %s from node %s", srv.request.tag.c_str(), srv.request.node[0].c_str()); - } - } else { - RCLCPP_INFO(logger_, "Failed to get response from server to remove tag %s from node %s", srv.request.tag.c_str(), srv.request.node[0].c_str()); - } - } - - for(int i = 0; i < edges_to_delete.size(); i++) { - topological_navigation_msgs::AddEdge srv; - srv.request.edge_id = edges_to_delete[i]->getEdgeId().c_str(); - - if (delEdgeSrv_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully removed edge %s", srv.request.edge_id.c_str()); - - } else { - RCLCPP_INFO(logger_, "Failed to remove edge %s", srv.request.edge_id.c_str()); - } - } else { - RCLCPP_INFO(logger_, "Failed to get response from server to remove edge %s", srv.request.edge_id.c_str()); - } - } - - // Update topological map only once after we remove all the stuff, to prevent - // update spam. - updateTopMap(); - -} - -void TopologicalMapPanel::onAddTagClicked() -{ - QList nodes = properties_view_->getSelectedObjects(); - - QString tag = QInputDialog::getText(this, tr("Add tag"), - tr("Tag to add:")); - - if (tag.toStdString().empty()){ - return; - } - - topological_navigation_msgs::AddTag srv; - srv.request.tag = tag.toStdString().c_str(); - - for(int i = 0; i < nodes.size(); i++) - { - srv.request.node.push_back(nodes[i]->getValue().toString().toStdString().c_str()); - if (nodes[i]->getTagController()->getHidden()){ - nodes[i]->getTagController()->setHidden(false); - } - } - - if (addTagSrv_.call(srv)) { - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully added tag \"%s\" to %d nodes", srv.request.tag.c_str(), nodes.size()); - updateTopMap(); - } else { - RCLCPP_INFO(logger_, "Failed to add tag \"%s\" to %d nodes: %s", srv.request.tag.c_str(), nodes.size(), srv.response.meta.c_str()); - } - } else { - RCLCPP_WARN(logger_, "Failed to get response from service to add tag \"%s\" to %d nodes", srv.request.tag.c_str(), nodes.size()); - } -} - - -void TopologicalMapPanel::renameSelected() -{ - // QList views_to_rename = properties_view_->getSelectedObjects(); - // if(views_to_rename.size() == 1) - // { - // NodeProperty* view = views_to_rename[ 0 ]; - - // // TODO: should eventually move to a scheme where the CURRENT view - // // is not in the same list as the saved views, at which point this - // // check can go away. - // if(view == topmap_man_->getCurrent()) - // { - // return; - // } - - // QString old_name = view->getName(); - // QString new_name = QInputDialog::getText(this, "Rename View", "New Name?", QLineEdit::Normal, old_name); - - // if(new_name.isEmpty() || new_name == old_name) - // { - // return; - // } - - // view->setName(new_name); - // } -} - -void TopologicalMapPanel::onCurrentChanged() -{ - //QString formatted_class_id = NodeController::formatClassId(topmap_man_->getCurrent()->getClassId()); - // RCLCPP_INFO("CUrrent changed"); - // properties_view_->setAnimated(false); - // topmap_man_->getCurrent()->expand(); - // properties_view_->setAnimated(true); -} - -void TopologicalMapPanel::updateTopMap(){ - RCLCPP_INFO(logger_, "updating topmap"); - std_msgs::Time t; - t.data = ros::Time::now(); - update_map_.publish(t); -} - -void TopologicalMapPanel::save(rviz_common::Config config) const -{ - rviz::Panel::save(config); - properties_view_->save(config); -} - -void TopologicalMapPanel::load(const rviz_common::Config& config) -{ - rviz::Panel::load(config); - properties_view_->load(config); -} - -} // namespace topological_rviz_tools - -#include -PLUGINLIB_EXPORT_CLASS(topological_rviz_tools::TopologicalMapPanel, rviz::Panel) diff --git a/topological_rviz_tools/src/topological_node_tool.cpp b/topological_rviz_tools/src/topological_node_tool.cpp deleted file mode 100644 index 6a25bce3..00000000 --- a/topological_rviz_tools/src/topological_node_tool.cpp +++ /dev/null @@ -1,138 +0,0 @@ -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include "topological_rviz_tools/topological_node_tool.hpp" - -namespace topological_rviz_tools -{ - -// BEGIN_TUTORIAL -// Construction and destruction -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -// -// The constructor must have no arguments, so we can't give the -// constructor the parameters it needs to fully initialize. -// -// Here we set the "shortcut_key_" member variable defined in the -// superclass to declare which key will activate the tool. -TopmapNodeTool::TopmapNodeTool() -{ - shortcut_key_ = 'n'; -} - -// The destructor destroys the Ogre scene nodes for the flags so they -// disappear from the 3D scene. The destructor for a Tool subclass is -// only called when the tool is removed from the toolbar with the "-" -// button. -TopmapNodeTool::~TopmapNodeTool() -{ -} - -// onInitialize() is called by the superclass after scene_manager_ and -// context_ are set. It should be called only once per instantiation. -// This is where most one-time initialization work should be done. -// onInitialize() is called during initial instantiation of the tool -// object. At this point the tool has not been activated yet, so any -// scene objects created should be invisible or disconnected from the -// scene at this point. -// -// In this case we load a mesh object with the shape and appearance of -// the flag, create an Ogre::SceneNode for the moving flag, and then -// set it invisible. -void TopmapNodeTool::onInitialize() -{ - - ros::NodeHandle nh; - ros::Rate r(10); - while(!ros::service::exists("/topological_map_manager/add_topological_node", true)) - { - r.sleep(); - RCLCPP_INFO(logger_, "Waiting for add_topological_node service\n"); - } - - addNodeSrv_ = nh.serviceClient("/topological_map_manager/add_topological_node", true); - update_map_ = nh.advertise("/update_map", 5); - -} - -// Activation and deactivation -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^ -// -// activate() is called when the tool is started by the user, either -// by clicking on its button in the toolbar or by pressing its hotkey. -void TopmapNodeTool::activate() -{ -} - -// deactivate() is called when the tool is being turned off because -// another tool has been chosen. -void TopmapNodeTool::deactivate() -{ -} - -// Handling mouse events -// ^^^^^^^^^^^^^^^^^^^^^ -// -// processMouseEvent() is sort of the main function of a Tool, because -// mouse interactions are the point of Tools. -// -// We use the utility function rviz::getPointOnPlaneFromWindowXY() to -// see where on the ground plane the user's mouse is pointing. -int TopmapNodeTool::processMouseEvent(rviz::ViewportMouseEvent& event) -{ - Ogre::Vector3 intersection; - Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); - if(rviz::getPointOnPlaneFromWindowXY(event.viewport, - ground_plane, - event.x, event.y, intersection)) - { - bool left = event.leftDown(); - bool right = event.rightDown(); - - if (right || left) { - geometry_msgs::msg::Pose clicked = geometry_msgs::msg::Pose(); - clicked.position.x = intersection.x; - clicked.position.y = intersection.y; - clicked.position.z = intersection.z; - clicked.orientation.w = 1.0; - // On the second click, send the edge to the service to be added to the - // map, and then reset the poses. - topological_navigation_msgs::AddNode srv; - srv.request.pose = clicked; - - // If rmb pressed, don't add edges to nodes close to the new node location - if (right){ - srv.request.add_close_nodes = false; - } - - if (addNodeSrv_.call(srv)){ - if (srv.response.success) { - RCLCPP_INFO(logger_, "Successfully added node"); - std_msgs::Time t; - t.data = ros::Time::now(); - update_map_.publish(t); - } else { - RCLCPP_INFO(logger_, "Failed to add node"); - } - } else { - RCLCPP_WARN(logger_, "Failed to connect to add node service"); - } - return Render | Finished; - } - } - return Render; -} - -} // end namespace topological_rviz_tools - -#include -PLUGINLIB_EXPORT_CLASS(topological_rviz_tools::TopmapNodeTool, rviz::Tool) diff --git a/topological_utils/CHANGELOG.rst b/topological_utils/CHANGELOG.rst deleted file mode 100644 index afcfe9c3..00000000 --- a/topological_utils/CHANGELOG.rst +++ /dev/null @@ -1,536 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package topological_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -3.0.5 (2024-01-15) ------------------- - -3.0.4 (2023-12-06) ------------------- -* Merge branch 'humble-dev' of github.com:LCAS/topological_navigation into humble-dev -* Contributors: GPrathap - -3.0.3 (2023-11-24) ------------------- - -3.0.2 (2023-11-24) ------------------- - -3.0.1 (2023-11-23) ------------------- -* Merge pull request `#160 `_ from Iranaphor/humble-dev - ROS 2 / Humble - porting -* Centralising topological_navigation stack messages to msgs package. (+version number sync across packages to 3.0.0) -* Updates to Maintainers and Authors -* full build working for topological_utils -* CMakeLists changes + restructuring for topological_utils -* initial work towards a working ros2 toponav -* Merge pull request `#156 `_ from LCAS/python3_marc_greg - Some Python3 and multi-robot fixes -* python 3 fix for goal converter -* map_converter py3 compatible -* Merge pull request `#154 `_ from adambinch/lcas - Auto set goal when adding an edge to the topological map -* Auto set goal when adding an edge to the topological map - If a file e.g. thorvald_navigation_actions/config/inrownavGoal.yaml exists then the map manager will automatically set the goal of the action to the goal specified in that file, when the add edge function/service is called with arg action_type set to thorvald_navigation_actions/inrownavGoal. If that arg is not set or the file does not exist then a move base type goal is assumed. -* Merge pull request `#152 `_ from adambinch/lcas - Goal Converter -* update with saga -* Merge pull request `#139 `_ from SAGARobotics/lcas - Update LCAS branch with SAGA branch -* clear route markers for previous route -* toponav launch files use new map visualiser -* update -* Removing mongodb as a dependency. - Removed from `topological_navigation` and `topological_utils` packages. -* Contributors: Adam Binch, James Heselden, JamesH, Marc Hanheide, adambinch - -2.4.0 (2022-01-25) ------------------- -* Merge pull request `#133 `_ from adambinch/melodic-devel - Map manager improvements -* update -* update -* new params namespaced -* Merge pull request `#118 `_ from adambinch/fix_dependencies - Fixing dependencies. -* Fixing dependencies. - `topological_map_edition.launch` moved to `topological_utils` package, which depends on `topological_rviz_tools` package. - `topological_navigation` package depends on `topological_navigation_msgs` package. -* Merge pull request `#117 `_ from LCAS/noetic - Basic navigation works in ros noetic but not all scripts are converted to python 3 -* python 3 compatible for most parts of toponav but not all! -* Merge pull request `#116 `_ from adambinch/final_things - A few final things. -* A few final things. - Set `advertise_srvs` arg to False when initialising the map manager 2 allows other scripts/nodes to use its functions without advertising 20+ services. - Descriptions of fail policy actions added to `UpdateFailPolicy.srv`. - Tidying. -* Merge pull request `#111 `_ from adambinch/remove_strands_dependencies - Removing strands navigation dependencies from topological navigation. -* Merge branch 'master' of github.com:LCAS/topological_navigation into remove_strands_dependencies -* Merge pull request `#113 `_ from gpdas/fixes - Mostly additive. - Minor fixes -* plot tmap2 from topic and yaml files -* strands dependencies removed from `topological_navigation/topological_utils` -* update -* Merge branch 'master' of github.com:LCAS/topological_navigation into francescodelduchetto-toponav2-restrictions -* Merge pull request `#101 `_ from adambinch/new_topics - New topics -* sleep in map converter -* Merge pull request `#100 `_ from adambinch/toponav2_launch - Launch files toponav 2 ready -* Making launch files toponav 2 ready. - Updated rviz config. - Tidying of nav script. -* Merge branch 'master' of github.com:LCAS/topological_navigation into toponav2_launch -* Merge branch 'master' of github.com:LCAS/topological_navigation into faster_route_search2 -* Merge branch 'toponav2-devel-restrictions' of github.com:francescodelduchetto/topological_navigation into toponav2-devel -* Merge branch 'master' of https://github.com/adambinch/topological_navigation into adam-master -* Contributors: Adam Binch, Gautham P Das, MikHut, adambinch, francescodelduchetto, gpdas - -2.3.0 (2021-07-15) ------------------- -* Merge pull request `#94 `_ from adambinch/tmap_to_tmap2 - Script for converting all tmaps found in repo to tmap2 format and script for populating tmap2s with params from edge reconfigure config files. -* Code simplification -* Script for populating tmap2s with edge reconfigure params from edge reconfigure groups config files. -* Moved map converter to topological_utils package -* Merge branch 'master' of github.com:LCAS/topological_navigation into tmap_to_tmap2 -* Merge pull request `#82 `_ from adambinch/fix_conflicts - Fix conflicts -* Merge branch 'master' of github.com:LCAS/topological_navigation into fix_conflicts - # Conflicts: - # topological_navigation/scripts/execute_policy_server.py - # topological_navigation/scripts/navigation.py -* Merge pull request `#25 `_ from heuristicus/ms-add-missing-scripts - add missing scripts to cmakelists -* add missing scripts to cmakelists -* Contributors: Adam Binch, Ayush Sharma, Michal Staniaszek, adambinch - -* Merge pull request `#94 `_ from adambinch/tmap_to_tmap2 - Script for converting all tmaps found in repo to tmap2 format and script for populating tmap2s with params from edge reconfigure config files. -* Code simplification -* Script for populating tmap2s with edge reconfigure params from edge reconfigure groups config files. -* Moved map converter to topological_utils package -* Merge branch 'master' of github.com:LCAS/topological_navigation into tmap_to_tmap2 -* Merge pull request `#82 `_ from adambinch/fix_conflicts - Fix conflicts -* Merge branch 'master' of github.com:LCAS/topological_navigation into fix_conflicts - # Conflicts: - # topological_navigation/scripts/execute_policy_server.py - # topological_navigation/scripts/navigation.py -* Merge pull request `#25 `_ from heuristicus/ms-add-missing-scripts - add missing scripts to cmakelists -* add missing scripts to cmakelists -* Contributors: Adam Binch, Ayush Sharma, Michal Staniaszek, adambinch - -2.1.0 (2020-04-20) ------------------- - -2.0.0 (2020-04-08) ------------------- - -1.1.1 (2020-04-08) ------------------- -* Merge pull request `#2 `_ from Jailander/master - Importing original version of topological navigation -* Merge branch 'temp_topoutils_only' of ../strands_navigation -* moving files into the correct folder -* Contributors: Marc Hanheide, jailander - -1.1.0 (2019-11-27) ------------------- -* scripts to add/remove tags to yaml topomaps (`#381 `_) - * scripts to add/remove tags to yaml topomaps - usage: - add_node_tags.py out_yaml_topomap> - remove_node_tags.py out_yaml_topomap> - takes a yaml formatted config file for node tags. - example - ``` - charging_node: - - WayPoint10 - door_transitions: - - WayPoint4 - - WayPoint5 - - WayPoint24 - - WayPoint25 - ``` - * bug fix and cleanup -* Merge pull request `#380 `_ from gpdas/plot_yaml_fix - fixed remaining rospy references -* fixed remaining rospy references -* Merge pull request `#379 `_ from gpdas/plot_topo_map - new script to plot topomaps from yaml files -* fix strip_str - removes strip_str from node_name only if the full string is in the node_name -* removed ros dependency -* install target added for new script -* new script to plot topomaps from yaml files - 1. plot_yaml.py - 2. minor modification to plot_topo_map.py -* Contributors: Gautham P Das, Jaime Pulido Fentanes, gpdas - -1.0.8 (2019-06-04) ------------------- -* Merge branch 'indigo-devel' of https://github.com/strands-project/strands_navigation into indigo-devel -* Merge pull request `#370 `_ from gpdas/plot_topo_map - plot_topo_map -* minor change -* plot_topo_map - A script for plotting the current topological map being published to /topological_map topic -* Corrected battery namespaces for localise by topic -* Merge pull request `#369 `_ from strands-project/ori-indigo-devel - Support for multi-robot and different global planners -* Merge remote-tracking branch 'ori/indigo-devel' into indigo-devel - Bringing in changes from ORI for multi-robot and different base planners. -* update dummy topo nav to use new feedback msg -* update of absolute/relative topic names for multi-robot setup -* Contributors: Bruno Lacerda, Jaime Pulido Fentanes, Nick Hawes, gpdas - -1.0.7 (2018-10-26) ------------------- -* Merge pull request `#354 `_ from gpdas/indigo-devel - tmap to yaml - adding meta info to nodes -* Code cleanup - Minor cleanup in usage information printing -* tmap to yaml export - adding meta info to nodes - When a yaml file is created from a tmap, it misses some tags and so is not as per the (yaml) format for topological map. So a yaml file exported from tmap cannot be imported to mongodb. - A small fix is done by adding some meta tag to the objects in the yaml file -* Contributors: Jaime Pulido Fentanes, gpdas - -1.0.6 (2018-07-17) ------------------- - -1.0.5 (2018-04-17) ------------------- -* Merge pull request `#349 `_ from mudrole1/indigo-devel - Adding waiting for the add_node service -* Fixed two arguments -* Contributors: Lenka Mudrova, Nick Hawes - -1.0.4 (2017-06-23) ------------------- -* Modifications to topological map tools to accommodate topological map editor (`#345 `_) - * fix weird space-colon - * Easier translational movement of waypoints, generic node field updater - Moving the waypoints that are displayed in the topological map in rviz is now - easier - just uses 2D planar motion as opposed to multiple handles for the x and - y dimensions. - Added a function which calls into the database to update any property of a node. - * Fixed not loading map after update, correctly updates edges on node rename - This should really not be the file being used - it seems like the one in util is - used to change things and as such is more up to date. - * remove unnecessary if - * update function for edge action and top_vel - * add deprecation warnings to topological_map.py - should use manager.py instead - * start on work to make manager services more useful for modifying map - * add callback for getting tags for a specific node - * partial switch to the using manager, updating and adding tags - * fix message fields and add messages to generation - * small script to insert empty map into a database - * add edge removal service - * change callbacks so that functions can be called without service -* Contributors: Michal Staniaszek - -1.0.3 (2017-01-11) ------------------- - -1.0.2 (2016-10-31) ------------------- -* makes sense -* now the parameters `/topological_prediction/success_values` and `/topological_prediction/fail_values` and be used to set the values considered for failures and successes -* Contributors: Jaime Pulido Fentanes - -1.0.1 (2016-06-21) ------------------- -* really fixing start now -* Contributors: Nick Hawes - -1.0.0 (2016-06-09) ------------------- -* More sensible starting point -* Contributors: Nick Hawes - -0.0.45 (2016-06-06) -------------------- - -0.0.44 (2016-05-30) -------------------- - -0.0.43 (2016-05-25) -------------------- -* Using pointset rather than map name. -* 0.0.42 -* updated changelogs -* 0.0.41 -* updated changelogs -* Using predictions for edge times -* Added ability to load dummy maps from yaml -* Better feedback timing as required by mdp exec. -* Aborting axserver on failure -* Simulating policy execution better. -* Contributors: Jenkins, Nick Hawes - -0.0.42 (2016-03-21) -------------------- - -0.0.41 (2016-03-03) -------------------- -* removing map name from query -* Contributors: Jaime Pulido Fentanes - -0.0.40 (2016-02-07) -------------------- -* adding missing install targets -* prediction changes -* Contributors: Jailander, Jaime Pulido Fentanes - -0.0.39 (2016-01-28) -------------------- -* removing prints and repeated node -* Fixes in topological utils -* Contributors: Jaime Pulido Fentanes - -0.0.38 (2015-11-17) -------------------- -* Extending the load yaml map functionality. Now based on a class in topological navigation to prevent circular test dependencies. -* Revert "Adding first version of topological test scenarios" -* Extending the load yaml map functionality. Now based on a class in topological navigation to prevent circular test dependencies. -* now you can launch topological navigation with an empty map (meaning no nodes) -* removing edge analysis -* Removed unnecessary import -* safety commit -* creating move base testing branch -* fixes on map exporting scripts -* minor fixes -* Contributors: Christian Dondrup, Jaime Pulido Fentanes, Nick Hawes - -0.0.37 (2015-08-26) -------------------- -* Fixed bug in dummy map where origin and ChargingPoint names were mixed up. -* Fix edge renaming. -* Fix node name check. -* Add utility to check map for errors. -* Add basic argument checking. -* Add utiltiy to automate renaming of map nodes. -* adding options for rotating and scaling the map and timezone management -* drawing maps in an epoch range -* coding expected speeds -* Compiles and visualises data based on nav predictions vs ground truth. -* added map_manager to create script -* added policy and prediction stuff to dummy system -* Added script to print out count of nav stats per edge -* removing unwanted file -* drawing predicted map -* map drawing utilities -* Contributors: Jailander, Jaime Pulido Fentanes, Nick Hawes, Rares Ambrus - -0.0.36 (2015-05-17) -------------------- - -0.0.35 (2015-05-10) -------------------- - -0.0.34 (2015-05-05) -------------------- -* Oops, that was almost embarrassing. -* Dummy system now sets top map name param. -* fixing insert yaml -* Contributors: Jaime Pulido Fentanes, Nick Hawes - -0.0.32 (2015-04-12) -------------------- -* fixing bug in insert map that I inserted myself -* Contributors: Jaime Pulido Fentanes - -0.0.31 (2015-04-10) -------------------- -* localisation by topic only works if the robot is in the influence zone of the node, migrate script now adds JSON string for localisation on ChargingPoint -* Fixing issues with topological Prediction -* second part of previous commit -* checking sanity on migrate scripts -* Contributors: Jaime Pulido Fentanes - -0.0.29 (2015-03-23) -------------------- -* adding install targets -* Contributors: Jaime Pulido Fentanes - -0.0.28 (2015-03-20) -------------------- -* removed scripts/LoadPointSet.py from install -* Contributors: Marc Hanheide - -0.0.27 (2015-03-19) -------------------- -* sending the robot to waypoint when in the influence area of the target node -* removing pointset b testing -* commiting migrate script plus typo fix -* map to Json utilities -* fixing bug by which undocking edge was not being created -* bug fixes -* Now waypoint to yaml automatically Includes ChargingPoint -* tmap_to_yaml.py now includes default values for edges -* Navigation and policy_executor working with new defs -* New map format export and insertion scripts -* committing map creation script -* Adding recovery behaviours to edges -* new branch created -* Contributors: Jailander, Jaime Pulido Fentanes - -0.0.26 (2015-03-18) -------------------- -* Forgot the install targets -* Contributors: Nick Hawes - -0.0.25 (2015-03-18) -------------------- -* Added the option to simulate time as an argument to the file. -* Renamed to .py to be consistent. -* Contributors: Nick Hawes - -0.0.24 (2015-03-17) -------------------- -* Fix in map to yaml -* Added a boolean value indicating whether the returned nodes are actual nodes in the topological map -* Clean up -* Print message -* Clean up -* returning nodes based on the mongodb node metadata -* Adding scripts for new file format -* Added map name to the service message -* Returning random data -* Adding topological node metadata query service - initial commit -* Added better handling of time for dummy navigation. -* Add list maps utility. -* Contributors: Chris Burbridge, Jailander, Nick Hawes, Rares Ambrus - -0.0.23 (2014-12-17) -------------------- - -0.0.22 (2014-11-26) -------------------- - -0.0.21 (2014-11-23) -------------------- - -0.0.20 (2014-11-21) -------------------- -* moving scripts here -* Contributors: Jaime Pulido Fentanes - -0.0.19 (2014-11-21) -------------------- - -0.0.18 (2014-11-21) -------------------- - -0.0.17 (2014-11-21) -------------------- - -0.0.16 (2014-11-21) -------------------- - -0.0.15 (2014-11-19) -------------------- -* fixing bug in top_map -* Contributors: Jaime Pulido Fentanes - -0.0.14 (2014-11-19) -------------------- -* adding new launch files for topological map creation -* Contributors: Jaime Pulido Fentanes - -0.0.12 (2014-11-17) -------------------- - -0.0.11 (2014-11-14) -------------------- - -0.0.10 (2014-11-14) -------------------- -* mapping launch files -* replanning when failing -* fixing influence areas on empty map -* Contributors: Jaime Pulido Fentanes - -0.0.9 (2014-11-12) ------------------- - -0.0.8 (2014-11-11) ------------------- - -0.0.6 (2014-11-06) ------------------- -* Corrected install locations. -* Contributors: Nick Hawes - -0.0.5 (2014-11-05) ------------------- -* Merge branch 'hydro-devel' of https://github.com/strands-project/strands_navigation into hydro-devel - Conflicts: - topological_utils/CMakeLists.txt -* adding install targets -* adding joystick creation of topological map -* Added launch file for dummy topological navigation and install targets. -* Added dummy script to stand in for topological navigation when missing a robot or proper simulation. - Useful for testing. -* Adding licences and bug fix -* Moved Vertex and Edge into strands_navigation_msgs. - Basic test for travel_time_tester passes. -* Contributors: Jaime Pulido Fentanes, Nick Hawes - -0.0.4 (2014-10-30) ------------------- - -0.0.3 (2014-10-29) ------------------- -* Merge pull request `#94 `_ from Jailander/hydro-devel - fixing mongodb_store deps -* fixing mongodb_store deps -* Contributors: Jaime Pulido Fentanes, Marc Hanheide - -0.0.2 (2014-10-29) ------------------- -* 0.0.1 -* added changelogs -* Adding install targets -* including visualization_msgs in package xml to sort `#83 `_ -* Adding Missing TopologicalMap.msg and changing maintainer emails, names and Licences for Packages -* scitos_apps_msgs has been removed. - All the imports were unused anyway. -* Renamed datacentre_ rosparams to mongodb_ -* Renamed ros_datacentre to mongodb_store - This simply bulk replaces all ros_datacentre strings to mongodb_store strings inside files and also in file names. - Needs `strands-project/ros_datacentre#76 `_ to be merged first. -* Adding add Node controller -* adding scripts to topological utils -* Adding Topological_map_manager -* now it is possible to edit the influence zones from rviz -* Adding an script for exporting the map to a text file -* Now Station is connected to WayPoint1 through `undocking` - ... not `docking` -* Improved waypoint to tmap script - Now when creating the topological map from a waypoint file it will add a - Charging node (ChargingPoint) at position {0,0,0,0,0,0,0} - (this waypoint can't be on the waypoint file) and this node will - be conected to the first waypoint in the file only using the - docking action -* Adding Node_to_IZ -* Small fix in topological map -* Now Topological Maps are stored in the topological_map collection -* Now is possible to move waypoints in Rviz using interactive marker and they will be updated on the ros_datacentre -* Adding topological map python class and edges marker array for visualisation of the topological map in Rviz -* Adding interactive markers to visualization -* Adding visualise_map.py tool -* adding max distance for edge creation between topological nodes -* Commit now vertex and Edge messages are capitalised, node message was moved to strands_navigation message - Using Message store proxy to store statistics -* Topological Navigation now works using message store proxy -* adding node message and move base reconfigure -* preliminary switch to ros_datacentre -* Adding Topological_Utils to repository -* Contributors: Bruno Lacerda, Christian Dondrup, Jaime Pulido Fentanes, Marc Hanheide, Nick Hawes diff --git a/topological_utils/CMakeLists.txt b/topological_utils/CMakeLists.txt deleted file mode 100644 index 3cbf2c4f..00000000 --- a/topological_utils/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(topological_utils) - -# Export information to downstream packages -ament_export_dependencies(rosidl_default_runtime) -ament_package() - diff --git a/topological_utils/LICENSE b/topological_utils/LICENSE deleted file mode 100644 index 261eeb9e..00000000 --- a/topological_utils/LICENSE +++ /dev/null @@ -1,201 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/topological_utils/launch/create_topological_map.launch b/topological_utils/launch/create_topological_map.launch deleted file mode 100644 index 6902399c..00000000 --- a/topological_utils/launch/create_topological_map.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_utils/launch/dummy_topological_navigation.launch b/topological_utils/launch/dummy_topological_navigation.launch deleted file mode 100644 index 4acce1ca..00000000 --- a/topological_utils/launch/dummy_topological_navigation.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_utils/launch/empty_topological_map.launch b/topological_utils/launch/empty_topological_map.launch deleted file mode 100644 index 3cc1d156..00000000 --- a/topological_utils/launch/empty_topological_map.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/topological_utils/launch/mapping.launch b/topological_utils/launch/mapping.launch deleted file mode 100644 index 97822a35..00000000 --- a/topological_utils/launch/mapping.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_utils/launch/topological_map_edition.launch b/topological_utils/launch/topological_map_edition.launch deleted file mode 100644 index 0c3dbc22..00000000 --- a/topological_utils/launch/topological_map_edition.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_utils/launch/topological_prediction_test.launch b/topological_utils/launch/topological_prediction_test.launch deleted file mode 100644 index a44ef00b..00000000 --- a/topological_utils/launch/topological_prediction_test.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/topological_utils/resource/topological_utils b/topological_utils/resource/topological_utils deleted file mode 100644 index e69de29b..00000000 diff --git a/topological_utils/setup.cfg b/topological_utils/setup.cfg deleted file mode 100644 index 828f2a0b..00000000 --- a/topological_utils/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/topological_utils -[install] -install_scripts=$base/lib/topological_utils diff --git a/topological_utils/setup.py b/topological_utils/setup.py deleted file mode 100644 index 79791073..00000000 --- a/topological_utils/setup.py +++ /dev/null @@ -1,64 +0,0 @@ -from setuptools import setup - -package_name = 'topological_utils' - -setup( - name=package_name, - version='3.0.5', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=[''], - zip_safe=True, - maintainer='Marc Hanheide', - maintainer_email='mhanheide@lincoln.ac.uk', - description='The ros2 topological_utils package', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'add_content.py = topological_navigation_utils.add_content:main.py', - 'add_edge.py = topological_navigation_utils.add_edge:main.py', - 'add_node.py = topological_navigation_utils.add_node:main.py', - 'add_node_tags.py = topological_navigation_utils.add_node_tags:main.py', - 'check_map = topological_navigation_utils.check_map.py', - 'crop_map.py = topological_navigation_utils.crop_map:main.py', - 'dummy_topological_navigation.py = topological_navigation_utils.dummy_topological_navigation:main.py', - 'draw_predicted_map.py = topological_navigation_utils.draw_predicted_map:main.py', - 'edge_length_analysis.py = topological_navigation_utils.edge_length_analysis:main.py', - 'edge_reconf_groups_to_tmap2.py = topological_navigation_utils.edge_reconf_groups_to_tmap2:main.py', - 'evaluate_predictions.py = topological_navigation_utils.evaluate_predictions:main.py', - 'insert_empty_map.py = topological_navigation_utils.insert_empty_map:main.py', - 'insert_map.py = topological_navigation_utils.insert_map:main.py', - 'joy_add_node.py = topological_navigation_utils.joy_add_node:main.py', - 'joy_add_waypoint.py = topological_navigation_utils.joy_add_waypoint:main.py', - 'list_maps = topological_navigation_utils.list_maps.py', - 'load_yaml_map.py = topological_navigation_utils.load_yaml_map:main.py', - 'load_json_map.py = topological_navigation_utils.load_json_map:main.py', - 'map_collection_change.py = topological_navigation_utils.map_collection_change:main.py', - 'map_export.py = topological_navigation_utils.map_export:main.py', - 'map_to_json.py = topological_navigation_utils.map_to_json:main.py', - 'map_to_yaml.py = topological_navigation_utils.map_to_yaml:main.py', - 'map_converter.py = topological_navigation_utils.map_converter:main.py', - 'migrate.py = topological_navigation_utils.migrate:main.py', - 'node_rm.py = topological_navigation_utils.node_rm:main.py', - 'node_metadata.py = topological_navigation_utils.node_metadata:main.py', - 'plot_topo_map.py = topological_navigation_utils.plot_topo_map:main.py', - 'plot_yaml.py = topological_navigation_utils.plot_yaml:main.py', - 'print_nav_stats.py = topological_navigation_utils.print_nav_stats:main.py', - 'rm_map_from_db.py = topological_navigation_utils.rm_map_from_db:main.py', - 'remove_node_tags.py = topological_navigation_utils.remove_node_tags:main.py', - 'rename_node = topological_navigation_utils.rename_node.py', - 'tmap_from_waypoints.py = topological_navigation_utils.tmap_from_waypoints:main.py', - 'tmap_to_yaml.py = topological_navigation_utils.tmap_to_yaml:main.py', - 'topological_map_update.py = topological_navigation_utils.topological_map_update:main.py', - 'visualise_map.py = topological_navigation_utils.visualise_map:main.py', - 'waypoints_to_yaml_tmap.py = topological_navigation_utils.waypoints_to_yaml_tmap:main.py' - ], - }, - -) - diff --git a/topological_utils/support/empty.tmap b/topological_utils/support/empty.tmap deleted file mode 100644 index a8b635de..00000000 --- a/topological_utils/support/empty.tmap +++ /dev/null @@ -1,28 +0,0 @@ -node: - ChargingPoint - waypoint: - 0,0,0,0,0,0,0 - edges: - Station, undocking - vertices: - 0.250000, 0.690000 - -0.075000, 0.690000 - -0.400000, 0.287000 - -0.400000,-0.287000 - -0.075000,-0.690000 - 0.250000,-0.690000 -node: - Station - waypoint: - -1.2,0.0,0.0,0,0,0.0,1 - edges: - ChargingPoint, docking - vertices: - 0.69, 0.287 - 0.287, 0.69 - -0.287, 0.69 - -0.69, 0.287 - -0.69, -0.287 - -0.287, -0.69 - 0.287, -0.69 - 0.69, -0.287 diff --git a/topological_utils/support/map_edition.rviz b/topological_utils/support/map_edition.rviz deleted file mode 100644 index a40a4f65..00000000 --- a/topological_utils/support/map_edition.rviz +++ /dev/null @@ -1,175 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /topology1 - Splitter Ratio: 0.7508310079574585 - Tree Height: 755 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 0.699999988079071 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /map - Unreliable: false - Use Timestamp: false - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /topological_map_visualisation - Name: MarkerArray - Namespaces: - /edges: true - /legend: true - /names: true - /nodes: true - /zones: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /topological_edges_policies - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: false - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /topological_map_markers/update - Value: false - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: false - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /topological_map_edges/update - Value: false - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: false - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /topological_map_zones/update - Value: false - Enabled: true - Name: topology - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Angle: 0 - Class: rviz/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 13.453375816345215 - Target Frame: - Value: TopDownOrtho (rviz) - X: 19.802148818969727 - Y: -9.416404724121094 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1052 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000037efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650200000921000000f60000013c000000c9fb0000000a0049006d00610067006501000002f4000000ca0000000000000000000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 1920 - Y: 0 diff --git a/topological_utils/topological_utils/scripts/__init__,py b/topological_utils/topological_utils/scripts/__init__,py deleted file mode 100644 index e69de29b..00000000 diff --git a/topological_utils/topological_utils/scripts/__init__.py b/topological_utils/topological_utils/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/topological_utils/topological_utils/scripts/add_content.py b/topological_utils/topological_utils/scripts/add_content.py deleted file mode 100755 index 8cd7b7c2..00000000 --- a/topological_utils/topological_utils/scripts/add_content.py +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import topological_navigation_msgs.srv - - -def content_client(node, content): - rospy.wait_for_service('/topological_map_manager/add_content_to_node') - try: - cont = rospy.ServiceProxy('/topological_map_manager/add_content_to_node', topological_navigation_msgs.srv.AddContent) - resp1 = cont(node, content) - return resp1 - except rospy.ServiceException as e: - return "Service call failed: %s"%e - - -if __name__ == "__main__": - print(sys.argv) - - filename=str(sys.argv[1]) - waypoint=str(sys.argv[2]) - #map_name=str(sys.argv[3]) - - json_data=open(filename, 'rb').read() - #text0 = "Hello ROS World" - print("Sending %s"%(json_data)) - resp=content_client(waypoint, json_data) - print(resp) - diff --git a/topological_utils/topological_utils/scripts/add_edge.py b/topological_utils/topological_utils/scripts/add_edge.py deleted file mode 100755 index 531bf1c0..00000000 --- a/topological_utils/topological_utils/scripts/add_edge.py +++ /dev/null @@ -1,32 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -#import pymongo - - -#from strands_navigation_msgs.msg import TopologicalNode -#from ros_datacentre.message_store import MessageStoreProxy -#from topological_navigation.topological_node import * -from topological_navigation.topological_map import * - -#import topological_navigation.msg - - - -class topologicalEdgeAdd(object): - - def __init__(self, pointset, or_waypoint, de_waypoint, action) : - - self.topo_map = topological_map(pointset) - self.topo_map.add_edge(or_waypoint, de_waypoint, action) - rospy.loginfo("All Done ...") - - -if __name__ == '__main__': - pointset=str(sys.argv[1]) - or_waypoint=str(sys.argv[2]) - de_waypoint=str(sys.argv[3]) - action = str(sys.argv[4]) - rospy.init_node('add_edge') - server = topologicalEdgeAdd(pointset,or_waypoint, de_waypoint, action) \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/add_node.py b/topological_utils/topological_utils/scripts/add_node.py deleted file mode 100755 index 5b4a7539..00000000 --- a/topological_utils/topological_utils/scripts/add_node.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -#import pymongo - -import std_msgs.msg -from geometry_msgs.msg import Pose - -#from strands_navigation_msgs.msg import TopologicalNode -#from ros_datacentre.message_store import MessageStoreProxy -#from topological_navigation.topological_node import * -from topological_navigation.topological_map import * - - -#import topological_navigation.msg - - - -class topologicalNodeAdd(object): - - def __init__(self, pointset, name, dist) : - - try: - pos = rospy.wait_for_message('robot_pose', Pose, timeout=10.0) - except rospy.ROSException : - rospy.logwarn("Failed to get robot pose") - return - - self.topo_map = topological_map(pointset) - self.topo_map.add_node(name,dist, pos, 'move_base') - - map_update = rospy.Publisher('/update_map', std_msgs.msg.Time) - map_update.publish(rospy.Time.now()) - rospy.loginfo("All Done ...") - - -if __name__ == '__main__': - pointset=str(sys.argv[1]) - name=str(sys.argv[2]) - dist=float(sys.argv[3]) - rospy.init_node('node_add') - server = topologicalNodeAdd(pointset,name,dist) \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/add_node_tags.py b/topological_utils/topological_utils/scripts/add_node_tags.py deleted file mode 100755 index 53887c28..00000000 --- a/topological_utils/topological_utils/scripts/add_node_tags.py +++ /dev/null @@ -1,50 +0,0 @@ -#! /usr/bin/env python -# -*- coding: utf-8 -*- -""" -Created on Thu Sep 12 20:21:38 2019 - -@author: gpdas - -""" - -import yaml -import sys -import os - - -def load_data_from_yaml(file_path): - assert os.path.exists(file_path) - - f_handle = open(file_path, "r") - f_data = yaml.load(f_handle) - f_handle.close() - return f_data - - -if __name__ == "__main__": - if len(sys.argv) < 4: - print("Usage: add_node_tags.py ") - else: - in_file = sys.argv[1] - out_file = sys.argv[2] - cfg_file = sys.argv[3] - - topo_map = load_data_from_yaml(in_file) - - tags = load_data_from_yaml(cfg_file) - node_tag = {} - for tag in tags: - for node in tags[tag]: - node_tag[node] = tag - - for node in topo_map: - if node["meta"]["node"] in node_tag: - if "tag" in node["meta"]: - if node_tag[node["meta"]["node"]] not in node["meta"]["tag"]: - node["meta"]["tag"].append(node_tag[node["meta"]["node"]]) - else: - node["meta"]["tag"] = [node_tag[node["meta"]["node"]]] - - with open(out_file, 'w') as f_out: - yaml.dump(topo_map, f_out, default_flow_style=False) - print("Successfully created the new file with node tags") diff --git a/topological_utils/topological_utils/scripts/check_map b/topological_utils/topological_utils/scripts/check_map deleted file mode 100755 index 3b90a9ef..00000000 --- a/topological_utils/topological_utils/scripts/check_map +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/python - -""" -Outputs a list of available topological maps -""" - -from topological_utils.queries import get_maps -from strands_navigation_msgs.msg import TopologicalNode -from mongodb_store.message_store import MessageStoreProxy -import sys - -if __name__ == "__main__": - if len(sys.argv)!=2: - print "Usage: check_map map_name" - sys.exit(1) - map_name = sys.argv[1] - print "Checking map '%s'" % map_name - maps = get_maps() - if map_name not in maps: - print "!!! MAP DOES NOT EXIST !!!" - sys.exit(1) - map_details = maps[map_name] - print "-" * 50 - print "Name: ", map_name - print "Number of nodes: ", map_details["number_nodes"] - print "Edge actions: ", str(list(map_details["edge_actions"])) - print "Last modified: ", map_details["last_modified"] - print "-" * 50 - - msg_store = MessageStoreProxy(collection='topological_maps') - - nodes = zip(*msg_store.query(TopologicalNode._type, {}, {'pointset':map_name}))[0] - node_names = [node.name for node in nodes] - - - - print "Checking consistency..." - valid = True - - count = map(lambda x: node_names.count(x), node_names) - duplicates = [node_names[i] for i, cnt in enumerate(count) if cnt > 1] - if len(duplicates) !=0: - print " !!! Duplicated nodes: %s" % ", ".join(set(duplicates)) - - edge_ids = [] - for node in nodes: - for edge in node.edges: - if edge.node not in node_names: - print " !!! Node '%s' has an edge to non-existent node '%s'" % (node.name, edge.node) - valid = False - if len(edge.edge_id) < 1: - print " !!! Node '%s' has an edge with a invalid blank id field" % node.name - valid = False - elif edge.edge_id in edge_ids: - print " !!! Node '%s' has an edge with a duplicate edge id '%s'." % (node.name, edge.edge_id) - valid = False - else: - edge_ids.append(edge.edge_id) - if not valid: - print "FAILED." - else: - print "PASSED." diff --git a/topological_utils/topological_utils/scripts/crop_map.py b/topological_utils/topological_utils/scripts/crop_map.py deleted file mode 100755 index 11fea119..00000000 --- a/topological_utils/topological_utils/scripts/crop_map.py +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/env python - -import sys -import yaml -from PIL import Image -import math - -def find_bounds(map_image): - x_min = map_image.size[0] - x_end = 0 - y_min = map_image.size[1] - y_end = 0 - pix = map_image.load() - for x in range(map_image.size[0]): - for y in range(map_image.size[1]): - val = pix[x, y] - if val != 205: # not unknown - x_min = min(x, x_min) - x_end = max(x, x_end) - y_min = min(y, y_min) - y_end = max(y, y_end) - return x_min, x_end, y_min, y_end - -def computed_cropped_origin(map_image, bounds, resolution, origin): - """ Compute the image for the cropped map when map_image is cropped by bounds and had origin before. """ - ox = origin[0] - oy = origin[1] - oth = origin[2] - - # First figure out the delta we have to translate from the lower left corner (which is the origin) - # in the image system - dx = bounds[0] * resolution - dy = (map_image.size[1] - bounds[3]) * resolution - - # Next rotate this by the theta and add to the old origin - - new_ox = ox + dx * math.cos(oth) - dy * math.sin(oth) - new_oy = oy + dx * math.sin(oth) + dy * math.cos(oth) - - return [new_ox, new_oy, oth] - -if __name__ == "__main__": - if len(sys.argv) < 2: - print >> sys.stderr, "Usage: %s map.yaml [cropped.yaml]" % sys.argv[0] - sys.exit(1) - - with open(sys.argv[1]) as f: - map_data = yaml.safe_load(f) - - if len(sys.argv) > 2: - crop_name = sys.argv[2] - if crop_name.endswith(".yaml"): - crop_name = crop_name[:-5] - crop_yaml = crop_name + ".yaml" - crop_image = crop_name + ".pgm" - else: - crop_yaml = "cropped.yaml" - crop_image = "cropped.pgm" - - map_image_file = map_data["image"] - resolution = map_data["resolution"] - origin = map_data["origin"] - - map_image = Image.open(map_image_file) - - bounds = find_bounds(map_image) - - # left, upper, right, lower - cropped_image = map_image.crop((bounds[0], bounds[2], bounds[1] + 1, bounds[3] + 1)) - - cropped_image.save(crop_image) - map_data["image"] = crop_image - map_data["origin"] = computed_cropped_origin(map_image, bounds, resolution, origin) - with open(crop_yaml, "w") as f: - yaml.dump(map_data, f) - diff --git a/topological_utils/topological_utils/scripts/draw_predicted_map.py b/topological_utils/topological_utils/scripts/draw_predicted_map.py deleted file mode 100755 index 528d6b68..00000000 --- a/topological_utils/topological_utils/scripts/draw_predicted_map.py +++ /dev/null @@ -1,314 +0,0 @@ -#! /usr/bin/env python - -import rospy -import yaml -import sys - -#from sensor_msgs.msg import Image -import cv2 -import cv - -from datetime import tzinfo, timedelta, datetime -import pytz -#from pytz import UTC - -import matplotlib as mpl -import matplotlib.cm as cm -import numpy as np - - -from geometry_msgs.msg import Point - -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation.tmap_utils import * -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation_msgs.msg import TopologicalNode -from nav_msgs.msg import OccupancyGrid -from topological_navigation_msgs.srv import * - - - - -def usage(): - print "\nFor one image of the current map:" - print "\t rosrun topological_utils draw_predicted_map.py" - print "\nFor one image of one specific timestamp:" - print "\t rosrun topological_utils draw_predicted_map.py -time epoch" - print "For images in a range use (every two hours):" - print "\t rosrun topological_utils draw_predicted_map.py -range from_epoch to_epoch" - print "\n\n" - #print "For all the stats from a date until now use:" - #print "\t rosrun topological_navigation topological_prediction.py -range from_epoch -1" - #print "For all the stats until one date:" - #print "\t rosrun topological_navigation topological_prediction.py -range 0 to_epoch" - #https://en.wikipedia.org/wiki/List_of_tz_database_time_zones - - -def predict_edges(epoch): - rospy.wait_for_service('topological_prediction/predict_edges') - try: - get_prediction = rospy.ServiceProxy('topological_prediction/predict_edges', topological_navigation_msgs.srv.PredictEdgeState) - print "Requesting prediction for %s"%epoch - resp1 = get_prediction(epoch) - return resp1 - except rospy.ServiceException as e: - print "Service call failed: %s"%e - - -def rotate_about_center(src, angle, scale=1.): - w = src.shape[1] - h = src.shape[0] - rangle = np.deg2rad(angle) # angle in radians - # now calculate new image width and height - nw = (abs(np.sin(rangle)*h) + abs(np.cos(rangle)*w))*scale - nh = (abs(np.cos(rangle)*h) + abs(np.sin(rangle)*w))*scale - # ask OpenCV for the rotation matrix - rot_mat = cv2.getRotationMatrix2D((nw*0.5, nh*0.5), angle, scale) - # calculate the move from the old center to the new center combined - # with the rotation - rot_move = np.dot(rot_mat, np.array([(nw-w)*0.5, (nh-h)*0.5,0])) - # the move only affects the translation, so update the translation - # part of the transform - rot_mat[0,2] += rot_move[0] - rot_mat[1,2] += rot_move[1] - return cv2.warpAffine(src, rot_mat, (int(math.ceil(nw)), int(math.ceil(nh))), flags=cv2.INTER_LANCZOS4) - - -def draw_arrow(image, V1, V2, color, origin, arrow_magnitude=5, thickness=1, line_type=8, shift=0): - xval = (int(V1.x/0.05))+origin[0] - yval = origin[1]+(int(V1.y/0.05)) - - V3 = Point() - V3.x = (V1.x+V2.x)/2 - V3.y = (V1.y+V2.y)/2 - p=(xval,yval) - - - xval2 = (int(V3.x/0.05))+origin[0]#map2d.info.resolution))+origin[0] - yval2 = origin[1]+(int(V3.y/0.05))#map2d.info.resolution)) - q=(xval2,yval2) - - # draw arrow tail - cv2.line(image, p, q, color, thickness, line_type, shift) - # calc angle of the arrow - angle = np.arctan2(p[1]-q[1], p[0]-q[0]) - # starting point of first line of arrow head - p = (int(q[0] + arrow_magnitude * np.cos(angle + np.pi/4)), - int(q[1] + arrow_magnitude * np.sin(angle + np.pi/4))) - # draw first half of arrow head - cv2.line(image, p, q, color, thickness, line_type, shift) - # starting point of second line of arrow head - p = (int(q[0] + arrow_magnitude * np.cos(angle - np.pi/4)), - int(q[1] + arrow_magnitude * np.sin(angle - np.pi/4))) - # draw second half of arrow head - cv2.line(image, p, q, color, thickness, line_type, shift) - - - -class DrawMap(object): - - def __init__(self, epoch, props): - freq =7200 - self.norm = mpl.colors.Normalize(vmin=0, vmax=100) - self.cmap = cm.jet - self.colmap = cm.ScalarMappable(norm=self.norm, cmap=self.cmap) - - map2d = self.get2dmap() - dat= [128 if i < 0 else ((100-i)*2.5) for i in map2d.data] - - points = self.loadTMap() - points.nodes.sort(key=lambda node: node.name) - map2dimg = self.draw_2d_map(dat, map2d.info) - - if 'rotate' in props: - self.rotate=props['rotate'] - else: - self.rotate=0 - - if 'scale' in props: - self.scale=props['rotate'] - else: - self.scale=1.0 - - - if 'tz' in props: - self.timezone = pytz.timezone(props['tz']) - else: - self.timezone = pytz.timezone('GMT') - - if len(epoch) <2: - self.epoch = epoch[0] - self.max_vel=0.01 - - est = predict_edges(self.epoch) - topmapimg = self.draw_top_map(points, map2d.info, map2dimg, est) - self.save_images(map2dimg, topmapimg) - else : - print "DO THIS" - print epoch - self.epoch = epoch[0] - - while self.epoch.secs <= epoch[1].secs: - print self.epoch.secs - self.max_vel=0.01 - est = predict_edges(self.epoch) - topmapimg = self.draw_top_map(points, map2d.info, map2dimg, est) - self.save_images(map2dimg, topmapimg) - self.epoch = self.epoch + rospy.Duration(freq) - #print est.edge_ids - - - def draw_2d_map(self, data, info): - imgdata = np.asarray(data, dtype = np.uint8) - imgdata = imgdata.reshape((info.height, info.width), order='C') - imgdata = cv2.cvtColor(imgdata, cv.CV_GRAY2BGRA); - return imgdata - - - def draw_top_map(self, points, info, map2dimg, est): - topmap_image = map2dimg.copy() - - origin=[] - origin.append(int(-(info.origin.position.x/info.resolution))) - origin.append(int(-(info.origin.position.y/info.resolution))) - - #thick=1 - for i in points.nodes: - V1=i.pose.position - for j in i.edges: - V2 = get_node(points, j.node).pose.position - indx= est.edge_ids.index(j.edge_id) - #print j.edge_id, est.probs[est.edge_ids.index(j.edge_id)], est.durations[est.edge_ids.index(j.edge_id)] - dist=math.hypot((V1.x-V2.x),(V1.y-V2.y)) - vel = (dist/float(est.durations[indx].secs)) - if vel > self.max_vel: - self.max_vel=round(vel,2) - - - for i in points.nodes: - V1=i.pose.position - for j in i.edges: - V2 = get_node(points, j.node).pose.position - indx= est.edge_ids.index(j.edge_id) - #print j.edge_id, est.probs[est.edge_ids.index(j.edge_id)], est.durations[est.edge_ids.index(j.edge_id)] - dist=math.hypot((V1.x-V2.x),(V1.y-V2.y)) - vel = round((dist/float(est.durations[indx].secs))/self.max_vel,2) - print j.edge_id, vel#cd - a= self.colmap.to_rgba(int(vel*100)) - thick = int(est.probs[indx]*5) - draw_arrow(topmap_image, V1, V2, (int(a[0]*255),int(a[1]*255),int(a[2]*255),255), origin, thickness=thick, arrow_magnitude=thick+5, line_type=1) - xval = (int(V1.x/info.resolution))+origin[0] - yval = origin[1]+(int(V1.y/info.resolution)) - #print xval, yval - cv2.circle(topmap_image, (int(xval), int(yval)), 10, (0,0,255,255), -1) - - - return topmap_image - - - def save_images(self, map2dimg, topmapimg): - topmapimg = cv2.flip(topmapimg, 0) - print topmapimg.shape - topmapimg = rotate_about_center(topmapimg, self.rotate, self.scale) - - size = topmapimg.shape[0]+200, topmapimg.shape[1]+200, 3 - - out_image = np.zeros(size, dtype=np.uint8) - out_image = cv2.cvtColor(out_image, cv.CV_BGR2BGRA); - - out_image[100:100+topmapimg.shape[0],100:100+topmapimg.shape[1]] = topmapimg - - - ts = datetime.fromtimestamp(self.epoch.secs, tz=pytz.UTC).astimezone(self.timezone).strftime('%a %d %b %Y %H:%M:%S (%Z)') - height, width, depth = out_image.shape - font = cv2.FONT_HERSHEY_SIMPLEX - size1 = cv2.getTextSize(ts, font, 2, 5) - hval = (width-size1[0][0])/2 - cv2.putText(out_image,ts,(hval, size1[0][1]+10), font, 2,(255,255,255,255),5) - - - size1 = cv2.getTextSize('Predicted Speed (m/s)', font, 1.5, 5) - hval = (width-size1[0][0])/2 - vval = height-95+size1[0][1] - cv2.putText(out_image,'Predicted Speed (m/s)',(hval, vval), font, 1.2,(255,255,255,255),3) - - step=(width*0.8)/100 - #step=int((width)/100) - barwend=(width*0.1) - print width, step, barwend - - bareend=width*0.9 - for i in range(0, 100): - a= self.colmap.to_rgba(i) - v1=int(round(barwend+(step*(i)))) - v2=int(round(v1+step)) - cv2.rectangle(out_image, (v1,height-50), (v2,height), (int(a[0]*255),int(a[1]*255),int(a[2]*255),255), -1) - - size1 = cv2.getTextSize('0', font, 1.5, 5) - hval = int(round(barwend))-size1[0][0] - vval = height-50+size1[0][1] - cv2.putText(out_image,'0',(hval, vval), font, 1.2,(255,255,255,255),3) - - size1 = cv2.getTextSize(str(self.max_vel), font, 1.5, 5) - hval = int(round(bareend)+5)#-size1[0][0] - vval = height-50+size1[0][1] - cv2.putText(out_image,str(self.max_vel),(hval, vval), font, 1.2,(255,255,255,255),3) - - #print barwend+step*(101) - #cv2.line(out_image,(barwend+(step*(101)),height-50), (barwend+(step*(101)),height), (255,255, 255, 255), 6) - datestr = datetime.fromtimestamp(self.epoch.secs).strftime('%Y-%m-%d_%H-%M-%S') - filename = 'predmap_'+datestr+'.png' - cv2.imwrite(filename,out_image) - - - - def get2dmap(self) : - try: - msg = rospy.wait_for_message('/map', OccupancyGrid, timeout=10.0) - return msg - except rospy.ROSException : - rospy.logwarn("Failed to get topological map") - return - - - def loadTMap(self) : - try: - msg = rospy.wait_for_message('topological_map', TopologicalMap, timeout=10.0) - return msg - except rospy.ROSException : - rospy.logwarn("Failed to get topological map") - return - - -if __name__ == '__main__': - rospy.init_node('draw_map') - epochs=[] - props={} - #if len(sys.argv) < 2: - if '-h' in sys.argv or '--help' in sys.argv: - usage() - sys.exit(1) - else: - if '-range' in sys.argv: - ind = sys.argv.index('-range') - epochs.append(rospy.Time.from_sec(float(sys.argv[ind+1]))) - epochs.append(rospy.Time.from_sec(float(sys.argv[ind+2]))) - print epochs - elif '-time' in sys.argv: - ind = sys.argv.index('-time') - epochs.append(rospy.Time.from_sec(float(sys.argv[ind+1]))) - print epochs - else: - epochs.append(rospy.Time.now()) - - if '-rotate' in sys.argv: - ind = sys.argv.index('-rotate') - props['rotate'] = float(sys.argv[ind+1]) - if '-scale' in sys.argv: - ind = sys.argv.index('-scale') - props['scale'] = float(sys.argv[ind+1]) - if '-tz' in sys.argv: - ind = sys.argv.index('-tz') - props['tz'] = str(sys.argv[ind+1]) - - server = DrawMap(epochs, props) diff --git a/topological_utils/topological_utils/scripts/dummy_topological_navigation.py b/topological_utils/topological_utils/scripts/dummy_topological_navigation.py deleted file mode 100755 index fbfdbb09..00000000 --- a/topological_utils/topological_utils/scripts/dummy_topological_navigation.py +++ /dev/null @@ -1,215 +0,0 @@ -#!/usr/bin/env python - -import sys -import topological_navigation.testing -import rospy -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation.manager import map_manager -from topological_navigation_msgs.msg import GotoNodeAction, GotoNodeResult, GotoNodeFeedback -from topological_navigation_msgs.msg import * -import actionlib -from actionlib_msgs.msg import GoalStatus -from std_msgs.msg import String -from topological_navigation.load_maps_from_yaml import YamlMapLoader -import random - -class DummyTopologicalNavigator(): - - def __init__(self, size, simulate_time, map_name=None): - if map_name is not None: - self.map_name = map_name - else: - self.map_name = self.create_and_insert_map(size = size) - - self.manager = map_manager(self.map_name) - self.node_names = set([node.name for node in self.manager.nodes.nodes]) - - self.nav_result = GotoNodeResult() - self.nav_feedback = GotoNodeFeedback() - self.nav_server = actionlib.SimpleActionServer('topological_navigation', GotoNodeAction, execute_cb = self.nav_callback, auto_start = False) - - self.policy_result = ExecutePolicyModeResult() - self.policy_feedback = ExecutePolicyModeFeedback() - self.policy_server = actionlib.SimpleActionServer('topological_navigation/execute_policy_mode', ExecutePolicyModeAction, execute_cb = self.policy_callback, auto_start = False) - - self.cn_pub = rospy.Publisher('current_node', String, queue_size=1) - self.cl_pub = rospy.Publisher('closest_node', String, queue_size=1) - self.cn = random.choice(list(self.node_names)) - self.simulate_time = simulate_time - - if self.simulate_time: - from topological_navigation_msgs.srv import PredictEdgeState - time_srv_name = 'topological_prediction/predict_edges' - try: - rospy.wait_for_service(time_srv_name, timeout=120) - self.time_srv = rospy.ServiceProxy(time_srv_name, PredictEdgeState) - response = self.time_srv(rospy.get_rostime()) - self.edge_times = {response.edge_ids[i]: response.durations[i] for i in range(len(response.edge_ids))} - # print self.edge_times - - except Exception, e: - print e - rospy.logwarn('travel time service not available') - - - - def start(self): - self.nav_server.start() - self.policy_server.start() - while not rospy.is_shutdown(): - self.cn_pub.publish(String(self.cn)) - self.cl_pub.publish(String(self.cn)) - rospy.sleep(1) - - - def policy_callback(self, goal): - - # print 'called with policy goal %s'%goal - - # this no longer seems valid - # target is the one which is not in the source list - # if len(goal.route.source) == 0: - # target_node = self.cn - # else: - # target_node = self.node_names - set(goal.route.source) - - - - target_node = None - - if self.cn in goal.route.source: - # get index of current state from source list, and map it to edge to take - edge = goal.route.edge_id[goal.route.source.index(self.cn)] - # split edge to get target waypoint - target_node = edge.split('_')[1] - - print target_node - - if self.simulate_time: - target = rospy.get_rostime() + self.edge_times[edge] - while not rospy.is_shutdown() and not self.policy_server.is_preempt_requested() and rospy.get_rostime() < target: - rospy.sleep(0.5) - else: - rospy.sleep(1) - - - if self.policy_server.is_preempt_requested(): - # print "done preempted" - self.policy_result.success = False - self.policy_server.set_preempted(self.policy_result) - elif target_node is None: - - - # print "done failed to find target node" - self.policy_result.success = False - self.policy_server.set_succeeded() - else: - # print "done normal" - - self.cn = target_node - - self.policy_feedback.current_wp = self.cn - self.policy_feedback.status = GoalStatus.SUCCEEDED - self.policy_server.publish_feedback(self.policy_feedback) - - rospy.sleep(0.1) - - self.policy_result.success = True - self.policy_server.set_succeeded(self.policy_result) - - # print "policy mode execution complete" - - - def nav_callback(self, goal): - # print 'called with nav goal %s'%goal.target - - self.nav_feedback.route = 'Starting...' - self.nav_server.publish_feedback(self.nav_feedback) - - self.nav_feedback.route = '%s to %s by %s' % (self.cn, goal.target, 'dummy_action') - self.nav_server.publish_feedback(self.nav_feedback) - - # wait for completion or prempt - - if self.simulate_time: - # target = rospy.get_rostime() + self.edge_times[edge] - # lack of edge info so faking for now - target = rospy.get_rostime() + rospy.Duration(20) - while not rospy.is_shutdown() and not self.nav_server.is_preempt_requested() and rospy.get_rostime() < target: - rospy.sleep(0.5) - else: - rospy.sleep(1) - - self.nav_feedback.route = goal.target - self.nav_server.publish_feedback(self.nav_feedback) - - if self.nav_server.is_preempt_requested(): - # print "done preempted" - self.nav_result.success = False - self.nav_server.set_preempted(self.nav_result) - else: - # print "done normal" - self.cn = goal.target - self.nav_result.success = True - self.nav_server.set_succeeded(self.nav_result) - - - # print "nav complete" - - def create_and_insert_map(self, size = 5, separation = 5.0): - self.nodes = topological_navigation.testing.create_line_map(width = size, nodeSeparation = separation) - self.top_map_store = MessageStoreProxy(collection='topological_maps') - - map_name = 'dummy_map' - - meta = {} - meta['map'] = map_name - meta['pointset'] = map_name - - for (node_name, node) in self.nodes.items(): - meta["node"] = node_name - node.map = meta['map'] - node.pointset = meta['pointset'] - self.top_map_store.insert(node,meta) - - return map_name - - - -if __name__ == '__main__': - rospy.init_node('dummy_topological_navigator') - size = rospy.get_param('~size', 20) - sim_times = rospy.get_param('~simulate_time', False) - map_name = rospy.get_param('~map', None) - map_file = rospy.get_param('~yaml_map', None) - - if map_file is not None and len(map_file) > 0: - rospy.loginfo('loading map from yaml: %s' % map_file) - map_loader = YamlMapLoader() - data = map_loader.read_maps(map_file) - map_loader.insert_maps(data=data, force=True) - map_name = data[0][0]['node']['pointset'] - rospy.set_param('topological_map_name', map_name) - - elif map_name is not None and len(map_name) > 0: - rospy.loginfo('simulating map: %s' % map_name) - rospy.set_param('topological_map_name', map_name) - - else: - rospy.set_param('topological_map_name', 'dummy_map') - - navigator = DummyTopologicalNavigator(size, sim_times, map_name) - navigator.start() - - - - - - - - - - - - - \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/edge_length_analysis.py b/topological_utils/topological_utils/scripts/edge_length_analysis.py deleted file mode 100755 index 5cba4bbf..00000000 --- a/topological_utils/topological_utils/scripts/edge_length_analysis.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import yaml -import math -import time -import json - -#import strands_navigation_msgs.msg -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation.tmap_utils import * -import mongodb_store.util as dc_util -from topological_navigation.tmap_utils import * -from geometry_msgs.msg import Point - - -""" - get_distance - - Returns the straight line distance between two poses -""" -def point_distance(point1, point2): - #dist=math.hypot((pose.position.x-node.pose[0].position.x),(pose.position.y-node.pose[0].position.y)) - dist=math.hypot((point1.x-point2.x),(point1.y-point2.y)) - return dist - - -def get_radius(node): - rad=0 - ori=Point() - ori.x=0.0 - ori.y=0.0 - ori.z=0.0 - for k in node.verts: - point = Point() - point.x = k.x - point.y = k.y - point.z = 0.0 - #print point, node.pose.position - vertrad = point_distance(ori, point) - #print vertrad, rad - rad+=vertrad - #print rad, len(node.verts) - rad=rad/len(node.verts) - return rad - -class EdgeAnalysis(object): - - def __init__(self, name) : - self.lnodes = [] - self.edgid=[] - self.eids = [] -# self.unknowns = [] - - self.map_received =False - rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) - rospy.loginfo("Waiting for Topological map ...") - - while not self.map_received: - pass - - self.get_list_of_edges() - - for i in self.eids: - print i - - yml = yaml.safe_dump(self.eids, default_flow_style=False) - - fh = open("edges.yaml", "w") - s_output = str(yml) -# print s_output - fh.write(s_output) - fh.close - - - """ - MapCallback - - This function receives the Topological Map - """ - def MapCallback(self, msg) : - self.lnodes = msg - self.map_received = True - #print self.lnodes - - - def get_list_of_edges(self): - self.eids = [] - - rospy.loginfo("Querying for list of edges") - for i in self.lnodes.nodes : - for j in i.edges: - if j.edge_id not in self.edgid : - self.edgid.append(j.edge_id) - val={} - val["edge_id"]=j.edge_id - #val["model_id"]=self.lnodes.name+'__'+j.edge_id - ori={} - ori['pos']= dc_util.msg_to_document(i.pose.position) - ori['name']=i.name - rad = get_radius(i) - ori['radius']=rad - val["ori"]=ori - - ddn=get_node(self.lnodes, j.node) - dest={} - dest['name']=j.node - dest['pos']= dc_util.msg_to_document(ddn.pose.position) - rad = get_radius(ddn) - dest['radius']=rad - val["dest"]=dest - val["dist"]= get_distance_to_node(i,ddn) - - self.eids.append(val) - fdbmsg = 'Done. %d edges found' %len(self.edgid) - rospy.loginfo(fdbmsg) - - - -if __name__ == '__main__': - rospy.init_node('edge_analisys') - server = EdgeAnalysis(rospy.get_name()) \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/edge_reconf_groups_to_tmap2.py b/topological_utils/topological_utils/scripts/edge_reconf_groups_to_tmap2.py deleted file mode 100755 index 0529e539..00000000 --- a/topological_utils/topological_utils/scripts/edge_reconf_groups_to_tmap2.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python -""" -Created on Tue Jul 6 13:07:18 2021 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -######################################################################################################### -import sys, rospy, yaml -from topological_navigation.manager2 import map_manager_2 - - -def load_yaml(filename): - with open(filename,'r') as f: - return yaml.load(f) - - -def edge_groups_to_tmap2(f_tmap2, groups, group_names): - - mm2 = map_manager_2(advertise_srvs=False) - mm2.init_map(filename=f_tmap2) - - for name in group_names: - rospy.loginfo("Adding parameters for edge group {}".format(name)) - group = groups["edge_nav_reconfig_groups"][name] - - for edge_id in group["edges"]: - for param in group["parameters"]: - if not isinstance(param["value"], str): - value = str(param["value"]) - value_is_string = False - else: - value = param["value"] - value_is_string = True - - mm2.add_param_to_edge_config(edge_id, param["ns"], param["name"], value, value_is_string, update=False, write_map=False) - - mm2.write_topological_map(f_tmap2) -######################################################################################################### - - -######################################################################################################### -if __name__ == '__main__' : - - if "-h" in sys.argv or "--help" in sys.argv or len(sys.argv) < 4: - print "usage is edge_reconf_groups_to_tmap2.py tmap.tmap2 edge_reconf_groups.yaml ['group_1', 'group_2',...,'group_n']" - sys.exit(1) - else: - print sys.argv - f_tmap2 = sys.argv[1] - f_groups = sys.argv[2] - group_names = sys.argv[3].strip('[]').split(',') - - print("Populating topological map 2 '{}' with parameters from edge reconfigure groups '{}'".format(f_tmap2, f_groups)) - - rospy.init_node("edge_reconf_groups_to_tmap2") - - groups = load_yaml(f_groups) - edge_groups_to_tmap2(f_tmap2, groups, group_names) -######################################################################################################### \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/evaluate_predictions.py b/topological_utils/topological_utils/scripts/evaluate_predictions.py deleted file mode 100755 index 17a4a1cc..00000000 --- a/topological_utils/topological_utils/scripts/evaluate_predictions.py +++ /dev/null @@ -1,362 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy -from topological_navigation_msgs.msg import * -from topological_navigation_msgs.srv import * -from mongodb_store.message_store import MessageStoreProxy -import actionlib -import pytz -from datetime import datetime, timedelta, time, date -import pymongo -import numpy as np -import matplotlib.pyplot as plt -from matplotlib.backends.backend_pdf import PdfPages -from math import fabs - -# import mongodb_store_msgs.srv as dc_srv -# import mongodb_store.util as dc_util -# from mongodb_store.message_store import MessageStoreProxy -# from datetime import datetime, timedelta, time, date - -# import matplotlib.patches as mpatches -# import numpy as np -# import matplotlib.pyplot as plt - -# import argparse -# import cmd - - -def model_client(): - #Creating monitored navigation client - rospy.loginfo("Getting model build server") - client = actionlib.SimpleActionClient('topological_prediction/build_temporal_model', BuildTopPredictionAction) - client.wait_for_server() - rospy.loginfo(" ...done") - return client - -def prediction_client(): - rospy.wait_for_service('topological_prediction/predict_edges') - return rospy.ServiceProxy('topological_prediction/predict_edges', PredictEdgeState) - - -def to_epoch(ptime): - return int((ptime - datetime(1970,1,1, tzinfo=ptime.tzinfo)).total_seconds()) - -def python_to_rostime(ptime): - return rospy.Time((ptime - datetime(1970,1,1, tzinfo=ptime.tzinfo)).total_seconds()) - -def evaluate_prediction(predictor, epoch, stats): - try: - response = predictor(rospy.Time(epoch)) - - errors = [] - - for nav_stat in stats: - # ignore the stay in one place transitions - if nav_stat.edge_id != 'none': - predicted_duration = response.durations[response.edge_ids.index(nav_stat.edge_id)] - actual_duration = nav_stat.operation_time - - predicted_success = response.probs[response.edge_ids.index(nav_stat.edge_id)] - actual_success = 1 if nav_stat.status == 'success' else 0 - - errors.append((nav_stat.edge_id, actual_duration - predicted_duration.to_sec(), fabs(actual_success - predicted_success))) - - return errors - - except rospy.ServiceException as e: - print "Service call failed: %s"%e - - -def write_stats(window_start, window_end, nav_stats): - - if len(nav_stats) == 0: - return - - filename = '{2}__{0}__{1}.csv'.format(window_start, window_end, nav_stats[-1][0].topological_map) - print 'Writing data for %s to %s, total stats %s, to %s' % (window_start, window_end, len(nav_stats), filename) - - # If these have time of start in the first column, success/fail in the - # second column and time it took to traverse in the third one, then I - # could try to create some nice stats out of it. - - with open(filename, 'w') as text_file: - for stat, meta in nav_stats: - if stat.edge_id != 'none': - text_file.write('{0}, {1}, {2}, {3}\n'.format(stat.edge_id, meta['epoch'], stat.status, stat.operation_time)) - - -def export_data(tmap, data_window_start, data_window_end, data_window_size, data_windows): - - - msg_store = MessageStoreProxy(collection='nav_stats') - - for window in range(data_windows): - - window_start = data_window_start + data_window_size * window - window_end = data_window_end + data_window_size * window - - # get nav stats for window - meta_query = {"epoch" : {"$gt": to_epoch(window_start), "$lt" : to_epoch(window_end)}} - nav_stats = msg_store.query(NavStatistics._type, {'topological_map': tmap}, meta_query) - - write_stats(window_start, window_end, nav_stats) - - - -def collect_predictions(predictor_name, now, tmap, training_window_start, training_window_end, training_window_size, training_windows, testing_window_start, testing_window_end): - - builder = model_client() - predictor = prediction_client() - - msg_store = MessageStoreProxy(collection='nav_stats') - - host = rospy.get_param('mongodb_host', 'localhost') - port = rospy.get_param('mongodb_port', '62345') - client = pymongo.MongoClient(host,port) - - results_collection = client['nav_results'][tmap] - - - - meta_query = {"epoch" : {"$gt": to_epoch(testing_window_start), "$lt" : to_epoch(testing_window_end)}} - test_stats = msg_store.query(NavStatistics._type, {'topological_map': tmap}, meta_query) - - test_epochs = {} - - test_stat_count = 0 - # group nav stats by epoch for faster processing later - for nav_stat, nav_stat_meta in test_stats: - if nav_stat_meta['epoch'] not in test_epochs: - # print nav_stat_meta['epoch'] - test_epochs[nav_stat_meta['epoch']] = [] - - test_epochs[nav_stat_meta['epoch']].append(nav_stat) - test_stat_count += 1 - - - print 'Testing with %s stats at %s different epochs' % (test_stat_count, len(test_epochs)) - - - processed = 0 - increment = 100 - increment_count = 100 - - for window in range(training_windows): - - window_start = training_window_start - window_end = training_window_end + training_window_size * window - - # get nav stats for window - meta_query = {"epoch" : {"$gt": to_epoch(window_start), "$lt" : to_epoch(window_end)}} - nav_stats = msg_store.query(NavStatistics._type, {'topological_map': tmap}, meta_query) - print 'Building model for window %s to %s, total stats %s' % (training_window_start, training_window_end + training_window_size * window, len(nav_stats)) - - build_goal = BuildTopPredictionGoal(python_to_rostime(window_start), python_to_rostime(window_end)) - builder.send_goal(build_goal) - builder.wait_for_result() - - # now test each nav stat from the windows - result_set = '%s___%s' % (window_start, window_end) - for epoch, stats in test_epochs.items(): - results = evaluate_prediction(predictor, epoch, stats) - result_doc = {'result_time': now, 'training_window_start': window_start, 'training_window_end': window_end, 'predictor' : predictor_name, 'epoch' : epoch, 'results': results} - results_collection.insert(result_doc) - processed += len(results) - - if processed > increment_count: - print 'Processed %s/%s' % (processed, test_stat_count) - increment_count += increment - - print 'Processed %s/%s' % (processed, test_stat_count) - - return now - - - - -edge_id_to_x = {} - -def visualise_window(result_time, predictor, results_collection, window_start, window_end, colour = 'b', label = None, duration_ax = None, success_ax = None): - # get all results for this set - # and build up answers by edge - edge_results = {} - edge_prob_results = {} - - query = {'result_time': result_time, 'predictor': predictor, 'training_window_start': window_start, 'training_window_end': window_end} - - print window_start - - for result in results_collection.find(query): - # duration - for edge_id, error, prob_error in result['results']: - if edge_id not in edge_results: - edge_results[edge_id] = [] - edge_prob_results[edge_id] = [] - - if edge_id not in edge_id_to_x: - edge_id_to_x[edge_id] = (len(edge_id_to_x) + 1) * 10 - - edge_results[edge_id].append(error) - edge_prob_results[edge_id].append(prob_error) - - # now we have a list of errors per edge - edge_ids = [] - real_edge_ids = [] - edge_means = [] - edge_stddevs = [] - - - for edge_id, edge_errors in edge_results.items(): - edge_errors = np.absolute(np.array(edge_errors)) - edge_ids.append(edge_id_to_x[edge_id]) - real_edge_ids.append(edge_id) - edge_means.append(edge_errors.mean()) - edge_stddevs.append(edge_errors.std()) - - edge_prob_means = [] - edge_prob_stddevs = [] - - # work out mean prediction - for edge_id in real_edge_ids: - edge_errors = np.array(edge_prob_results[edge_id]) - edge_prob_means.append(edge_errors.mean()) - edge_prob_stddevs.append(edge_errors.std()) - - # show edges and error - duration_ax.errorbar(edge_ids, edge_means, edge_stddevs, linestyle='None', marker='^', color = colour, label = label) - success_ax.errorbar(edge_ids, edge_prob_means, edge_prob_stddevs, linestyle='None', marker='^', color = colour, label = label) - - -def visualise_results(tmap, result_time): - # first reconstruct windows from the data - - host = rospy.get_param('mongodb_host', 'localhost') - port = rospy.get_param('mongodb_port', '62345') - client = pymongo.MongoClient(host,port) - - results_collection = client['nav_results'][tmap] - - predictors = results_collection.find({'result_time': result_time}, {'predictor': 1}) - predictors = set([predictor['predictor'] for predictor in predictors]) - - - # gather in the unique windows that defined the training regime - - predictor_windows = [] - for predictor in predictors: - print predictor - window_dates = {} - for window in results_collection.find({'result_time': result_time, 'predictor': predictor}, {'training_window_start': 1, 'training_window_end':1}): - if window['training_window_start'] not in window_dates: - window_dates[window['training_window_start']] = [] - window_dates[window['training_window_start']].append(window['training_window_end']) - - windows = [] - for start, ends in window_dates.items(): - ends = set(ends) - windows += [[predictor, start, end] for end in ends] - - - predictor_windows += windows - - - predictor_windows.sort(key=lambda w: w[2]) - predictor_windows.sort(key=lambda w: w[0]) - print predictor_windows - - - colour_map = plt.get_cmap('Paired') - colours = [colour_map(i * 30) for i in range(len(predictor_windows))] - - count = 0 - - with PdfPages('{0}_nav_stats.pdf'.format(tmap)) as pdf: - - plt.figure(figsize=(40,5)) - duration_ax = plt.subplot(2, 1, 1) - success_ax = plt.subplot(2, 1, 2) - - for window in predictor_windows: - print window - visualise_window(result_time, window[0], results_collection, window[1], window[2], colours[count], window[0] + str(count), duration_ax, success_ax) - count += 1 - - for edge, x in edge_id_to_x.items(): - edge_id_to_x[edge] = x + 1 - - plt.legend() - # plt.show() - pdf.savefig() # saves the current figure into a pdf page - plt.close() - - -def do_g4s_predictions(version=1): - # get all nav stats - tz = pytz.timezone(pytz.country_timezones['gb'][0]) - - training_window_start = datetime(2015,5,6,8,0,tzinfo=tz) - training_window_end = datetime(2015,5,12,18,0,tzinfo=tz) - training_window_size = timedelta(days=7) - training_windows = 4 - - testing_window_start = datetime(2015,6,4,8,0,tzinfo=tz) - testing_window_end = datetime(2015,6,10,18,0,tzinfo=tz) - - tmap = 'g4s_tmap' - collect_predictions('mean', version, tmap, training_window_start, training_window_end, training_window_size, training_windows, testing_window_start, testing_window_end) - - -def do_aaf_predictions(): - - tz = pytz.timezone(pytz.country_timezones['at'][0]) - training_window_start = datetime(2015,5,18,8,0,tzinfo=tz) - training_window_end = datetime(2015,5,24,21,0,tzinfo=tz) - training_window_size = timedelta(days=7) - training_windows = 3 - - testing_window_start = training_window_start + training_windows * training_window_size - testing_window_end = training_window_end + training_windows * training_window_size - - tmap = 'aaf_deployment' - - collect_predictions('fremen', 1, tmap, training_window_start, training_window_end, training_window_size, training_windows, testing_window_start, testing_window_end) - - - -def do_g4s_data_export(): - # g4s data export - tz = pytz.timezone(pytz.country_timezones['gb'][0]) - data_window_start = datetime(2015,5,6,8,0,tzinfo=tz) - data_window_end = datetime(2015,5,12,18,0,tzinfo=tz) - data_window_size = timedelta(days=7) - data_windows = 5 - tmap = 'g4s_tmap' - export_data(tmap, data_window_start, data_window_end, data_window_size, data_windows) - - -def do_aaf_data_export(): - tz = pytz.timezone(pytz.country_timezones['at'][0]) - data_window_start = datetime(2015,5,18,8,0,tzinfo=tz) - data_window_end = datetime(2015,5,24,21,0,tzinfo=tz) - data_window_size = timedelta(days=7) - data_windows = 5 - tmap = 'aaf_deployment' - - export_data(tmap, data_window_start, data_window_end, data_window_size, data_windows) - - - -if __name__ == '__main__': - rospy.init_node("topological_prediction_anlysis") - - - # get all nav stats - version=2 - do_g4s_predictions(version) - visualise_results('g4s_tmap', version) - - - - diff --git a/topological_utils/topological_utils/scripts/goal_converter.py b/topological_utils/topological_utils/scripts/goal_converter.py deleted file mode 100755 index 6e8ec908..00000000 --- a/topological_utils/topological_utils/scripts/goal_converter.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python -""" -Created on Thu Apr 29 10:04:30 2021 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -######################################################################################################### -import sys, os, traceback, rospy - -from topological_navigation.manager2 import map_manager_2 - - -def convert_goal(root_dir, action, action_type): - - tmap_files = [] - for root, dirs, files in os.walk(root_dir): - for _file in files: - if _file.endswith(".tmap2"): - tmap_files.append(os.path.join(root, _file)) - - mm = map_manager_2(advertise_srvs=False) - for f in tmap_files: - rospy.loginfo("CHANGING THE GOAL IN " + f) - - try: - mm.init_map(filename=f) - mm.update_action(action, action_type, "", update=False, write_map=False) - mm.write_topological_map(f) - except Exception: - rospy.logerr("Unable to convert goal in {}\n{}".format(f, traceback.format_exc())) - sys.exit(1) - - rospy.sleep(1.0) -######################################################################################################### - - -######################################################################################################### -if __name__ == '__main__' : - - if "-h" in sys.argv or "--help" in sys.argv or len(sys.argv) < 4: - print("usage is goal_converter.py root_directory action action_type") - sys.exit(1) - else: - root_dir = sys.argv[1] - action = sys.argv[2] - action_type = sys.argv[3] - - print("Converting the goal type of {} action to {} for topological maps found in {}".format(action, action_type, root_dir)) - - rospy.init_node("topological_goal_converter") - convert_goal(root_dir, action, action_type) -######################################################################################################### diff --git a/topological_utils/topological_utils/scripts/insert_empty_map.py b/topological_utils/topological_utils/scripts/insert_empty_map.py deleted file mode 100755 index 517e2f20..00000000 --- a/topological_utils/topological_utils/scripts/insert_empty_map.py +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env python -import sys -import rospy -import yaml -from topological_navigation.load_maps_from_yaml import YamlMapLoader -from mongodb_store.message_store import MessageStoreProxy -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation_msgs.srv import AddNode -from geometry_msgs.msg import Pose - -if __name__ == '__main__': - rospy.init_node('insert_empty_map') - - if len(sys.argv) != 2: - rospy.logwarn("usage: insert_empty_map.py pointset_name") - sys.exit(1) - - pointset = sys.argv[1] - map_loader = YamlMapLoader() - if pointset in map_loader.get_maps(): - rospy.logwarn("Map with name {0} already exists. Try another one.".format(sys.argv[1])) - sys.exit(1) - - msg_store = MessageStoreProxy(collection='topological_maps') - - empty_node = TopologicalNode() - empty_node.name = "temp_node" - empty_node.map = "unused" - empty_node.pointset = pointset - - meta = {} - meta['pointset'] = pointset - meta['map'] = "unused" - meta['node'] = "temp_node" - - msg_store.insert(empty_node, meta) diff --git a/topological_utils/topological_utils/scripts/insert_map.py b/topological_utils/topological_utils/scripts/insert_map.py deleted file mode 100755 index fbeedfa1..00000000 --- a/topological_utils/topological_utils/scripts/insert_map.py +++ /dev/null @@ -1,154 +0,0 @@ -#!/usr/bin/env python - -import json -import sys -import rospy -from geometry_msgs.msg import Pose -from topological_navigation_msgs.msg import TopologicalNode, Edge, Vertex - -import pymongo -#import mongodb_store.util -from mongodb_store.message_store import MessageStoreProxy - - -class topological_node(object): - def __init__(self,node_name, dataset_name, map_name): - self.name=node_name - self.pointset=dataset_name - self.map=map_name - - def _insert_waypoint(self, swaypoint): - self.waypoint=swaypoint.split(',') - - def _insert_edges(self, edges): - self.edges=edges - - def _insert_vertices(self, vertices): - self.vertices=vertices - -def get_edge_id(source, target, eids): - test=0 - eid = '%s_%s' %(source, target) - while eid in eids: - eid = '%s_%s_%3d' %(source, target, test) - test += 1 - return eid - - -def loadMap(inputfile, dataset_name, map_name) : - - print "openning %s" %inputfile - fin = open(inputfile, 'r') - print "Done" - - line = fin.readline() - anode=topological_node("Empty", dataset_name, map_name) - lnodes=[anode] - while line: - #node line - if line.startswith('node:'): - #Saving Name of the Node - line = fin.readline() - name = line.strip('\t') - name = name.strip('\n') - anode=topological_node(name, dataset_name, map_name) - - #Saving WayPoint - line = fin.readline() - if line.startswith('\t') : - if line.startswith('\twaypoint:') : - #Reading Line with WayPoint - line = fin.readline() - ways = line.strip('\t') - ways = ways.strip('\n') - anode._insert_waypoint(ways) - - #Saving edges - line = fin.readline() - if line.startswith('\t') : - if line.startswith('\tedges:') : - aedge = {'node':"empty", 'action':"move_base"} - edges=[aedge] - line = fin.readline() - while line and not(line.startswith('\tvertices:')) : - info= line.strip('\t') - inf = info.split(',',2) - aedge = {'node':inf[0].strip(), 'action':inf[1].strip()} - edges.append(aedge) - line = fin.readline() - edges.pop(0) - anode._insert_edges(edges) - - #Saving vertices - #line = fin.readline() - if line.startswith('\t') : - if line.startswith('\tvertices:') : - vertices=[] - line = fin.readline() - while line and not(line.startswith('node:')) : - info= line.strip('\t') - inf = info.split(',',2) - vertex = (float(inf[0].strip()), float(inf[1].strip())) - vertices.append(vertex) - line = fin.readline() - anode._insert_vertices(vertices) - lnodes.append(anode) - fin.close() - lnodes.pop(0) - - return lnodes - - -if __name__ == '__main__': - if len(sys.argv) < 4 : - print "usage: insert_map input_file.txt dataset_name map_name" - sys.exit(2) - - filename=str(sys.argv[1]) - dataset_name=str(sys.argv[2]) - map_name=str(sys.argv[3]) - - msg_store = MessageStoreProxy(collection='topological_maps') - - eids=[] #list of known edge id's - lnodes=loadMap(filename, dataset_name, map_name) - - meta = {} - meta["map"] = map_name - meta["pointset"] = dataset_name - - for i in lnodes: - #val=i.__dict__#json.loads(vala) print val #+ '\n' - n = TopologicalNode() - n.name = i.name - meta["node"] = i.name - n.map = i.map - n.pointset = i.pointset - p = Pose() - p.position.x=float(i.waypoint[0]) - p.position.y=float(i.waypoint[1]) - p.position.z=float(i.waypoint[2]) - p.orientation.x=float(i.waypoint[3]) - p.orientation.y=float(i.waypoint[4]) - p.orientation.z=float(i.waypoint[5]) - p.orientation.w=float(i.waypoint[6]) - n.pose = p - for j in i.vertices : - v = Vertex() - v.x = float(j[0]) - v.y = float(j[1]) - n.verts.append(v) - for k in i.edges : - e = Edge() - e.node = k['node'] - e.action = k['action'] - eid = get_edge_id(i.name, e.node, eids) - eids.append(eid) - e.edge_id = eid - e.top_vel =0.55 - e.map_2d = map_name - n.edges.append(e) - - print n - msg_store.insert(n,meta) - #mongodb_store.util.store_message(points_db,p,val) diff --git a/topological_utils/topological_utils/scripts/joy_add_node.py b/topological_utils/topological_utils/scripts/joy_add_node.py deleted file mode 100755 index 49303aa4..00000000 --- a/topological_utils/topological_utils/scripts/joy_add_node.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy - -import std_msgs.msg -import sensor_msgs.msg -from geometry_msgs.msg import Pose - -#from strands_navigation_msgs.msg import TopologicalNode -#from ros_datacentre.message_store import MessageStoreProxy -#from topological_navigation.topological_node import * -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation.topological_map import * - - -#import topological_navigation.msg - - - -class topologicalNodeAdd(object): - - def __init__(self, base_name, dist) : - self.map_received = False - rospy.Subscriber('joy', sensor_msgs.msg.Joy, self.JoyCallback) - rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) - - self.button = rospy.get_param('~button', 1) - - rospy.loginfo("Waiting for Topological map ...") - while not self.map_received : - pass - - rospy.loginfo("All Done ...") - rospy.spin() - - - def JoyCallback(self, msg) : - if msg.buttons[self.button]: - #print "JOY" - self.add_node() - - def MapCallback(self, msg) : - self.topo_map = topological_map(msg.name, msg=msg) - self.map_received =True - - - def add_node(self) : - - try: - current = rospy.wait_for_message('current_node', std_msgs.msg.String, timeout=10.0) - except rospy.ROSException : - rospy.logwarn("Failed to get current node") - return - - print current.data - if current.data == 'none': - try: - pos = rospy.wait_for_message('robot_pose', Pose, timeout=10.0) - except rospy.ROSException : - rospy.logwarn("Failed to get robot pose") - return - - lnames=[] - for i in self.topo_map.nodes : - if i.name.startswith('WayPoint') : - nam = i.name.strip('WayPoint') - lnames.append(int(nam)) - - lnames.sort() - print "used names:" - for j in lnames : - print j - - print "Chosen name!!!!!!" - if lnames: - nodname = 'WayPoint%d'%(int(lnames[-1])+1) - else : - nodname = 'WayPoint1' - print nodname - #self.topo_map = topological_map(pointset) - self.topo_map.add_node(nodname,8.0, pos, 'move_base') - - map_update = rospy.Publisher('/update_map', std_msgs.msg.Time) - map_update.publish(rospy.Time.now()) - - else: - rospy.loginfo("I will NOT add a node within the influence area of another! Solve this and try again!") - - - -if __name__ == '__main__': -# pointset=str(sys.argv[1]) -# name=str(sys.argv[2]) -# dist=float(sys.argv[3]) - name = 'WayPoint' - dist = 10.0 - rospy.init_node('node_add') - server = topologicalNodeAdd(name,dist) \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/joy_add_waypoint.py b/topological_utils/topological_utils/scripts/joy_add_waypoint.py deleted file mode 100755 index db063be6..00000000 --- a/topological_utils/topological_utils/scripts/joy_add_waypoint.py +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy - -import std_msgs.msg -import sensor_msgs.msg -from geometry_msgs.msg import Pose - -#from strands_navigation_msgs.msg import TopologicalNode -#from ros_datacentre.message_store import MessageStoreProxy -#from topological_navigation.topological_node import * -from topological_navigation_msgs.msg import TopologicalMap -from topological_navigation.topological_map import * - - -#import topological_navigation.msg - - - -class addWaypoint(object): - - def __init__(self, filename) : - rospy.Subscriber('joy', sensor_msgs.msg.Joy, self.JoyCallback) - self.filename = filename - #fh = open(filename, "a") - #fh.close - rospy.loginfo("All Done ...") - rospy.spin() - - - def JoyCallback(self, msg) : - if msg.buttons[1]: - print "JOY" - self.add_waypoint() - - - def add_waypoint(self) : - - try: - p = rospy.wait_for_message('robot_pose', Pose, timeout=10.0) - except rospy.ROSException : - rospy.logwarn("Failed to get robot pose") - return - - fh = open(filename, "a") - - s_output = "%s,%s,%s,%s,%s,%s,%s\n" %(str(p.position.x),str(p.position.y),str(p.position.z), - str(p.orientation.x),str(p.orientation.y),str(p.orientation.z),str(p.orientation.w)) - fh.write(s_output) - fh.close - - - - -if __name__ == '__main__': - filename=str(sys.argv[1]) -# name=str(sys.argv[2]) -# dist=float(sys.argv[3]) - name = 'WayPoint' - dist = 10.0 - rospy.init_node('node_add') - server = addWaypoint(filename) \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/list_maps b/topological_utils/topological_utils/scripts/list_maps deleted file mode 100755 index 16571a43..00000000 --- a/topological_utils/topological_utils/scripts/list_maps +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/python - -""" -Outputs a list of available topological maps -""" - -from topological_utils.queries import get_maps - -if __name__ == "__main__": - for map_name,details in get_maps().items(): - print "-" * 50 - print "Name: ", map_name - print "Number of nodes: ", details["number_nodes"] - print "Edge actions: ", str(list(details["edge_actions"])) - print "Last modified: ", details["last_modified"] diff --git a/topological_utils/topological_utils/scripts/load_json_map.py b/topological_utils/topological_utils/scripts/load_json_map.py deleted file mode 100755 index a94b4e1a..00000000 --- a/topological_utils/topological_utils/scripts/load_json_map.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python - -import json -import yaml -import sys - -from topological_navigation_msgs.msg import TopologicalNode -import mongodb_store.util as dc_util -from mongodb_store.message_store import MessageStoreProxy - - -if __name__ == '__main__': - if len(sys.argv) < 2 : - print "usage: insert_map input_file.txt" - sys.exit(2) - - filename=str(sys.argv[1]) - #dataset_name=str(sys.argv[2]) - #map_name=str(sys.argv[3]) - - msg_store = MessageStoreProxy(collection='topological_maps') - - json_data=open(filename, 'rb').read() - - data = json.loads(json_data) - - for i in data: - meta = i['meta'] - msgv = dc_util.dictionary_to_message(i['node'], TopologicalNode) - msg_store.insert(msgv,meta) - #mongodb_store.util.store_message(points_db,p,val) - - - - - - - - - - - - - - - - - - - - - - - diff --git a/topological_utils/topological_utils/scripts/load_yaml_map.py b/topological_utils/topological_utils/scripts/load_yaml_map.py deleted file mode 100755 index 2e7a903e..00000000 --- a/topological_utils/topological_utils/scripts/load_yaml_map.py +++ /dev/null @@ -1,32 +0,0 @@ -#!/usr/bin/env python - -import rospy -import yaml -import argparse - -from topological_navigation.load_maps_from_yaml import YamlMapLoader -from std_srvs.srv import Empty - - -def load_yaml(filename): - rospy.loginfo("loading %s"%filename) - with open(filename, 'r') as f: - return yaml.load(f) - -if __name__ == '__main__': - parser = argparse.ArgumentParser() - parser.add_argument("yaml_map", help="The yaml file or directory containing yaml files to insert into the datacentre", type=str) - parser.add_argument("--pointset", help="Override the pointset name defined in the yaml before inserting it into datacentre, i.e. rename map. If a directory is given the pointset will be extended by an index to keep the maps separate.", type=str) - parser.add_argument("-f", "--force", help="Override maps with the same name in the datacentre", action="store_true") - parser.add_argument("--keep-alive", help="Keeps the node alive and advertises the service '/load_yaml_map/loaded' as soon as all the maps are stored in the datacentre. Can be used for testing in a launch file to trigger components that rely on maps being inserted in the datacentre first.", action="store_true") - args, unknown = parser.parse_known_args() # Necessary due to roslaunch injecting rubbish arguments - - rospy.init_node('load_yaml_map') - y = YamlMapLoader() - - data = y.read_maps(args.yaml_map) - y.insert_maps(data=data, new_pointset=args.pointset, force=args.force) - - if args.keep_alive: - rospy.Service("~loaded", Empty, None) - rospy.spin() diff --git a/topological_utils/topological_utils/scripts/map_collection_change.py b/topological_utils/topological_utils/scripts/map_collection_change.py deleted file mode 100755 index d7543234..00000000 --- a/topological_utils/topological_utils/scripts/map_collection_change.py +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python - -import json -import sys -import rospy -from geometry_msgs.msg import Pose -from topological_navigation_msgs.msg import TopologicalNode, Vertex, Edge - -import pymongo -#import mongodb_store.util -from mongodb_store.message_store import MessageStoreProxy - - - - - -if __name__ == '__main__': - #if len(sys.argv) < 4 : - # print "usage: insert_map input_file.txt dataset_name map_name" - # sys.exit(2) - - #filename=str(sys.argv[1]) - #dataset_name=str(sys.argv[2]) - #map_name=str(sys.argv[3]) - - msg_store = MessageStoreProxy() - msg_store_maps = MessageStoreProxy(collection='topological_maps') - - - query_meta = {} - query_meta["stored_type"] = "topological_navigation_msgs/TopologicalNode" - - available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 - - #print available - - if available <= 0 : - #rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") - #rospy.logerr("Available pointsets: "+str(available)) - raise Exception("Can't find waypoints.") - - else : - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - for i in message_list: - #print i - meta = {} - meta["node"] = i[0].name - meta["map"] = i[0].map - meta["pointset"] = i[0].pointset - available = len(msg_store_maps.query(TopologicalNode._type, {}, meta)) - if available == 0 : - msg_store_maps.insert(i[0],meta) - else : - rospy.logerr("this point is already in datacentre:") - print meta \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/map_converter.py b/topological_utils/topological_utils/scripts/map_converter.py deleted file mode 100755 index 9a15f7ab..00000000 --- a/topological_utils/topological_utils/scripts/map_converter.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python -""" -Created on Thu Apr 29 10:04:30 2021 - -@author: Adam Binch (abinch@sagarobotics.com) -""" -######################################################################################################### -import sys, os, traceback -import rospy - -from topological_navigation.manager import map_manager - - -def convert_tmaps(root_dir, ext_old, ext_new): - - tmap_files = [] - for root, dirs, files in os.walk(root_dir): - for _file in files: - if _file.endswith(ext_old): - tmap_files.append(os.path.join(root, _file)) - - mm = map_manager() - for f in tmap_files: - rospy.loginfo("CONVERTING " + f) - - try: - mm.init_map(f, load_from_file=True) - mm.manager2.write_topological_map(os.path.splitext(f)[0] + ext_new) - except Exception: - rospy.logerr("Unable to convert {}\n{}".format(f, traceback.format_exc())) - - rospy.sleep(1.0) -######################################################################################################### - - -######################################################################################################### -if __name__ == '__main__' : - - if "-h" in sys.argv or "--help" in sys.argv or len(sys.argv) < 4: - print("usage is map_converter.py root_directory old_topomap_ext new_topomap_ext") - sys.exit(1) - else: - root_dir = sys.argv[1] - ext_old = sys.argv[2] - ext_new = sys.argv[3] - - print("Converting topological maps with ext {} found in {} to new format with ext {}".format(ext_old, root_dir, ext_new)) - - rospy.init_node("topological_map_converter") - convert_tmaps(root_dir, ext_old, ext_new) -######################################################################################################### \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/map_export.py b/topological_utils/topological_utils/scripts/map_export.py deleted file mode 100755 index bf515253..00000000 --- a/topological_utils/topological_utils/scripts/map_export.py +++ /dev/null @@ -1,126 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import actionlib -import pymongo -import json -import sys -import math - - - -from topological_navigation.topological_node import * -from actionlib_msgs.msg import * -from move_base_msgs.msg import * -from geometry_msgs.msg import Pose -from std_msgs.msg import String - -from topological_navigation_msgs.msg import TopologicalNode - -from mongodb_store.message_store import MessageStoreProxy -import topological_navigation.msg - - - -class TopologicalNavLoc(object): - - def __init__(self, dataset_name, filename) : - #print "loading file from map %s" %filename - self.lnodes = self.loadMap(dataset_name) - self.export_map(filename) - - - def export_map(self, filename): - #Clean the file in case it existed - fh = open(filename, "w") - fh.close - - #Write File - for i in self.lnodes : - fh = open(filename, "a") - print "node: \n\t%s" %i.name - s_output = "node: \n\t%s\n" %i.name - fh.write(s_output) - print "\twaypoint:\n\t%s" %i.waypoint - s_output = "\twaypoint:\n\t\t%f,%f,%f,%f,%f,%f,%f\n" %(float(i.waypoint[0]),float(i.waypoint[1]),float(i.waypoint[2]),float(i.waypoint[3]),float(i.waypoint[4]),float(i.waypoint[5]),float(i.waypoint[6])) - fh.write(s_output) - print "\tedges:" - s_output = "\tedges:\n" - fh.write(s_output) - for k in i.edges : - print "\t\t %s, %s" %(k['node'],k['action']) - s_output = "\t\t %s, %s\n" %(k['node'],k['action']) - fh.write(s_output) - print "\tvertices:" - s_output = "\tvertices:\n" - fh.write(s_output) - for k in i.vertices : - print "\t\t%f,%f" %(k[0],k[1]) - s_output = "\t\t%f,%f\n" %(k[0],k[1]) - fh.write(s_output) - fh.close - - - def loadMap(self, point_set): - - point_set=str(sys.argv[1]) - #map_name=str(sys.argv[3]) - - msg_store = MessageStoreProxy(collection='topological_maps') - - query_meta = {} - query_meta["pointset"] = point_set - - available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 - - print available - - if available <= 0 : - rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") - rospy.logerr("Available pointsets: "+str(available)) - raise Exception("Can't find waypoints.") - - else : - query_meta = {} - query_meta["pointset"] = point_set - - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - - points = [] - for i in message_list: - #print i[0].name - b = topological_node(i[0].name) - edges = [] - for j in i[0].edges : - data = {} - data["node"]=j.node - data["action"]=j.action - edges.append(data) - b.edges = edges - - verts = [] - for j in i[0].verts : - data = [j.x,j.y] - verts.append(data) - b._insert_vertices(verts) - - c=i[0].pose - waypoint=[str(c.position.x), str(c.position.y), str(c.position.z), str(c.orientation.x), str(c.orientation.y), str(c.orientation.z), str(c.orientation.w)] - b.waypoint = waypoint - b._get_coords() - - points.append(b) - - return points - - -if __name__ == '__main__': - if len(sys.argv) < 3 : - print "usage: map_export dataset_name output_file.tplg" - sys.exit(2) - - dataset_name=str(sys.argv[1]) - filename=str(sys.argv[2]) - rospy.init_node('topological_map_exporter') - server = TopologicalNavLoc(dataset_name, filename) diff --git a/topological_utils/topological_utils/scripts/map_to_json.py b/topological_utils/topological_utils/scripts/map_to_json.py deleted file mode 100755 index 40c68399..00000000 --- a/topological_utils/topological_utils/scripts/map_to_json.py +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import actionlib -import pymongo -import json -import bson -import sys -import math -import yaml -import pickle - -from topological_navigation.topological_node import * -from actionlib_msgs.msg import * -from move_base_msgs.msg import * -from geometry_msgs.msg import Pose -from std_msgs.msg import String - -from topological_navigation_msgs.msg import TopologicalNode - -from mongodb_store.message_store import MessageStoreProxy -import topological_navigation.msg - - - - -class MapExport(object): - - def __init__(self, dataset_name, filename) : - #print "loading file from map %s" %filename - self.lnodes = self.loadMap(dataset_name, filename) - - - def loadMap(self, point_set, filename): - - point_set=str(sys.argv[1]) - #map_name=str(sys.argv[3]) - - msg_store = MessageStoreProxy(collection='topological_maps') - - query_meta = {} - query_meta["pointset"] = point_set - - available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 - - print available - - if available <= 0 : - rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") - rospy.logerr("Available pointsets: "+str(available)) - raise Exception("Can't find waypoints.") - - else : - query_meta = {} - query_meta["pointset"] = point_set - - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - - #node=TopologicalNode() - - top_map=[] - for i in message_list: - nodeinf = {} - nodeinf["node"] = yaml.load(str(i[0])) - nodeinf["node"]["localise_by_topic"] = json.dumps(nodeinf["node"]["localise_by_topic"]) - nodeinf["meta"] = i[1] #str(bson.json_util.dumps(i[1], indent=1)) - - nodeinf["meta"].pop("last_updated_by", None) - nodeinf["meta"].pop('inserted_at', None) - nodeinf["meta"].pop('last_updated_at', None) - nodeinf["meta"].pop('stored_type', None) - nodeinf["meta"].pop('stored_class', None) - nodeinf["meta"].pop('inserted_by', None) - nodeinf["meta"].pop('_id', None) - top_map.append(nodeinf) - #val = bson.json_util.dumps(nodeinf["meta"], indent=1) - - fh = open(filename, "w") - #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) - s_output = str(bson.json_util.dumps(top_map, indent=1, sort_keys=True) ) - #print s_output - fh.write(s_output) - fh.close - - -if __name__ == '__main__': - if len(sys.argv) < 3 : - print "usage: map_to_json dataset_name output_file.json" - sys.exit(2) - - dataset_name=str(sys.argv[1]) - filename=str(sys.argv[2]) - rospy.init_node('topological_map_exporter') - server = MapExport(dataset_name, filename) diff --git a/topological_utils/topological_utils/scripts/map_to_yaml.py b/topological_utils/topological_utils/scripts/map_to_yaml.py deleted file mode 100755 index f6959bb0..00000000 --- a/topological_utils/topological_utils/scripts/map_to_yaml.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import json -import sys -import yaml -#import pickle - -from topological_navigation.topological_node import * -from actionlib_msgs.msg import * - -from geometry_msgs.msg import Pose -from std_msgs.msg import String - -from topological_navigation_msgs.msg import TopologicalNode - -from mongodb_store.message_store import MessageStoreProxy -import topological_navigation.msg - - - - -class MapExport(object): - - def __init__(self, dataset_name, filename) : - #print "loading file from map %s" %filename - self.lnodes = self.loadMap(dataset_name, filename) - - - def loadMap(self, point_set, filename): - - point_set=str(sys.argv[1]) - #map_name=str(sys.argv[3]) - - msg_store = MessageStoreProxy(collection='topological_maps') - - query_meta = {} - query_meta["pointset"] = point_set - - available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 - - print available - - if available <= 0 : - rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") - rospy.logerr("Available pointsets: "+str(available)) - raise Exception("Can't find waypoints.") - - else : - query_meta = {} - query_meta["pointset"] = point_set - - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - - #node=TopologicalNode() - - top_map=[] - for i in message_list: - nodeinf = {} - nodeinf["node"] = yaml.load(str(i[0])) - if nodeinf["node"]["localise_by_topic"]: - nodeinf["node"]["localise_by_topic"] = json.dumps(nodeinf["node"]["localise_by_topic"]) - nodeinf["meta"] = i[1] #str(bson.json_util.dumps(i[1], indent=1)) - nodeinf["meta"].pop("last_updated_by", None) - nodeinf["meta"].pop('inserted_at', None) - nodeinf["meta"].pop('last_updated_at', None) - nodeinf["meta"].pop('stored_type', None) - nodeinf["meta"].pop('stored_class', None) - nodeinf["meta"].pop('inserted_by', None) - nodeinf["meta"].pop('_id', None) - top_map.append(nodeinf) - #val = bson.json_util.dumps(nodeinf["meta"], indent=1) - - - - top_map.sort(key=lambda x: x['node']['name']) - yml = yaml.safe_dump(top_map, default_flow_style=False) - #print yml - #print s_output - - fh = open(filename, "w") - #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) - s_output = str(yml) - #print s_output - fh.write(s_output) - fh.close - -# fh = open(filename, "w") -# #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) -# s_output = str(bson.json_util.dumps(top_map, indent=1, sort_keys=True) ) -# print s_output -# fh.write(s_output) -# fh.close - - -if __name__ == '__main__': - if len(sys.argv) < 3 : - print "usage: map_to_yaml dataset_name output_file.yaml" - sys.exit(2) - - dataset_name=str(sys.argv[1]) - filename=str(sys.argv[2]) - rospy.init_node('topological_map_exporter') - server = MapExport(dataset_name, filename) diff --git a/topological_utils/topological_utils/scripts/migrate.py b/topological_utils/topological_utils/scripts/migrate.py deleted file mode 100755 index 39c8c4ee..00000000 --- a/topological_utils/topological_utils/scripts/migrate.py +++ /dev/null @@ -1,271 +0,0 @@ -#!/usr/bin/env python -import sys -import rospy -import pymongo -import json -import sys -import math - - -from topological_navigation_msgs.msg import TopologicalNode, Edge, Vertex -from geometry_msgs.msg import Pose -import mongodb_store.util as dc_util -from mongodb_store.message_store import MessageStoreProxy -import topological_navigation_msgs.msg - - -def get_edge_id(source, target, eids): - test=0 - eid = '%s_%s' %(source, target) - while eid in eids: - eid = '%s_%s_%3d' %(source, target, test) - test += 1 - return eid - - -def update_node(node, pointset): - to_pop=['edges','verts','pose'] - n = TopologicalNode() - - n.yaw_goal_tolerance = 0.1 - n.xy_goal_tolerance = 0.3 - - lsl = n.__slots__ - for j in lsl: - if node.has_key(j) and not j in to_pop: - setattr(n, j, node[j]) - - ppos = node['pose'] - - p = dc_util.dictionary_to_message(ppos, Pose) - #print p - #n.pose.append(p) - n.pose = p - n.pointset = pointset - return n - - -def update_edge(edge, nna, nma, eids): - e = Edge() - e.top_vel =0.55 - e.map_2d = nma - lsl = e.__slots__ - for j in lsl: - if edge.has_key(j): - setattr(e, j, edge[j]) - if e.edge_id == '': - eid = get_edge_id(nna, e.node, eids) - e.edge_id = eid - eids.append(eid) - return e, eids - -def update_vert(vert): - v = Vertex() - - lsl = v.__slots__ - for j in lsl: - if vert.has_key(j): - setattr(v, j, vert[j]) - - return v - -def update_meta(meta, pointset): - cm=meta - to_pop=['inserted_at','stored_type','stored_class','inserted_by'] - - lsl = meta.keys() - - for j in lsl: - if j in to_pop: - cm.pop(j) - - cm['pointset']=pointset - return cm - -def check_for_update(b, d, client): - to_update=[] - db=client.message_store - collection=db["topological_maps"] - available = collection.find().distinct("_meta.pointset") - - #for every point_set in the db - for pointset in available: - #get one message - search = {"_meta.pointset": pointset, "name" : 'ChargingPoint'} - aa =collection.find_one(search) - a = aa.keys() - #pop out meta form msg store from the dict keys - for i in to_pop: - if i in a: - a.pop(a.index(i)) - #count the difference - c = len(list(set(b).difference(set(a)))) - if c > 0: - #add to update list - to_update.append(pointset) - else: -# print "No differences at node level testing edges" -# print 'Edge comparison for pointset %s' %pointset - - e = aa["edges"][0].keys() - edef =len(list(set(d).difference(set(e)))) -# print edef - if edef > 0: - to_update.append(pointset) - return to_update#, available - -def update_maps(to_update, client): - db=client.message_store - collection=db["topological_maps"] -# available = collection.find().distinct("_meta.pointset") - - msg_store = MessageStoreProxy(collection='topological_maps') - - del_ids = [] - for pointset in to_update: - #pointsetb='%s_b'%pointset - #print pointsetb - search = {"_meta.pointset": pointset} - av =collection.find(search) - #lnodes=[] - eids=[] #list of known edge id's - for a in av: - #print a - bc = update_node(a, pointset) - - nna = a['name'] - nma = a['map'] - vt = a['verts'] - if a['edges']: - es = a['edges'] - for i in es: - ed, eids = update_edge(i, nna, nma, eids) - bc.edges.append(ed) - - for i in vt: - v = update_vert(i) - bc.verts.append(v) - - meta = update_meta(a['_meta'], pointset) - - #print bc - #print meta - del_ids.append(a['_id']) - #lnodes.append(bc) - msg_store.insert(bc,meta) - #print lnodes - - print "Deleting updated nodes" - for i in del_ids: - msg_store.delete(str(i)) - print "done" - - -def check_sanity(client): - msg_store = MessageStoreProxy(collection='topological_maps') - db=client.message_store - collection=db["topological_maps"] - available = collection.find().distinct("_meta.pointset") - print available - - for point_set in available: - #get one message - #search = {"_meta.pointset": pointset} - query_meta = {} - query_meta["pointset"] = point_set - - message_list = msg_store.query(TopologicalNode._type, {}, query_meta) - - #points = strands_navigation_msgs.msg.TopologicalMap() - #points.name = point_set - #points.map = point_set - #points.pointset = point_set - #string last_updated - eids = [] - for i in message_list: - update = False - print i[0].pointset, i[0].name, i[1]['_id'] - if i[0].edges: - for j in i[0].edges : - if j.edge_id == '': - update = True - print 'needs edge id' - j.edge_id = get_edge_id(i[0].name, j.node, eids) - print 'new edge_id %s' %j.edge_id - eids.append(j.edge_id) - if j.top_vel <= 0.1 : - update = True - print 'needs top vel' - j.top_vel = 0.55 - if j.map_2d == '': - update = True - print 'needs map_2d' - j.map_2d = i[0].map - if update: - msg_store.update_id(i[1]['_id'], i[0], i[1], upsert = False) - -def add_localise_by_topic(tmap, node, json_str): - #print req - #data = json.loads(req.content) - #print data - - msg_store = MessageStoreProxy(collection='topological_maps') - query = {"name" : node, "pointset": tmap} - query_meta = {} - #query_meta["pointset"] = tmap - #query_meta["map"] = self.nodes.map - - #print query, query_meta - available = msg_store.query(topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) - #print len(available) - if len(available) != 1: - #succeded = False - print 'there are no nodes or more than 1 with that name' - else: - #succeded = True - for i in available: - if not i[0].localise_by_topic: - msgid= i[1]['_id'] - i[0].localise_by_topic=json_str - #print i[0] - print "Updating %s--%s" %(i[0].pointset, i[0].name) - msg_store.update_id(msgid, i[0], i[1], upsert = False) - - -if __name__ == '__main__': - - rospy.init_node('topological_navigation_migration') - host = rospy.get_param("mongodb_host") - port = rospy.get_param("mongodb_port") - client = pymongo.MongoClient(host, port) - - to_pop=['_id','_meta'] - - b = TopologicalNode().__slots__ - d = Edge().__slots__ - - print '========= Current Topological NODE definition Slots ===========' - print b - print '========= Current Topological EDGE definition Slots ===========' - print d - - - to_update = check_for_update(b, d, client) - - print '========= The following maps need to be updated ===========' - print to_update - - update_maps(to_update, client) - - check_sanity(client) - #print available - - db=client.message_store - collection=db["topological_maps"] - available = collection.find().distinct("_meta.pointset") - print available - - for i in available: - add_localise_by_topic(i, 'ChargingPoint', "{\"topic\":\"battery_state\",\"field\":\"charging\", \"val\": true}") - - \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/node_metadata.py b/topological_utils/topological_utils/scripts/node_metadata.py deleted file mode 100755 index 1a3f893b..00000000 --- a/topological_utils/topological_utils/scripts/node_metadata.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python - -import json -import sys -import rospy - - -from topological_navigation.topological_node import * -from topological_navigation_msgs.msg import TopologicalNode -from mongodb_store.message_store import MessageStoreProxy - -from topological_utils.queries import get_nodes -from topological_utils.srv import * - -def handle_query_nodes(req): - print "Queried for %s %s"%(req.pointset, req.meta_category) - - resp = NodeMetadataResponse() - resp.name=[] - resp.description = [] - resp.goto_node=[] - resp.node_type = [] - - nodes = get_nodes(req.pointset, req.meta_category) - - for node in nodes: - for gui_node in node[1]["contains"]: - if (gui_node["category"] == req.meta_category) : - resp.name.append(gui_node["name"]) - resp.node_type.append(gui_node["category"]) - resp.goto_node.append(node[1]["node"]) - if gui_node.has_key("description"): - resp.description.append(gui_node["description"]) - else: - resp.description.append("") - - if gui_node.has_key("at_node"): - resp.at_node.append(gui_node["at_node"]) - else: - resp.at_node.append(False) - - print "Returning ",len(resp.name)," nodes for the meta_query ",req.meta_category - return resp; - -def query_nodes_server(): - rospy.init_node('query_nodes_server') - s = rospy.Service('query_node_metadata', NodeMetadata, handle_query_nodes) - print "Ready to query the database for topological nodes." - rospy.spin() - -if __name__ == '__main__': - query_nodes_server() - - diff --git a/topological_utils/topological_utils/scripts/node_rm.py b/topological_utils/topological_utils/scripts/node_rm.py deleted file mode 100755 index fa12bde1..00000000 --- a/topological_utils/topological_utils/scripts/node_rm.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import pymongo - -import std_msgs.msg -#from strands_navigation_msgs.msg import TopologicalNode -#from ros_datacentre.message_store import MessageStoreProxy -#from topological_navigation.topological_node import * -from topological_navigation.topological_map import * - -#import topological_navigation.msg - - - -class topologicalNodeRM(object): - - def __init__(self, pointset, waypoint) : - - self.topo_map = topological_map(pointset) - self.topo_map.remove_node(waypoint) - - - map_update = rospy.Publisher('/update_map', std_msgs.msg.Time) - map_update.publish(rospy.Time.now()) - - rospy.loginfo("All Done ...") - - -if __name__ == '__main__': - pointset=str(sys.argv[1]) - waypoint=str(sys.argv[2]) - rospy.init_node('node_rm') - server = topologicalNodeRM(pointset,waypoint) \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/plot_topo_map.py b/topological_utils/topological_utils/scripts/plot_topo_map.py deleted file mode 100755 index 9b410f94..00000000 --- a/topological_utils/topological_utils/scripts/plot_topo_map.py +++ /dev/null @@ -1,120 +0,0 @@ -#!/usr/bin/env python -# ---------------------------------- -# @author: gpdas -# @email: pdasgautham@gmail.com -# @date: -# ---------------------------------- - - -import os -import sys -import rospy -import matplotlib.pyplot - -import topological_navigation_msgs.msg - - -class TopoMapPlotter(object): - """ - """ - def __init__(self, fig_file, fig_width, fig_height): - """ - """ - self.topo_map = None - self.rec_map = False - self.nodes = {} - self.node_names = [] - rospy.loginfo("Waiting for topo_map") - while not self.rec_map: - self.topo_map_sub = rospy.Subscriber("/topological_map", topological_navigation_msgs.msg.TopologicalMap, self.topo_map_cb) - rospy.sleep(0.5) - rospy.loginfo("Received topo_map") - - self.fig_file = fig_file - self.width = fig_width - self.height = fig_height - - def topo_map_cb(self, msg): - """ - """ - self.topo_map = msg - for node in self.topo_map.nodes: - self.nodes[node.name] = node - self.node_names.append(node.name) - self.rec_map = True - - def plot_map(self, strip_str=""): - fig = matplotlib.pyplot.figure(figsize=(self.width, self.height)) - min_x = None - max_x = None - min_y = None - max_y = None - ax = fig.add_subplot(111) - # node markers - for node_name in self.node_names: - x = self.nodes[node_name].pose.position.x - y = self.nodes[node_name].pose.position.y - ax.plot(x, y, color="black", marker="o", markersize=6, markeredgecolor="r") - - if min_x is None: - min_x = x - max_x = x - min_y = y - max_y = y - else: - if x < min_x: - min_x = x - if x > max_x: - max_x = x - if y < min_y: - min_y = y - if y > max_y: - max_y = y - - x_delta = (max_x - min_x) * 0.1 - y_delta = (max_y - min_y) * 0.1 - # node names - for node_name in self.node_names: - x = self.nodes[node_name].pose.position.x - y = self.nodes[node_name].pose.position.y - _node_name = node_name - if strip_str in node_name: - _node_name = node_name.strip(strip_str) - ax.text(x-x_delta/4.0, y + y_delta/4.0, _node_name, fontsize=10) - - for node_name in self.node_names: - from_x = self.nodes[node_name].pose.position.x - from_y = self.nodes[node_name].pose.position.y - for edge in self.nodes[node_name].edges: - to_node = edge.node - to_x = self.nodes[to_node].pose.position.x - to_y = self.nodes[to_node].pose.position.y -# ax.plot([from_x, to_x], [from_y, to_y], color="black", linewidth=2) - ax.arrow(from_x, from_y, to_x-from_x, to_y-from_y, color="black", - head_width=0.2 * min(x_delta, y_delta), - head_length=0.2 * min(x_delta, y_delta), length_includes_head=True, - visible=True) - - ax.set_xlabel("X") - ax.set_ylabel("Y") - ax.set_xlim(min_x - x_delta, max_x + x_delta) - ax.set_ylim(min_y - y_delta, max_y + y_delta) - matplotlib.pyplot.show() - fig.savefig(self.fig_file) - -if __name__ == "__main__": - rospy.init_node("plot_topo_map") - - if len(sys.argv) < 4: - rospy.loginfo("Usage is plot_topo_map path_to_plot_file fig_width_inches fig_height_inches") - else: - fig_file = os.path.abspath(sys.argv[1]) - - try: - map_plotter = TopoMapPlotter(fig_file, float(sys.argv[2]), float(sys.argv[3])) - except: - rospy.signal_shutdown("Couldn't create TopoMapPlotter object") - else: - map_plotter.plot_map(strip_str="WayPoint") - -# rospy.spin() diff --git a/topological_utils/topological_utils/scripts/plot_topo_map2.py b/topological_utils/topological_utils/scripts/plot_topo_map2.py deleted file mode 100755 index 2da6fdeb..00000000 --- a/topological_utils/topological_utils/scripts/plot_topo_map2.py +++ /dev/null @@ -1,121 +0,0 @@ -#!/usr/bin/env python -# ---------------------------------- -# @author: gpdas -# @email: pdasgautham@gmail.com -# @date: -# ---------------------------------- - - -import os -import sys -import rospy -import matplotlib.pyplot -import yaml - -import std_msgs.msg - - -class TopoMapPlotter(object): - """ - """ - def __init__(self, fig_file, fig_width, fig_height): - """ - """ - self.topo_map = None - self.rec_map = False - self.nodes = {} - self.node_names = [] - - self.topo_map_sub = rospy.Subscriber("topological_map_2", std_msgs.msg.String, self.topo_map_cb) - rospy.loginfo("Waiting for topo_map") - while not self.rec_map: - rospy.sleep(0.5) - - self.fig_file = fig_file - self.width = fig_width - self.height = fig_height - - def topo_map_cb(self, msg): - """ - """ - self.topo_map = yaml.safe_load(msg.data) - self.nodes = {node["node"]["name"]:node["node"] for node in self.topo_map["nodes"]} - self.node_names = [node["node"]["name"] for node in self.topo_map["nodes"]] - rospy.loginfo("Received topo_map") - self.rec_map = True - - def plot_map(self, strip_str=""): - fig = matplotlib.pyplot.figure(figsize=(self.width, self.height)) - min_x = None - max_x = None - min_y = None - max_y = None - ax = fig.add_subplot(111) - # node markers - for node_name in self.node_names: - x = self.nodes[node_name]["pose"]["position"]["x"] - y = self.nodes[node_name]["pose"]["position"]["y"] - ax.plot(x, y, color="black", marker="o", markersize=6, markeredgecolor="r") - - if min_x is None: - min_x = x - max_x = x - min_y = y - max_y = y - else: - if x < min_x: - min_x = x - if x > max_x: - max_x = x - if y < min_y: - min_y = y - if y > max_y: - max_y = y - - x_delta = (max_x - min_x) * 0.1 - y_delta = (max_y - min_y) * 0.1 - # node names - for node_name in self.node_names: - x = self.nodes[node_name]["pose"]["position"]["x"] - y = self.nodes[node_name]["pose"]["position"]["y"] - _node_name = node_name - if strip_str in node_name: - _node_name = node_name.strip(strip_str) - ax.text(x-x_delta/4.0, y + y_delta/4.0, _node_name, fontsize=10) - - for node_name in self.node_names: - from_x = self.nodes[node_name]["pose"]["position"]["x"] - from_y = self.nodes[node_name]["pose"]["position"]["y"] - for edge in self.nodes[node_name]["edges"]: - to_node = edge["node"] - to_x = self.nodes[to_node]["pose"]["position"]["x"] - to_y = self.nodes[to_node]["pose"]["position"]["y"] -# ax.plot([from_x, to_x], [from_y, to_y], color="black", linewidth=2) - ax.arrow(from_x, from_y, to_x-from_x, to_y-from_y, color="black", - head_width=0.2 * min(x_delta, y_delta), - head_length=0.2 * min(x_delta, y_delta), length_includes_head=True, - visible=True) - - ax.set_xlabel("X") - ax.set_ylabel("Y") - ax.set_xlim(min_x - x_delta, max_x + x_delta) - ax.set_ylim(min_y - y_delta, max_y + y_delta) - matplotlib.pyplot.show() - fig.savefig(self.fig_file) - -if __name__ == "__main__": - rospy.init_node("plot_topo_map") - - if len(sys.argv) < 4: - rospy.loginfo("Usage is plot_topo_map path_to_plot_file fig_width_inches fig_height_inches") - else: - fig_file = os.path.abspath(sys.argv[1]) - - try: - map_plotter = TopoMapPlotter(fig_file, float(sys.argv[2]), float(sys.argv[3])) - except: - rospy.signal_shutdown("Couldn't create TopoMapPlotter object") - else: - map_plotter.plot_map(strip_str="WayPoint") - -# rospy.spin() diff --git a/topological_utils/topological_utils/scripts/plot_yaml.py b/topological_utils/topological_utils/scripts/plot_yaml.py deleted file mode 100755 index 77698d40..00000000 --- a/topological_utils/topological_utils/scripts/plot_yaml.py +++ /dev/null @@ -1,126 +0,0 @@ -#!/usr/bin/env python -# ---------------------------------- -# @author: gpdas -# @email: pdasgautham@gmail.com -# @date: -# ---------------------------------- - - -import os -import sys -import matplotlib.pyplot -import yaml - - -class TopoMapPlotter(object): - """TopoMapPlotter class definition - """ - def __init__(self, yaml_file, fig_file, fig_width, fig_height): - """initialise TopoMapPlotter object - - Keyword arguments: - - yaml_file -- topomap in yaml format - fig_file -- name of the map image file to be saved with extension - fig_width -- width of map image file - fig_height -- height of map image file - """ - - self.fig_file = fig_file - self.width = fig_width - self.height = fig_height - - f_handle = open(yaml_file, "r") - self.topo_map = yaml.load(f_handle) - f_handle.close() - - self.nodes = {} - self.node_names = [] - self._extract_topo_map() - - - def _extract_topo_map(self): - """extract node infor from topomap - """ - for node in self.topo_map: - self.nodes[node["node"]["name"]] = node["node"] - self.node_names.append(node["node"]["name"]) - - def plot_map(self, strip_str=""): - """plot into the topo map image file - """ - fig = matplotlib.pyplot.figure(figsize=(self.width, self.height)) - min_x = None - max_x = None - min_y = None - max_y = None - ax = fig.add_subplot(111) - # node markers - for node_name in self.node_names: - x = self.nodes[node_name]["pose"]["position"]["x"] - y = self.nodes[node_name]["pose"]["position"]["y"] - ax.plot(x, y, color="black", marker="o", markersize=6, markeredgecolor="r") - - if min_x is None: - min_x = x - max_x = x - min_y = y - max_y = y - else: - if x < min_x: - min_x = x - if x > max_x: - max_x = x - if y < min_y: - min_y = y - if y > max_y: - max_y = y - - x_delta = (max_x - min_x) * 0.1 - y_delta = (max_y - min_y) * 0.1 - # node names - for node_name in self.node_names: - x = self.nodes[node_name]["pose"]["position"]["x"] - y = self.nodes[node_name]["pose"]["position"]["y"] - _node_name = node_name - if strip_str in node_name: - _node_name = node_name.strip(strip_str) - ax.text(x-x_delta/4.0, y + y_delta/4.0, _node_name, fontsize=10) - - for node_name in self.node_names: - from_x = self.nodes[node_name]["pose"]["position"]["x"] - from_y = self.nodes[node_name]["pose"]["position"]["y"] - for edge in self.nodes[node_name]["edges"]: - to_node = edge["node"] - to_x = self.nodes[to_node]["pose"]["position"]["x"] - to_y = self.nodes[to_node]["pose"]["position"]["y"] -# ax.plot([from_x, to_x], [from_y, to_y], color="black", linewidth=2) - ax.arrow(from_x, from_y, to_x-from_x, to_y-from_y, color="black", - head_width=0.2 * min(x_delta, y_delta), - head_length=0.2 * min(x_delta, y_delta), length_includes_head=True, - visible=True) - - ax.set_xlabel("X") - ax.set_ylabel("Y") - ax.set_xlim(min_x - x_delta, max_x + x_delta) - ax.set_ylim(min_y - y_delta, max_y + y_delta) - fig.savefig(self.fig_file) - -if __name__ == "__main__": - - if len(sys.argv) < 5: - print("Usage is plot_yaml ") - else: - yaml_map = os.path.abspath(sys.argv[1]) - fig_file = os.path.abspath(sys.argv[2]) - - try: - assert os.path.exists(yaml_map) - map_plotter = TopoMapPlotter(yaml_map, fig_file, float(sys.argv[3]), float(sys.argv[4])) - except AssertionError: - print("yaml map does not exist") - except: - print("Couldn't create TopoMapPlotter object") - else: - map_plotter.plot_map(strip_str="WayPoint") - diff --git a/topological_utils/topological_utils/scripts/plot_yaml2.py b/topological_utils/topological_utils/scripts/plot_yaml2.py deleted file mode 100755 index b1b208a5..00000000 --- a/topological_utils/topological_utils/scripts/plot_yaml2.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python -# ---------------------------------- -# @author: gpdas -# @email: pdasgautham@gmail.com -# @date: -# ---------------------------------- - - -import os -import sys -import matplotlib.pyplot -import yaml - - -class TopoMapPlotter(object): - """TopoMapPlotter class definition - """ - def __init__(self, yaml_file, fig_file, fig_width, fig_height): - """initialise TopoMapPlotter object - - Keyword arguments: - - yaml_file -- topomap in yaml format - fig_file -- name of the map image file to be saved with extension - fig_width -- width of map image file - fig_height -- height of map image file - """ - - self.fig_file = fig_file - self.width = fig_width - self.height = fig_height - - with open(yaml_file, "r") as f_handle: - self.topo_map = yaml.safe_load(f_handle) - - self.nodes = {} - self.node_names = [] - self._extract_topo_map() - - - def _extract_topo_map(self): - """extract node infor from topomap - """ - self.nodes = {node["node"]["name"]:node["node"] for node in self.topo_map["nodes"]} - self.node_names = [node["node"]["name"] for node in self.topo_map["nodes"]] - - def plot_map(self, strip_str=""): - """plot into the topo map image file - """ - fig = matplotlib.pyplot.figure(figsize=(self.width, self.height)) - min_x = None - max_x = None - min_y = None - max_y = None - ax = fig.add_subplot(111) - # node markers - for node_name in self.node_names: - x = self.nodes[node_name]["pose"]["position"]["x"] - y = self.nodes[node_name]["pose"]["position"]["y"] - ax.plot(x, y, color="black", marker="o", markersize=6, markeredgecolor="r") - - if min_x is None: - min_x = x - max_x = x - min_y = y - max_y = y - else: - if x < min_x: - min_x = x - if x > max_x: - max_x = x - if y < min_y: - min_y = y - if y > max_y: - max_y = y - - x_delta = (max_x - min_x) * 0.1 - y_delta = (max_y - min_y) * 0.1 - # node names - for node_name in self.node_names: - x = self.nodes[node_name]["pose"]["position"]["x"] - y = self.nodes[node_name]["pose"]["position"]["y"] - _node_name = node_name - if strip_str in node_name: - _node_name = node_name.strip(strip_str) - ax.text(x-x_delta/4.0, y + y_delta/4.0, _node_name, fontsize=10) - - for node_name in self.node_names: - from_x = self.nodes[node_name]["pose"]["position"]["x"] - from_y = self.nodes[node_name]["pose"]["position"]["y"] - for edge in self.nodes[node_name]["edges"]: - to_node = edge["node"] - to_x = self.nodes[to_node]["pose"]["position"]["x"] - to_y = self.nodes[to_node]["pose"]["position"]["y"] -# ax.plot([from_x, to_x], [from_y, to_y], color="black", linewidth=2) - ax.arrow(from_x, from_y, to_x-from_x, to_y-from_y, color="black", - head_width=0.2 * min(x_delta, y_delta), - head_length=0.2 * min(x_delta, y_delta), length_includes_head=True, - visible=True) - - ax.set_xlabel("X") - ax.set_ylabel("Y") - ax.set_xlim(min_x - x_delta, max_x + x_delta) - ax.set_ylim(min_y - y_delta, max_y + y_delta) - fig.savefig(self.fig_file) - -if __name__ == "__main__": - - if len(sys.argv) < 5: - print("Usage is plot_yaml ") - else: - yaml_map = os.path.abspath(sys.argv[1]) - fig_file = os.path.abspath(sys.argv[2]) - - try: - assert os.path.exists(yaml_map) - map_plotter = TopoMapPlotter(yaml_map, fig_file, float(sys.argv[3]), float(sys.argv[4])) - except AssertionError: - print("yaml map does not exist") - except: - print("Couldn't create TopoMapPlotter object") - else: - map_plotter.plot_map(strip_str="WayPoint") - diff --git a/topological_utils/topological_utils/scripts/print_nav_stats.py b/topological_utils/topological_utils/scripts/print_nav_stats.py deleted file mode 100755 index 60294322..00000000 --- a/topological_utils/topological_utils/scripts/print_nav_stats.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python - -import rospy -from topological_navigation_msgs.msg import TopologicalMap, NavStatistics, NavRoute -from mongodb_store.message_store import MessageStoreProxy -#from strands_executive_msgs.msg import Task -#from strands_executive_msgs.srv import AddTasks -#from strands_executive_msgs import task_utils - -class NavRelaxant(object): - def __init__(self, count_threshold): - super(NavRelaxant, self).__init__() - self.node_pairs = [] - rospy.Subscriber('topological_map', TopologicalMap, self.map_callback) - self.msg_store = MessageStoreProxy(collection='nav_stats') - - - def map_callback(self, msg): - node_pairs = [] - for node in msg.nodes: - for edge in node.edges: - node_pairs.append((node.name, edge.node, edge.edge_id)) - self.node_pairs = node_pairs - - def print_pair(self, start, end): - count = len(self.msg_store.query(NavStatistics._type, {"origin": start, "target": end, "final_node": end})) - rospy.loginfo('Nav stats from %s to %s: %s' % (start, end, count)) - - def print_nav_stats(self): - - # only really needed for testing - while len(self.node_pairs) == 0 and not rospy.is_shutdown(): - rospy.sleep(1) - rospy.loginfo('Waiting for nodes') - - - for (start, end, edge_id) in self.node_pairs: - self.print_pair(start, end) - -if __name__ == '__main__': - rospy.init_node('nav_relaxant') - relaxant = NavRelaxant(count_threshold=rospy.get_param('~count_threshold', 5)) - relaxant.print_nav_stats() diff --git a/topological_utils/topological_utils/scripts/remove_node_tags.py b/topological_utils/topological_utils/scripts/remove_node_tags.py deleted file mode 100755 index 8b0cabc4..00000000 --- a/topological_utils/topological_utils/scripts/remove_node_tags.py +++ /dev/null @@ -1,50 +0,0 @@ -#! /usr/bin/env python -# -*- coding: utf-8 -*- -""" -Created on Thu Sep 12 20:21:38 2019 - -@author: gpdas - -""" - - -import yaml -import sys -import os - - -def load_data_from_yaml(file_path): - assert os.path.exists(file_path) - - f_handle = open(file_path, "r") - f_data = yaml.load(f_handle) - f_handle.close() - return f_data - -if __name__ == "__main__": - if len(sys.argv) < 4: - print ("Usage: remove_node_tags.py ") - else: - in_file = sys.argv[1] - out_file = sys.argv[2] - cfg_file = sys.argv[3] - - topo_map = load_data_from_yaml(in_file) - - tags = load_data_from_yaml(cfg_file) - node_tag = {} - for tag in tags: - for node in tags[tag]: - node_tag[node] = tag - - for node in topo_map: - if node["meta"]["node"] in node_tag: - if "tag" in node["meta"]: - if node_tag[node["meta"]["node"]] in node["meta"]["tag"]: - node["meta"]["tag"].remove(node_tag[node["meta"]["node"]]) - if not node["meta"]["tag"]: - node["meta"].pop("tag") - - with open(out_file, 'w') as f_out: - yaml.dump(topo_map, f_out, default_flow_style=False) - print("Successfully created the new file removing configured node tags") diff --git a/topological_utils/topological_utils/scripts/rename_node b/topological_utils/topological_utils/scripts/rename_node deleted file mode 100755 index f98e0915..00000000 --- a/topological_utils/topological_utils/scripts/rename_node +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/python - -""" -Outputs a list of available topological maps -""" - -from topological_utils.nodes import rename_node -import sys - -if __name__ == "__main__": - if len(sys.argv) != 4: - print "Usage: rename_node current_node_name new_node_name tological_map_name)" - sys.exit(1) - old_name, new_name, map_name = sys.argv[1:] - print "Changing %s[%s] to %s"%(map_name, old_name, new_name) - try: - n, e = rename_node(old_name, new_name, map_name) - except Exception, e: - print "Failed: ", e.message - sys.exit(1) - print "Changed ", n, " nodes." - print "Changed ", e, " edges." diff --git a/topological_utils/topological_utils/scripts/rm_map_from_db.py b/topological_utils/topological_utils/scripts/rm_map_from_db.py deleted file mode 100755 index 2be6669f..00000000 --- a/topological_utils/topological_utils/scripts/rm_map_from_db.py +++ /dev/null @@ -1,24 +0,0 @@ -#!/usr/bin/env python - -import sys -import rospy -import pymongo - - -from topological_navigation.topological_map import * - - - -class topologicalNodeRM(object): - - def __init__(self, pointset) : - - self.topo_map = topological_map(pointset) - self.topo_map.delete_map() - rospy.loginfo("All Done ...") - - -if __name__ == '__main__': - pointset=str(sys.argv[1]) - rospy.init_node('map_rm') - server = topologicalNodeRM(pointset) \ No newline at end of file diff --git a/topological_utils/topological_utils/scripts/tmap_from_waypoints.py b/topological_utils/topological_utils/scripts/tmap_from_waypoints.py deleted file mode 100755 index bde77bc6..00000000 --- a/topological_utils/topological_utils/scripts/tmap_from_waypoints.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python - -import sys -import math - -class Topo_node(object): - def __init__(self,node_name,waypoint): - self.node_name=node_name - self.waypoint=waypoint - - def _insert_edges(self, edges): - self.edges=edges - - def _insert_vertices(self, vertices): - self.vertices=vertices - - - -def node_dist(waypoint1,waypoint2): - vector1=waypoint1.split(',') - vector2=waypoint2.split(',') - print vector1[1] - dist = math.sqrt( (float(vector1[0]) - float(vector2[0]))**2 + (float(vector1[1]) - float(vector2[1]))**2 ) - return dist - - -if __name__ == '__main__': - if len(sys.argv) < 3 : - print "usage: tmap_from_waypoint input_file output_file [max_dist_connect]" - sys.exit(2) - - - - filename=str(sys.argv[1]) - outfile=str(sys.argv[2]) - - - if len(sys.argv) == 4: - max_dist_connect=float(sys.argv[3]) - else: - max_dist_connect=1000 - - - fin = open(filename, 'r') - - #Inserting charging point - nnodes=0 - #line = fin.readline() - line = "0,0,0,0,0,0,0\n" - cnode=Topo_node("ChargingPoint",line) - #lnodes=[node] - - lnodes=[] - line = fin.readline() - #Inserting waypoints - while line: - nnodes=nnodes+1 - wname= "WayPoint%d" %nnodes - node=Topo_node(wname,line) - lnodes.append(node) - line = fin.readline() - fin.close() - - #inserting edges - cedges=[] - cedge = {'node':lnodes[0].node_name, 'action':"undocking"} - cedges.append(cedge) - cnode._insert_edges(cedges) - - nnodes=len(lnodes) - eind=0 - for i in lnodes : - edge = {'node':"empty", 'action':"move_base"} - edges=[edge] - if eind == 0: - edge = {'node':cnode.node_name, 'action':"docking"} - edges.append(edge) - for j in lnodes : - if i.node_name is not j.node_name and node_dist(i.waypoint,j.waypoint) %s" %(node.name, self.topo_map.nodes[ind].name) - V1=Point() - V2=Point() - V1= (node._get_pose()).position - V2= (self.topo_map.nodes[ind]._get_pose()).position - marker.scale.x = 0.1 - marker.color.a = 0.8 - marker.color.r = 0.2 - marker.color.g = 0.8 - marker.color.b = 0.2 - marker.points.append(V1) - marker.points.append(V2) - self.map_edges.markers.append(marker) - edge_name=node.name+"_"+self.topo_map.nodes[ind].name - self._edge_marker(edge_name, V1, V2, edge_name) - idn = 0 - for m in self.map_edges.markers: - m.id = idn - idn += 1 - - - - def _create_vertices_array(self) : - - for node in self.topo_map.nodes : - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.LINE_STRIP - #marker.lifetime=2 - marker.scale.x = 0.1 - marker.color.a = 0.8 - marker.color.r = 0.7 - marker.color.g = 0.1 - marker.color.b = 0.2 - node._get_coords() - count=0 - for i in node.vertices : - #print i[0], i[1] - Vert = Point() - Vert.z = 0.05 - Vert.x = node.px + i[0] - Vert.y = node.py + i[1] - marker.points.append(Vert) - vertname = "%s-%d" %(node.name, count) - Pos = Pose() - Pos.position = Vert - self._vertex_marker(vertname, Pos, vertname) - count+=1 - marker.points.append(marker.points[0]) - self.node_zone.markers.append(marker) - - idn = 0 - for m in self.node_zone.markers: - m.id = idn - idn += 1 - - - - def _create_interactive_markers(self): - for i in self.topo_map.nodes : - self._create_marker(i.name, i._get_pose(), i.name) - - - - def _initMenu(self): - self.h_first_entry = self.menu_handler.insert( "First Entry" ) - entry = self.menu_handler.insert( "deep", parent=h_first_entry) - entry = self.menu_handler.insert( "sub", parent=entry ); - entry = self.menu_handler.insert( "menu", parent=entry, callback=deepCb ); - - self.menu_handler.setCheckState( self.menu_handler.insert( "Show First Entry", callback=enableCb ), MenuHandler.CHECKED ) - - self.sub_menu_handle = self.menu_handler.insert( "Switch" ) - for i in range(5): - s = "Mode " + str(i) - self.h_mode_last = self.menu_handler.insert( s, parent=sub_menu_handle, callback=modeCb ) - self.menu_handler.setCheckState( self.h_mode_last, MenuHandler.UNCHECKED) - # check the very last entry - menu_handler.setCheckState( self.h_mode_last, MenuHandler.CHECKED ) - - - - def _edge_marker(self, marker_name, point1, point2, marker_description="edge marker") : - # create an interactive marker for our server - marker = InteractiveMarker() - marker.header.frame_id = "map" - marker.name = marker_name - marker.description = marker_description - - # the marker in the middle - box_marker = Marker() - box_marker.type = Marker.CYLINDER - box_marker.scale.x = 0.45 - box_marker.scale.y = 0.25 - box_marker.scale.z = 0.15 - box_marker.color.r = 0.0 - box_marker.color.g = 0.5 - box_marker.color.b = 0.5 - box_marker.color.a = 1.0 - - # create a non-interactive control which contains the box - box_control = InteractiveMarkerControl() - box_control.always_visible = True - box_control.markers.append( box_marker ) - marker.controls.append( box_control ) - - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 1 - control.orientation.y = 0 - control.orientation.z = 0 - control.name = "context_menu" - control.interaction_mode = InteractiveMarkerControl.MENU - marker.controls.append(control) - - self._edge_server.insert(marker, self._menu_feedback) - self._edge_server.applyChanges() - - pose = Pose() - - pose.position.x = (point1.x+point2.x)/2 - pose.position.y = (point1.y+point2.y)/2 - pose.position.z = (point1.z+point2.z)/2 - - if pose is not None: - pose.position.z=pose.position.z+0.15 - self._edge_server.setPose( marker.name, pose ) - self._edge_server.applyChanges() - - - def _create_marker(self, marker_name, pose, marker_description="waypoint marker") : - # create an interactive marker for our server - marker = InteractiveMarker() - marker.header.frame_id = "map" - marker.name = marker_name - marker.description = marker_description - - # the marker in the middle - box_marker = Marker() - box_marker.type = Marker.ARROW - box_marker.scale.x = 0.45 - box_marker.scale.y = 0.25 - box_marker.scale.z = 0.15 - box_marker.color.r = 0.0 - box_marker.color.g = 0.5 - box_marker.color.b = 0.5 - box_marker.color.a = 1.0 - - # create a non-interactive control which contains the box - box_control = InteractiveMarkerControl() - box_control.always_visible = True - box_control.markers.append( box_marker ) - marker.controls.append( box_control ) - - # move x - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 1 - control.orientation.y = 0 - control.orientation.z = 0 - control.name = "move_x" - control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - marker.controls.append(control) - - #move y - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 0 - control.orientation.z = 1 - control.name = "move_y" - control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - marker.controls.append(control) - - #rotate z - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 1 - control.orientation.z = 0 - control.name = "rotate_z" - control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS - marker.controls.append(control) - - self._marker_server.insert(marker, self._marker_feedback) - self._marker_server.applyChanges() - - if pose is not None: - pose.position.z=pose.position.z+0.15 - self._marker_server.setPose( marker.name, pose ) - self._marker_server.applyChanges() - - - - def _vertex_marker(self, marker_name, pose, marker_description="vertex marker"): - # create an interactive marker for our server - marker = InteractiveMarker() - marker.header.frame_id = "map" - marker.name = marker_name - marker.description = marker_description - - # the marker in the middle - box_marker = Marker() - box_marker.type = Marker.SPHERE - box_marker.scale.x = 0.25 - box_marker.scale.y = 0.25 - box_marker.scale.z = 0.25 - box_marker.color.r = 0.5 - box_marker.color.g = 0.0 - box_marker.color.b = 0.5 - box_marker.color.a = 0.8 - - # create a non-interactive control which contains the box - box_control = InteractiveMarkerControl() - box_control.always_visible = True - #box_control.always_visible = False - box_control.markers.append( box_marker ) - marker.controls.append( box_control ) - - # move x - control = InteractiveMarkerControl() - control.orientation.w = 1 - control.orientation.x = 0 - control.orientation.y = 1 - control.orientation.z = 0 - control.always_visible = False -# control.name = "move_x" -# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - control.name = "move_plane" - control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE - marker.controls.append(control) - - - self._vertex_server.insert(marker, self._vertex_feedback) - self._vertex_server.applyChanges() - - if pose is not None: - pose.position.z=pose.position.z+0.15 - self._vertex_server.setPose( marker.name, pose ) - self._vertex_server.applyChanges() - - - - def _marker_feedback(self, feedback): - self.in_feedback=True - self.topo_map.update_node_waypoint(feedback.marker_name, feedback.pose) - self.update_needed=True - - - def _menu_feedback(self, feedback): - print feedback - - - def _vertex_feedback(self, feedback): - self.in_feedback=True - vertex_name = feedback.marker_name.rsplit('-', 1) - node_name = vertex_name[0] - vertex_index = int(vertex_name[1]) - self.topo_map.update_node_vertex(node_name, vertex_index, feedback.pose) - self.update_needed=True - - - def timer_callback(self): - - if self.update_needed and not self.in_feedback : - self._delete_everything() - self._update_everything() - else : - if self.in_feedback : - self.in_feedback=False - self.map_pub.publish(self.map_nodes) - self.map_edge_pub.publish(self.map_edges) - self.map_zone_pub.publish(self.node_zone) - - if not self._killall_timers : - t = Timer(1.0, self.timer_callback) - t.start() - - - - def _update_everything(self) : - - print "loading file from map %s" %self._point_set - self.topo_map = topological_map(self._point_set) - print "Done" - - self._marker_server = InteractiveMarkerServer(self._point_set+"_markers") - self._vertex_server = InteractiveMarkerServer(self._point_set+"_zones") - self._edge_server = InteractiveMarkerServer(self._point_set+"_edges") - - self.map_edges = MarkerArray() - self.map_nodes = MarkerArray() - self.node_zone = MarkerArray() - - self._create_interactive_markers() - self._create_marker_array() - self._create_edges_array() - self._create_vertices_array() - - self.update_needed=False - - - - def _delete_everything(self) : - - del self._marker_server - del self._vertex_server - - del self.map_edges - del self.map_nodes - del self.node_zone - del self.topo_map - - - - def _on_node_shutdown(self): - self._killall_timers=True - #sleep(2) - - -if __name__ == '__main__': - mapname=str(sys.argv[1]) - rospy.init_node('topological_visualisation') - server = TopologicalMapVis(rospy.get_name(),mapname) diff --git a/topological_utils/topological_utils/scripts/waypoints_to_yaml_tmap.py b/topological_utils/topological_utils/scripts/waypoints_to_yaml_tmap.py deleted file mode 100755 index e20d2cd5..00000000 --- a/topological_utils/topological_utils/scripts/waypoints_to_yaml_tmap.py +++ /dev/null @@ -1,175 +0,0 @@ -#!/usr/bin/env python - -#============================================================================== -# This Script takes a waypoint file and creates a Yaml file with a topological -# map definition -# -# Usage: -# -# `rosrun topological_utils waypoints_to_yaml_tmap.py input_file output_file pointset map [max_dist_connect]` -# -#============================================================================== - -import sys -import math -import json -import bson -import pymongo -import yaml -import mongodb_store.util as dc_util - -from topological_navigation_msgs.msg import TopologicalNode -from topological_navigation_msgs.msg import Vertex -from topological_navigation_msgs.msg import Edge -from geometry_msgs.msg import Pose - - -def pose_from_waypoint(waypoint): - v=waypoint.split(',') - p = Pose() - p.position.x = float(v[0]) - p.position.y = float(v[1]) - p.position.z = float(v[2]) - p.orientation.x = float(v[3]) - p.orientation.y = float(v[4]) - p.orientation.z = float(v[5]) - p.orientation.w = float(v[6]) - return p - -def get_vertex_list(vertices): - verts = [] - for i in vertices: - v = Vertex() - v.x = i[0] - v.y = i[1] - verts.append(v) - return verts - -def node_dist(node1,node2): - dist = math.sqrt((node1.pose.position.x - node2.pose.position.x)**2 + (node1.pose.position.y - node2.pose.position.y)**2 ) - return dist - - -def get_edge_id(source, target, eids): - test=0 - eid = '%s_%s' %(source, target) - while eid in eids: - eid = '%s_%s_%3d' %(source, target, test) - test += 1 - return eid - - -def get_empty_edge(mapname,standard_action): - e = Edge() - e.action = standard_action - e.top_vel =0.55 - e.map_2d = mapname - e.use_default_recovery = True - e.use_default_nav_recovery = True - e.use_default_helpers = True - return e - -def create_node(name, mapname, pointset, line, vertices): - o=[] - n = TopologicalNode() - n.name = name - n.map = mapname - n.pointset = pointset - n.pose = pose_from_waypoint(line) - n.yaw_goal_tolerance = 0.1 - n.xy_goal_tolerance = 0.3 - n.verts = get_vertex_list(vertices) - m = {} - m["map"] = mapname - m["pointset"] = pointset - m["node"] = n.name - o.append(n) - o.append(m) - return o - -if __name__ == '__main__': - if len(sys.argv) < 5 : - print "usage: waypoints_to_yaml_tmap.py input_file output_file pointset map [max_dist_connect]" - sys.exit(2) - - filename=str(sys.argv[1]) - outfile=str(sys.argv[2]) - pointset=str(sys.argv[3]) - mapname=str(sys.argv[4]) - vertices=[(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69), (-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69), (0.287, -0.69), (0.69, -0.287)] - - if len(sys.argv) == 6: - max_dist_connect=float(sys.argv[5]) - else: - max_dist_connect=100 - - standard_action = 'move_base' - eids=[] #list of known edge id's - - - lnodes=[] - - - fin = open(filename, 'r') - line = fin.readline() - nnodes=0 - - #Inserting waypoints - while line: - nnodes=nnodes+1 - wname= "WayPoint%d" %nnodes - o=create_node(wname, mapname, pointset, line, vertices) - lnodes.append(o) - line = fin.readline() - fin.close() - - -# for i in lnodes: print i["node"] - print "calculate edges" - edge_names = [] - for i in lnodes: - for j in lnodes: - if node_dist(i[0],j[0]) < max_dist_connect and i[0].name != j[0].name : - print "connecting %s to %s"%(i[0].name,j[0].name) - e = get_empty_edge(mapname, standard_action) - e.edge_id = "%s_%s"%(i[0].name,j[0].name) - e.node = j[0].name - i[0].edges.append(e) - - print "charging point" - #a=[] - a=create_node('ChargingPoint', mapname, pointset, "0,0,0,0,0,0,0\n", vertices) - print "Connecting %s to %s"%(a[0].name, lnodes[0][0].name) - e = get_empty_edge(mapname, 'undocking') - e.edge_id = "%s_%s"%(a[0].name, lnodes[0][0].name) - e.node = lnodes[0][0].name - a[0].edges.append(e) - lnodes.append(a) - - - print "Connecting %s to %s"%(lnodes[0][0].name, a[0].name) - e = get_empty_edge(mapname, 'docking') - e.edge_id = "%s_%s"%(lnodes[0][0].name, a[0].name) - e.node = a[0].name - lnodes[0][0].edges.append(e) - #print lnodes[0][0].edges - - onodes = [] - for i in lnodes: - r = [] - q = dc_util.msg_to_document(i[0]) - r.append(q) - r.append(i[1]) - onodes.append(r) - - - yml = yaml.safe_dump(onodes, default_flow_style=False) - print yml - #print s_output - - fh = open(outfile, "w") - #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) - s_output = str(yml) - print s_output - fh.write(s_output) - fh.close diff --git a/topological_utils/topological_utils/topological_utils/__init__.py b/topological_utils/topological_utils/topological_utils/__init__.py deleted file mode 100644 index 8b137891..00000000 --- a/topological_utils/topological_utils/topological_utils/__init__.py +++ /dev/null @@ -1 +0,0 @@ - diff --git a/topological_utils/topological_utils/topological_utils/nodes.py b/topological_utils/topological_utils/topological_utils/nodes.py deleted file mode 100644 index 6c2e0cc5..00000000 --- a/topological_utils/topological_utils/topological_utils/nodes.py +++ /dev/null @@ -1,63 +0,0 @@ -""" -Provides routines for working with topological map nodes -""" - -from topological_navigation_msgs.msg import TopologicalNode -from mongodb_store.message_store import MessageStoreProxy -import queries -import rospy -import copy - -def rename_node(name, new_name, top_map_name): - """ - Renames a topological map node. All edges will be updated to reflect the change. - - @param name: the current name of the node to be changec - @param new_name: the new name to give the node - @param top_map_name: the name of the topological map to work with (pointset) - - @return (number of nodes changed, number of edges changed) - """ - - maps = queries.get_maps() - if top_map_name not in maps: - raise Exception("Unknown topological map.") - - msg_store = MessageStoreProxy(collection='topological_maps') - - nodes = msg_store.query(TopologicalNode._type, {}, {'pointset':top_map_name}) - node_names = [node.name for node,meta in nodes] - node_renames = 0 - edge_changes = 0 - node_changes = 0 - - if name not in node_names: - raise Exception("No such node.") - if new_name in node_names: - raise Exception("New node name already in use.") - - old_metas = [] - for node, meta in nodes: - old_metas.append(copy.deepcopy(meta)) - if meta["node"] == name: - meta["node"] = new_name - if node.name == name: - node.name = new_name - node_renames += 1 - if node_renames > 1: - raise Exception("More than one node has the same name!") - for edge in node.edges: - if edge.node == name: - edge.node = new_name - if edge.edge_id.find(name) > -1: - edge.edge_id = edge.edge_id.replace(name, new_name) - edge_changes += 1 - - # Save the changed nodes - for ((node, meta), old_meta) in zip(nodes, old_metas): - changed = msg_store.update(node, meta, {'name':old_meta['node'], - 'pointset':meta['pointset']}) - if changed.success: - node_changes += 1 - - return (node_changes, edge_changes) diff --git a/topological_utils/topological_utils/topological_utils/queries.py b/topological_utils/topological_utils/topological_utils/queries.py deleted file mode 100644 index a808c284..00000000 --- a/topological_utils/topological_utils/topological_utils/queries.py +++ /dev/null @@ -1,60 +0,0 @@ -""" -Provides routines for querying the database for map information. -""" - -from topological_navigation_msgs.msg import TopologicalNode -from mongodb_store.message_store import MessageStoreProxy -import rospy - -def get_maps(): - """ - Queries the database and returns details of the available topological maps. - :return: A dictionary where each key is the name of a topological map and each - item is a dictionary of details. Details are: - "number_nodes" ; integer - "edge_actions" : list of action server names used for traversal - "last_modified" : datetime.datetime object for the last time a node was inserted - """ - maps = dict() - msg_store = MessageStoreProxy(collection='topological_maps') - - nodes = msg_store.query(TopologicalNode._type) - - for node in nodes: - pointset = node[1]["pointset"] - if not maps.has_key(pointset): - maps[pointset] = {"number_nodes": 0, "edge_actions": set(), "last_modified": ""} - maps[pointset]["number_nodes"] += 1 - if (maps[pointset]["last_modified"] == "" or - node[1]["inserted_at"] > maps[pointset]["last_modified"]): - maps[pointset]["last_modified"] = node[1]["inserted_at"] - for edge in node[0].edges: - maps[pointset]["edge_actions"].add(edge.action) - - return maps - - -def get_nodes(point_set, meta_query): - - msg_store = MessageStoreProxy(collection="topological_maps") - - query_meta = {} - query_meta["pointset"] = {} - #query_meta["map"] = {} - query_meta["contains.category"] = {} - query_meta["pointset"]['$regex'] = point_set - #query_meta["map"]['$regex'] = map_name - query_meta["contains.category"]['$regex'] = meta_query - - - nodes = msg_store.query(TopologicalNode._type, {}, query_meta); - available = len(nodes) > 0 - - if available <= 0 : - print "Desired pointset '"+point_set+"' not in datacentre" - print "Available pointsets: "+str(available) - - return nodes - -if __name__ == "__main__": - get_maps()