├── .devcontainer ├── Dockerfile ├── devcontainer.json ├── install.sh ├── lcas.repos └── run.sh ├── .github └── workflows │ └── ci.yaml ├── .gitignore ├── LICENSE ├── README.md ├── topological_navigation ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── __init__.py ├── config │ ├── bt_tree_default.xml │ ├── bt_tree_goal_align.xml │ ├── bt_tree_in_row.xml │ ├── bt_tree_in_row_operation.xml │ ├── bt_tree_in_row_recovery.xml │ ├── navigation_goal.yaml │ ├── sample_edge_reconfig_groups.yaml │ ├── template_edge.yaml │ ├── template_node_2.yaml │ └── tmap-schema.yaml ├── cyclonedds.log ├── launch │ ├── manual_topomapping.launch.py │ ├── minimal_topological_navigation.launch │ ├── reconf_at_edges_server.launch │ ├── topo_nav_global.launch │ ├── topo_nav_local.launch │ ├── topological_map_visualise.launch │ ├── topological_navigation.launch │ ├── topological_navigation_empty_map.launch │ └── topological_restrictions_multirobot.launch ├── package.xml ├── resource │ └── topological_navigation ├── setup.cfg ├── setup.py ├── test │ ├── conf │ │ └── network_autogen.tmap2.yaml │ ├── static_transformer.py │ └── test_navigationcore.py ├── tests │ ├── README.md │ ├── conf │ │ ├── scenario_server.yaml │ │ └── test.tmap2 │ ├── map_manager.test │ ├── map_manager_tester.py │ ├── navigation_scenarios.test │ ├── navigation_test_joypad_control.py │ ├── scenario_server.py │ ├── topological_navigation_tester_critical.py │ ├── topological_navigation_tester_supplementary.py │ ├── travel_time_estimator.test │ └── travel_time_tester.py └── topological_navigation │ ├── __init__.py │ ├── edge_action_manager.py │ ├── edge_action_manager2.py │ ├── edge_controller.py │ ├── edge_reconfigure_manager.py │ ├── edge_reconfigure_manager2.py │ ├── edge_std.py │ ├── goto.py │ ├── load_maps_from_yaml.py │ ├── manager.py │ ├── manager2.py │ ├── map_marker.py │ ├── marker_arrays.py │ ├── navigation_stats.py │ ├── node_controller.py │ ├── node_manager.py │ ├── point2line.py │ ├── policies.py │ ├── policy_marker.py │ ├── policy_marker2.py │ ├── publisher.py │ ├── restrictions_impl.py │ ├── route_search.py │ ├── route_search2.py │ ├── scripts │ ├── __init__.py │ ├── actions_bt.py │ ├── evaluate_top_pred.py │ ├── get_simple_policy.py │ ├── get_simple_policy2.py │ ├── in_row_operations.py │ ├── localisation.py │ ├── localisation2.py │ ├── manual_edge_predictions.py │ ├── manual_topomapping.py │ ├── map_manager.py │ ├── map_manager2.py │ ├── map_publisher.py │ ├── mean_based_prediction.py │ ├── nav_client.py │ ├── navigation.py │ ├── navigation2.py │ ├── navstats_logger.py │ ├── param_processing.py │ ├── reconf_at_edges_server.py │ ├── restrictions_manager.py │ ├── search_route.py │ ├── speed_based_prediction.py │ ├── topological_prediction.py │ ├── topological_transform_publisher.py │ ├── travel_time_estimator.py │ ├── visualise_map.py │ ├── visualise_map2.py │ └── visualise_map_ros2.py │ ├── testing.py │ ├── tmap_utils.py │ ├── topological_map.py │ ├── topological_node.py │ ├── topomap_marker.py │ ├── topomap_marker2.py │ └── vertex_controller.py ├── topological_navigation_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── action │ ├── BuildTopPrediction.action │ ├── ExecutePolicyMode.action │ └── GotoNode.action ├── msg │ ├── AddEdgeReq.msg │ ├── AddNodeReq.msg │ ├── ClosestEdges.msg │ ├── CurrentEdge.msg │ ├── Edge.msg │ ├── ExecutePolicyModeFeedback.msg │ ├── ExecutePolicyModeGoal.msg │ ├── GotoNodeFeedback.msg │ ├── NavRoute.msg │ ├── NavStatistics.msg │ ├── SetInfluenceZoneReq.msg │ ├── TopologicalMap.msg │ ├── TopologicalNode.msg │ ├── TopologicalRoute.msg │ ├── UpdateEdgeConfigReq.msg │ └── Vertex.msg ├── package.xml └── srv │ ├── AddContent.srv │ ├── AddDatum.srv │ ├── AddEdge.srv │ ├── AddEdgeArray.srv │ ├── AddEdgeRviz.srv │ ├── AddNode.srv │ ├── AddNodeArray.srv │ ├── AddTag.srv │ ├── EstimateTravelTime.srv │ ├── EvaluateEdge.srv │ ├── EvaluateNode.srv │ ├── GetEdgesBetweenNodes.srv │ ├── GetNodeTags.srv │ ├── GetRouteBetween.srv │ ├── GetRouteTo.srv │ ├── GetTaggedNodes.srv │ ├── GetTags.srv │ ├── GetTopologicalMap.srv │ ├── LoadTopoNavTestScenario.srv │ ├── LocalisePose.srv │ ├── ModifyTag.srv │ ├── NodeMetadata.srv │ ├── PredictEdgeState.srv │ ├── ReconfAtEdges.srv │ ├── RestrictMap.srv │ ├── RmvNode.srv │ ├── RunTopoNavTestScenario.srv │ ├── SetInfluenceZone.srv │ ├── SetInfluenceZoneArray.srv │ ├── UpdateAction.srv │ ├── UpdateEdge.srv │ ├── UpdateEdgeConfig.srv │ ├── UpdateEdgeConfigArray.srv │ ├── UpdateEdgeLegacy.srv │ ├── UpdateFailPolicy.srv │ ├── UpdateNodeName.srv │ ├── UpdateNodeTolerance.srv │ ├── UpdateRestrictions.srv │ └── WriteTopologicalMap.srv ├── topological_rviz_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── COLCON_IGNORE ├── LICENSE ├── README.md ├── conf │ └── topological_rviz_tools.rviz ├── images │ ├── 00_annotated.png │ └── 00_annotated.xcf ├── include │ └── topological_rviz_tools │ │ ├── edge_controller.hpp │ │ ├── edge_property.hpp │ │ ├── node_controller.hpp │ │ ├── node_property.hpp │ │ ├── pose_property.hpp │ │ ├── tag_controller.hpp │ │ ├── tag_property.hpp │ │ ├── topmap_manager.hpp │ │ ├── topological_edge_tool.hpp │ │ ├── topological_map_panel.hpp │ │ └── topological_node_tool.hpp ├── launch │ └── strands_rviz_topmap.launch ├── package.xml ├── plugin_description.xml ├── scripts │ └── python_topmap_interface.py ├── setup.py └── src │ ├── edge_controller.cpp │ ├── edge_property.cpp │ ├── node_controller.cpp │ ├── node_property.cpp │ ├── pose_property.cpp │ ├── tag_controller.cpp │ ├── tag_property.cpp │ ├── topmap_manager.cpp │ ├── topological_edge_tool.cpp │ ├── topological_map_panel.cpp │ └── topological_node_tool.cpp └── topological_utils ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── __init__.py ├── launch ├── create_topological_map.launch ├── dummy_topological_navigation.launch ├── empty_topological_map.launch ├── mapping.launch ├── topological_map_edition.launch └── topological_prediction_test.launch ├── package.xml ├── resource ├── __init__.py └── topological_utils ├── setup.cfg ├── setup.py ├── support ├── empty.tmap └── map_edition.rviz └── topological_utils ├── __init__.py ├── scripts ├── __init__,py ├── __init__.py ├── add_content.py ├── add_edge.py ├── add_node.py ├── add_node_tags.py ├── check_map ├── crop_map.py ├── draw_predicted_map.py ├── dummy_topological_navigation.py ├── edge_length_analysis.py ├── edge_reconf_groups_to_tmap2.py ├── evaluate_predictions.py ├── goal_converter.py ├── insert_empty_map.py ├── insert_map.py ├── joy_add_node.py ├── joy_add_waypoint.py ├── list_maps ├── load_json_map.py ├── load_yaml_map.py ├── map_collection_change.py ├── map_converter.py ├── map_export.py ├── map_to_json.py ├── map_to_yaml.py ├── migrate.py ├── node_metadata.py ├── node_rm.py ├── plot_topo_map.py ├── plot_topo_map2.py ├── plot_yaml.py ├── plot_yaml2.py ├── print_nav_stats.py ├── remove_node_tags.py ├── rename_node ├── rm_map_from_db.py ├── tmap_from_waypoints.py ├── tmap_to_yaml.py ├── topological_map_update.py ├── toponav_tool.py ├── visualise_map.py └── waypoints_to_yaml_tmap.py └── topological_utils ├── __init__.py ├── nodes.py └── queries.py /.devcontainer/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM lcas.lincoln.ac.uk/lcas/ros:humble 2 | 3 | RUN apt-get update && export DEBIAN_FRONTEND=noninteractive \ 4 | && apt-get -y upgrade \ 5 | && apt-get -y install --no-install-recommends ros-humble-desktop ros-humble-simulation bash-completion python3-colcon-common-extensions \ 6 | nano syslog-ng \ 7 | build-essential \ 8 | curl \ 9 | swig \ 10 | python3-pip \ 11 | ros-humble-rviz2 \ 12 | python3-rosinstall-generator \ 13 | wget 14 | 15 | RUN useradd -rm -d /home/lcas -s /bin/bash -g root -G sudo -u 1001 lcas 16 | RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers 17 | 18 | RUN echo "source /opt/ros/humble/setup.bash" >> /home/lcas/.bashrc 19 | 20 | RUN pip3 install bloom 21 | 22 | COPY *repos *.sh /tmp/.devcontainer/ 23 | RUN bash /tmp/.devcontainer/install.sh 24 | 25 | RUN mkdir -p /home/lcas/ws/src && ln -s /workspaces /home/lcas/ws/src/workspaces 26 | WORKDIR /home/lcas/ws 27 | -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | // For format details, see https://aka.ms/devcontainer.json. For config options, see the 2 | // README at: https://github.com/devcontainers/templates/tree/main/src/ubuntu 3 | { 4 | "name": "topological_navigation", 5 | // Or use a Dockerfile or Docker Compose file. More info: https://containers.dev/guide/dockerfile 6 | // "image": "lcas.lincoln.ac.uk/lcas/ros:humble", 7 | "build": { 8 | "dockerfile": "Dockerfile", 9 | "context": "." 10 | }, 11 | 12 | // Features to add to the dev container. More info: https://containers.dev/features. 13 | "features": { 14 | "ghcr.io/devcontainers/features/desktop-lite:1": {} 15 | }, 16 | 17 | // Use 'forwardPorts' to make a list of ports inside the container available locally. 18 | // "forwardPorts": [], 19 | "forwardPorts": [6080, 5901], 20 | "portsAttributes": { 21 | "6080": { 22 | "label": "novnc" 23 | }, 24 | "5901": { 25 | "label": "vnc" 26 | } 27 | }, 28 | // Use 'postCreateCommand' to run commands after the container is created. 29 | "postCreateCommand": "bash .devcontainer/run.sh", 30 | "containerUser": "lcas", 31 | // Configure tool-specific properties. 32 | // "customizations": {}, 33 | "runArgs": [ "--security-opt", "seccomp=unconfined", "--cap-add=NET_ADMIN" ], 34 | 35 | "containerEnv": { 36 | "ROS_LOCALHOST_ONLY": "0", 37 | "LIBGL_ALWAYS_SOFTWARE": "1" // Needed for software rendering of opengl 38 | }, 39 | 40 | // Uncomment to connect as root instead. More info: https://aka.ms/dev-containers-non-root. 41 | "customizations": { 42 | "vscode": { 43 | "extensions": [ 44 | "dotjoshjohnson.xml", 45 | "zachflower.uncrustify", 46 | "ms-python.python", 47 | "ms-vscode.cpptools", 48 | "redhat.vscode-yaml", 49 | "smilerobotics.urdf", 50 | "streetsidesoftware.code-spell-checker", 51 | "twxs.cmake", 52 | "king2021.vnc-extension", 53 | "nonanonno.vscode-ros2", 54 | "deitry.colcon-helper", 55 | "yzhang.markdown-all-in-one", 56 | "github.vscode-github-actions" 57 | ] 58 | } 59 | } 60 | 61 | } 62 | -------------------------------------------------------------------------------- /.devcontainer/install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | source /opt/ros/humble/setup.bash 6 | apt update 7 | rosdep --rosdistro=humble update 8 | 9 | 10 | rm -rf /opt/ros/lcas 11 | mkdir -p /opt/ros/lcas/src 12 | chown -R lcas /opt/ros/lcas 13 | cd /opt/ros/lcas/src 14 | vcs import < /tmp/.devcontainer/lcas.repos 15 | rosdep install --from-paths . -i -y 16 | cd /opt/ros/lcas 17 | colcon build 18 | 19 | 20 | #cd /home/lcas/ws 21 | #colcon build 22 | echo "source /opt/ros/lcas/install/setup.bash" >> ~/.bashrc 23 | 24 | -------------------------------------------------------------------------------- /.devcontainer/lcas.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | # teaching: 3 | # type: git 4 | # url: https://github.com/LCAS/teaching.git 5 | # version: 2324-devel 6 | # limo_ros2: 7 | # type: git 8 | # url: https://github.com/LCAS/limo_ros2.git 9 | # version: humble 10 | -------------------------------------------------------------------------------- /.devcontainer/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | WORKSPACE="`pwd`" 6 | 7 | source /opt/ros/lcas/install/setup.bash 8 | sudo apt update 9 | rosdep --rosdistro=humble update 10 | 11 | 12 | # sudo rm -rf /opt/ros/lcas 13 | # sudo mkdir -p /opt/ros/lcas/src 14 | # sudo chown -R lcas /opt/ros/lcas 15 | # cd /opt/ros/lcas/src 16 | # vcs import < $WORKSPACE/.devcontainer/lcas.repos 17 | # rosdep install --from-paths . -i -y 18 | # cd /opt/ros/lcas 19 | # colcon build 20 | 21 | 22 | cd /home/lcas/ws 23 | rosdep install --from-paths ./src -i -y 24 | colcon build --symlink-install 25 | echo "source /home/lcas/ws/install/setup.bash" >> ~/.bashrc 26 | 27 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: ros CI 2 | 3 | on: 4 | push: 5 | branches: [ humble-dev ] 6 | pull_request: 7 | branches: [ humble-dev ] 8 | 9 | jobs: 10 | test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. 11 | runs-on: ubuntu-latest 12 | strategy: 13 | matrix: 14 | ros_distribution: 15 | # - noetic 16 | - humble 17 | - iron 18 | 19 | # Define the Docker image(s) associated with each ROS distribution. 20 | # The include syntax allows additional variables to be defined, like 21 | # docker_image in this case. See documentation: 22 | # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build 23 | # 24 | # Platforms are defined in REP 3 and REP 2000: 25 | # https://ros.org/reps/rep-0003.html 26 | # https://ros.org/reps/rep-2000.html 27 | include: 28 | # Noetic Ninjemys (May 2020 - May 2025) 29 | # - docker_image: ubuntu:focal 30 | # ros_distribution: noetic 31 | # ros_version: 1 32 | 33 | # Humble Hawksbill (May 2022 - May 2027) 34 | - docker_image: ubuntu:jammy 35 | ros_distribution: humble 36 | ros_version: 2 37 | 38 | # Iron Irwini (May 2023 - November 2024) 39 | - docker_image: ubuntu:jammy 40 | ros_distribution: iron 41 | ros_version: 2 42 | 43 | # # Rolling Ridley (No End-Of-Life) 44 | # - docker_image: ubuntu:jammy 45 | # ros_distribution: rolling 46 | # ros_version: 2 47 | 48 | container: 49 | image: ${{ matrix.docker_image }} 50 | steps: 51 | - name: setup ROS environment 52 | uses: LCAS/setup-ros@master 53 | with: 54 | required-ros-distributions: ${{ matrix.ros_distribution }} 55 | - name: build and test ROS 1 56 | if: ${{ matrix.ros_version == 1 }} 57 | uses: ros-tooling/action-ros-ci@v0.3 58 | with: 59 | target-ros1-distro: ${{ matrix.ros_distribution }} 60 | - name: build and test ROS 2 61 | if: ${{ matrix.ros_version == 2 }} 62 | uses: ros-tooling/action-ros-ci@v0.3 63 | with: 64 | target-ros2-distro: ${{ matrix.ros_distribution }} 65 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .coverage 43 | .coverage.* 44 | .cache 45 | nosetests.xml 46 | coverage.xml 47 | *.cover 48 | .hypothesis/ 49 | .pytest_cache/ 50 | 51 | # Translations 52 | *.mo 53 | *.pot 54 | 55 | #vscode 56 | .vscode 57 | .vscode/* 58 | !.vscode/settings.json 59 | !.vscode/tasks.json 60 | !.vscode/launch.json 61 | !.vscode/extensions.json 62 | *.code-workspace 63 | 64 | # Local History for Visual Studio Code 65 | .history/ 66 | 67 | # Nano auto-backups 68 | **/*.*~ 69 | **/*.*.swp 70 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # topological_navigation 2 | 3 | A topological navigation planning framework 4 | -------------------------------------------------------------------------------- /topological_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(topological_navigation) 3 | 4 | # Export information to downstream packages 5 | ament_export_dependencies(rosidl_default_runtime) 6 | ament_package() 7 | -------------------------------------------------------------------------------- /topological_navigation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_navigation/__init__.py -------------------------------------------------------------------------------- /topological_navigation/config/bt_tree_default.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /topological_navigation/config/bt_tree_goal_align.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /topological_navigation/config/bt_tree_in_row.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /topological_navigation/config/bt_tree_in_row_operation.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /topological_navigation/config/bt_tree_in_row_recovery.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /topological_navigation/config/navigation_goal.yaml: -------------------------------------------------------------------------------- 1 | topological_navigation/navigation_goal: 2 | action_type: geometry_msgs/PoseStamped 3 | action: NavigateToPose 4 | goal: 5 | target_pose: 6 | header: 7 | frame_id: $node.parent_frame 8 | pose: $node.pose 9 | -------------------------------------------------------------------------------- /topological_navigation/config/template_edge.yaml: -------------------------------------------------------------------------------- 1 | action: move_base 2 | action_type: move_base_msgs/MoveBaseGoal 3 | config: [] 4 | edge_id: origin_destination 5 | fail_policy: fail 6 | fluid_navigation: True 7 | goal: 8 | target_pose: 9 | header: 10 | frame_id: $node.parent_frame 11 | pose: $node.pose 12 | node: destination 13 | recovery_behaviours_config: '' 14 | restrictions_planning: 'True' 15 | restrictions_runtime: 'True' -------------------------------------------------------------------------------- /topological_navigation/config/template_node_2.yaml: -------------------------------------------------------------------------------- 1 | meta: 2 | map: map_2d 3 | node: NodeName 4 | pointset: PointSet 5 | node: 6 | edges: [] 7 | localise_by_topic: '' 8 | name: NodeName 9 | parent_frame: map 10 | pose: 11 | orientation: 12 | w: 1.0 13 | x: 0.0 14 | y: 0.0 15 | z: 0.0 16 | position: 17 | x: 0.0 18 | y: 0.0 19 | z: 0.0 20 | properties: 21 | xy_goal_tolerance: 0.3 22 | yaw_goal_tolerance: 6.29 23 | restrictions_planning: 'True' 24 | restrictions_runtime: 'True' 25 | verts: 26 | - x: 0.689999997616 27 | y: 0.287000000477 28 | - x: 0.287000000477 29 | y: 0.490000009537 30 | - x: -0.287000000477 31 | y: 0.490000009537 32 | - x: -0.689999997616 33 | y: 0.287000000477 34 | - x: -0.689999997616 35 | y: -0.287000000477 36 | - x: -0.287000000477 37 | y: -0.490000009537 38 | - x: 0.287000000477 39 | y: -0.490000009537 40 | - x: 0.689999997616 41 | y: -0.287000000477 -------------------------------------------------------------------------------- /topological_navigation/config/tmap-schema.yaml: -------------------------------------------------------------------------------- 1 | $schema: http://json-schema.org/draft-07/schema# 2 | title: Topological Map Schema 3 | type: object 4 | required: 5 | - meta 6 | - metric_map 7 | - name 8 | - nodes 9 | - pointset 10 | additionalProperties: true 11 | properties: 12 | meta: 13 | type: object 14 | properties: 15 | last_updated: 16 | type: string 17 | pattern: ^[0-9]{4}-[0-9]{2}-[0-9]{2}_[0-9]{2}-[0-9]{2}-[0-9]{2}$ 18 | required: 19 | - last_updated 20 | additionalProperties: true 21 | metric_map: 22 | type: string 23 | name: 24 | type: string 25 | nodes: 26 | type: array 27 | items: 28 | type: object 29 | required: 30 | - meta 31 | - node 32 | properties: 33 | meta: 34 | type: object 35 | required: 36 | - map 37 | - node 38 | - pointset 39 | properties: 40 | map: 41 | type: string 42 | node: 43 | type: string 44 | pointset: 45 | type: string 46 | node: 47 | type: object 48 | required: 49 | - edges 50 | - name 51 | - parent_frame 52 | additionalProperties: true 53 | properties: 54 | edges: 55 | type: array 56 | items: 57 | type: object 58 | additionalProperties: true 59 | required: 60 | - edge_id 61 | - node 62 | properties: 63 | edge_id: 64 | type: string 65 | node: 66 | type: string 67 | name: 68 | type: string 69 | parent_frame: 70 | type: string 71 | pointset: 72 | type: string 73 | -------------------------------------------------------------------------------- /topological_navigation/launch/manual_topomapping.launch.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Author: Ibrahim Hroob 2024 3 | ''' 4 | import os 5 | from launch import LaunchDescription 6 | from launch.actions import DeclareLaunchArgument 7 | from launch.substitutions import LaunchConfiguration 8 | from launch_ros.actions import Node 9 | 10 | def generate_launch_description(): 11 | return LaunchDescription([ 12 | # Declare the launch arguments 13 | DeclareLaunchArgument('tmap', default_value='test.yaml'), 14 | DeclareLaunchArgument('tmap_dir', default_value='/home/ros/tmap'), 15 | DeclareLaunchArgument('node_thresh', default_value='0.5'), 16 | DeclareLaunchArgument('lock_btn', default_value='6'), 17 | DeclareLaunchArgument('add_btn', default_value='1'), 18 | DeclareLaunchArgument('remove_btn', default_value='2'), 19 | DeclareLaunchArgument('gen_map_btn', default_value='3'), 20 | DeclareLaunchArgument('topic_joy', default_value='/joy'), 21 | DeclareLaunchArgument('topic_pose', default_value='/gps_base/odometry'), 22 | 23 | # Launch the manual_topomapping node 24 | Node( 25 | package='topological_navigation', 26 | executable='manual_topomapping.py', 27 | name='manual_topomapping', 28 | output='screen', 29 | parameters=[{ 30 | 'tmap' : LaunchConfiguration('tmap'), 31 | 'tmap_dir' : LaunchConfiguration('tmap_dir'), 32 | 'site_name' : LaunchConfiguration('tmap'), 33 | 'node_thresh': LaunchConfiguration('node_thresh'), 34 | 'lock_btn' : LaunchConfiguration('lock_btn'), 35 | 'add_btn' : LaunchConfiguration('add_btn'), 36 | 'remove_btn' : LaunchConfiguration('remove_btn'), 37 | 'gen_map_btn': LaunchConfiguration('gen_map_btn'), 38 | 'topic_joy' : LaunchConfiguration('topic_joy'), 39 | 'topic_pose' : LaunchConfiguration('topic_pose'), 40 | }] 41 | ), 42 | ]) 43 | -------------------------------------------------------------------------------- /topological_navigation/launch/minimal_topological_navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /topological_navigation/launch/reconf_at_edges_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /topological_navigation/launch/topo_nav_global.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /topological_navigation/launch/topo_nav_local.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /topological_navigation/launch/topological_map_visualise.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /topological_navigation/launch/topological_navigation_empty_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /topological_navigation/launch/topological_restrictions_multirobot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /topological_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | topological_navigation 5 | 3.0.5 6 | The ros2 topological_navigation package 7 | 8 | 9 | Marc Hanheide 10 | James R Heselden 11 | 12 | Jaime Pulido Fentanes 13 | Adam Binch 14 | Francesco Del Duchetto 15 | 16 | Apache License 2.0 17 | 18 | ament_copyright 19 | ament_flake8 20 | ament_pep257 21 | python3-pytest 22 | launch_pytest 23 | 24 | ament_cmake 25 | 26 | geometry_msgs 27 | std_msgs 28 | nav_msgs 29 | sensor_msgs 30 | actionlib_msgs 31 | visualization_msgs 32 | topological_navigation_msgs 33 | 34 | tf_transformations 35 | launch_pytest 36 | 37 | 38 | ament_python 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /topological_navigation/resource/topological_navigation: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_navigation/resource/topological_navigation -------------------------------------------------------------------------------- /topological_navigation/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/topological_navigation 3 | [install] 4 | install_scripts=$base/lib/topological_navigation 5 | -------------------------------------------------------------------------------- /topological_navigation/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages 2 | from setuptools import setup 3 | from glob import glob 4 | 5 | package_name = 'topological_navigation' 6 | 7 | setup( 8 | name=package_name, 9 | version='3.0.5', 10 | packages=find_packages(), 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | ('share/' + package_name + '/config/', glob('config/*', recursive=True)), 16 | ('share/' + package_name + '/launch/', glob('launch/*', recursive=True)) 17 | ], 18 | install_requires=['setuptools'], 19 | zip_safe=True, 20 | maintainer='Adam Binch', 21 | maintainer_email='abinch@sagarobotics.com', 22 | description='The ros2 topological_navigation package', 23 | license='MIT', 24 | tests_require=['pytest', 'launch-pytest'], 25 | entry_points={ 26 | 'console_scripts': [ 27 | 'get_simple_policy2.py = topological_navigation.scripts.get_simple_policy2:main', 28 | 'localisation2.py = topological_navigation.scripts.localisation2:main', 29 | 'manual_edge_predictions.py = topological_navigation.scripts.manual_edge_predictions:main', 30 | 'map_manager2.py = topological_navigation.scripts.map_manager2:main', 31 | 'map_manager.py = topological_navigation.scripts.map_manager:main', 32 | 'map_publisher.py = topological_navigation.scripts.map_publisher:main', 33 | 'mean_based_prediction.py = topological_navigation.scripts.mean_based_prediction:main', 34 | 'nav_client.py = topological_navigation.scripts.nav_client:main', 35 | 'navigation.py = topological_navigation.scripts.navigation:main', 36 | 'navigation2.py = topological_navigation.scripts.navigation2:main', 37 | 'navstats_logger.py = topological_navigation.scripts.navstats_logger:main', 38 | 'reconf_at_edges_server.py = topological_navigation.scripts.reconf_at_edges_server:main', 39 | 'restrictions_manager.py = topological_navigation.scripts.restrictions_manager:main', 40 | 'search_route.py = topological_navigation.scripts.search_route:main', 41 | 'speed_based_prediction.py = topological_navigation.scripts.speed_based_prediction:main', 42 | 'test_top_pred.py = topological_navigation.scripts.test_top_pred:main', 43 | 'topological_prediction.py = topological_navigation.scripts.topological_prediction:main', 44 | 'topological_transform_publisher.py = topological_navigation.scripts.topological_transform_publisher:main', 45 | 'travel_time_estimator.py = topological_navigation.scripts.travel_time_estimator:main', 46 | 'visualise_map2.py = topological_navigation.scripts.visualise_map2:main', 47 | 'visualise_map.py = topological_navigation.scripts.visualise_map:main', 48 | 'visualise_map_ros2.py = topological_navigation.scripts.visualise_map_ros2:main', 49 | 'topomap_marker.py = topological_navigation.topomap_marker:main', 50 | 'topomap_marker2.py = topological_navigation.topomap_marker2:main', 51 | 'policy_marker.py = topological_navigation.policy_marker:main', 52 | 'manual_topomapping.py = topological_navigation.scripts.manual_topomapping:main' 53 | ], 54 | }, 55 | 56 | ) 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /topological_navigation/test/static_transformer.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from geometry_msgs.msg import TransformStamped 3 | import tf2_ros 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | node = rclpy.create_node('static_transform_publisher') 9 | 10 | broadcaster = tf2_ros.StaticTransformBroadcaster(node) 11 | 12 | static_transform_stamped = TransformStamped() 13 | static_transform_stamped.header.frame_id = 'topo_map' 14 | static_transform_stamped.child_frame_id = 'base_link' 15 | 16 | static_transform_stamped.transform.translation.x = 0.0 17 | static_transform_stamped.transform.translation.y = 0.0 18 | static_transform_stamped.transform.translation.z = 0.0 19 | 20 | static_transform_stamped.transform.rotation.x = 0.0 21 | static_transform_stamped.transform.rotation.y = 0.0 22 | static_transform_stamped.transform.rotation.z = 0.0 23 | static_transform_stamped.transform.rotation.w = 1.0 24 | 25 | # Publish the static transform 26 | broadcaster.sendTransform(static_transform_stamped) 27 | 28 | rclpy.spin(node) 29 | rclpy.shutdown() 30 | 31 | if __name__ == '__main__': 32 | main() -------------------------------------------------------------------------------- /topological_navigation/tests/conf/scenario_server.yaml: -------------------------------------------------------------------------------- 1 | robot_start_node: Start 2 | robot_goal_node: End 3 | obstacle_node_prefix: Obstacle 4 | obstacle_types: 5 | - chair 6 | - officechair 7 | - wheelchair 8 | - statichuman 9 | success_metrics: 10 | nav_timeout: 60.0 # 0 for inf 11 | -------------------------------------------------------------------------------- /topological_navigation/tests/conf/test.tmap2: -------------------------------------------------------------------------------- 1 | meta: 2 | last_updated: 2022-03-02_10-07-31 3 | metric_map: map_2d 4 | name: new_map 5 | nodes: 6 | - meta: 7 | map: map_2d 8 | node: WayPoint1 9 | pointset: new_map 10 | node: 11 | edges: 12 | - action: NavigateToPose 13 | action_type: move_base_msgs/MoveBaseGoal 14 | config: &id001 [] 15 | edge_id: WayPoint1_WayPoint2 16 | fail_policy: fail 17 | fluid_navigation: true 18 | goal: 19 | target_pose: 20 | header: 21 | frame_id: $node.parent_frame 22 | pose: $node.pose 23 | node: WayPoint2 24 | recovery_behaviours_config: '' 25 | restrictions_planning: 'True' 26 | restrictions_runtime: 'True' 27 | localise_by_topic: '' 28 | name: WayPoint1 29 | parent_frame: map 30 | pose: 31 | orientation: 32 | w: 1.0 33 | x: 0.0 34 | y: 0.0 35 | z: 0.0 36 | position: 37 | x: 0.0 38 | y: 0.0 39 | z: 0.0 40 | properties: 41 | xy_goal_tolerance: 0.3 42 | yaw_goal_tolerance: 0.1 43 | restrictions_planning: 'True' 44 | restrictions_runtime: 'True' 45 | verts: 46 | - x: 0.692909649383465 47 | y: 0.2870125742738173 48 | - x: 0.2870125742738174 49 | y: 0.692909649383465 50 | - x: -0.2870125742738173 51 | y: 0.692909649383465 52 | - x: -0.692909649383465 53 | y: 0.28701257427381743 54 | - x: -0.6929096493834651 55 | y: -0.28701257427381727 56 | - x: -0.28701257427381777 57 | y: -0.6929096493834649 58 | - x: 0.2870125742738169 59 | y: -0.6929096493834652 60 | - x: 0.6929096493834649 61 | y: -0.28701257427381777 62 | - meta: 63 | map: map_2d 64 | node: WayPoint2 65 | pointset: new_map 66 | node: 67 | edges: 68 | - action: NavigateToPose 69 | action_type: move_base_msgs/MoveBaseGoal 70 | config: *id001 71 | edge_id: WayPoint2_WayPoint1 72 | fail_policy: fail 73 | fluid_navigation: true 74 | goal: 75 | target_pose: 76 | header: 77 | frame_id: $node.parent_frame 78 | pose: $node.pose 79 | node: WayPoint1 80 | recovery_behaviours_config: '' 81 | restrictions_planning: 'True' 82 | restrictions_runtime: 'True' 83 | localise_by_topic: '' 84 | name: WayPoint2 85 | parent_frame: map 86 | pose: 87 | orientation: 88 | w: 1.0 89 | x: 0.0 90 | y: 0.0 91 | z: 0.0 92 | position: 93 | x: 10.0 94 | y: 10.0 95 | z: 0.0 96 | properties: 97 | xy_goal_tolerance: 0.3 98 | yaw_goal_tolerance: 0.1 99 | restrictions_planning: 'True' 100 | restrictions_runtime: 'True' 101 | verts: 102 | - x: 0.692909649383465 103 | y: 0.2870125742738173 104 | - x: 0.2870125742738174 105 | y: 0.692909649383465 106 | - x: -0.2870125742738173 107 | y: 0.692909649383465 108 | - x: -0.692909649383465 109 | y: 0.28701257427381743 110 | - x: -0.6929096493834651 111 | y: -0.28701257427381727 112 | - x: -0.28701257427381777 113 | y: -0.6929096493834649 114 | - x: 0.2870125742738169 115 | y: -0.6929096493834652 116 | - x: 0.6929096493834649 117 | y: -0.28701257427381777 118 | pointset: new_map 119 | transformation: 120 | child: topo_map 121 | parent: map 122 | rotation: 123 | w: 1.0 124 | x: 0.0 125 | y: 0.0 126 | z: 0.0 127 | translation: 128 | x: 0.0 129 | y: 0.0 130 | z: 0.0 131 | -------------------------------------------------------------------------------- /topological_navigation/tests/map_manager.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /topological_navigation/tests/map_manager_tester.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Created on Fri Jan 22 10:34:58 2021 4 | 5 | @author: Adam Binch (abinch@sagarobotics.com) 6 | """ 7 | ######################################################################################################### 8 | import unittest, rostest 9 | import rospy, json 10 | 11 | from std_msgs.msg import String 12 | from topological_navigation_msgs.srv import GetEdgesBetweenNodes, GetEdgesBetweenNodesRequest 13 | 14 | PKG = "topological_navigation" 15 | 16 | 17 | class MapManagerTester(unittest.TestCase): 18 | 19 | def __init__(self, *args): 20 | super(MapManagerTester, self).__init__(*args) 21 | rospy.init_node('map_manager_tester') 22 | 23 | self.map_received = False 24 | rospy.Subscriber("/topological_map_2", String, self.map_callback) 25 | 26 | 27 | def map_callback(self, msg): 28 | self.tmap2 = json.loads(msg.data) 29 | self.map_received = True 30 | 31 | 32 | def test_map_manager(self): 33 | 34 | rospy.sleep(5.0) 35 | self.assertTrue(self.map_received) 36 | 37 | try: 38 | s = rospy.ServiceProxy("/topological_map_manager2/get_edges_between_nodes", GetEdgesBetweenNodes) 39 | s.wait_for_service(timeout=5.0) 40 | 41 | req = GetEdgesBetweenNodesRequest() 42 | req.nodea = "WayPoint1" 43 | req.nodeb = "WayPoint2" 44 | res = s(req) 45 | 46 | self.assertGreater(len(res.ids_a_to_b), 0) 47 | self.assertGreater(len(res.ids_b_to_a), 0) 48 | 49 | self.assertEqual(res.ids_a_to_b[0], "WayPoint1_WayPoint2") 50 | self.assertEqual(res.ids_b_to_a[0], "WayPoint2_WayPoint1") 51 | 52 | except (rospy.ServiceException, rospy.ROSInterruptException) as e: 53 | rospy.logfatal(e) 54 | self.assertTrue(False) 55 | ######################################################################################################### 56 | 57 | 58 | ######################################################################################################### 59 | if __name__ == "__main__": 60 | rostest.rosrun(PKG, "map_manager_tester", MapManagerTester) 61 | ######################################################################################################### -------------------------------------------------------------------------------- /topological_navigation/tests/navigation_scenarios.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /topological_navigation/tests/navigation_test_joypad_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | from scitos_teleop.msg import action_buttons 6 | from topological_navigation_msgs.srv import RunTopoNavTestScenario 7 | from std_srvs.srv import Empty 8 | 9 | 10 | class JoyPadControl(object): 11 | _start = "start" 12 | _reset = "reset" 13 | 14 | def __init__(self, name): 15 | rospy.loginfo("Starting %s ..." % name) 16 | self.action = self._start 17 | rospy.Subscriber("/teleop_joystick/action_buttons", action_buttons, self.callback, queue_size=1) 18 | rospy.loginfo("... done") 19 | 20 | def callback(self, msg): 21 | if msg.A: 22 | self.action = self._start if self.action == self._reset else self._reset 23 | rospy.loginfo("+++ Current action '%s'. Press 'B' to confirm or 'A' to toggle. +++" % self.action) 24 | elif msg.B: 25 | if self.action == self._start: 26 | try: 27 | s = rospy.ServiceProxy("/scenario_server/start", RunTopoNavTestScenario) 28 | s.wait_for_service() 29 | rospy.loginfo(s()) 30 | except (rospy.ServiceException, rospy.ROSInterruptException) as e: 31 | rospy.logwarn(e) 32 | elif self.action == self._reset: 33 | try: 34 | s = rospy.ServiceProxy("/scenario_server/reset", Empty) 35 | s.wait_for_service() 36 | s() 37 | except (rospy.ServiceException, rospy.ROSInterruptException) as e: 38 | rospy.logwarn(e) 39 | 40 | 41 | if __name__ == "__main__": 42 | rospy.init_node("navigation_test_joypad_control") 43 | JoyPadControl(rospy.get_name()) 44 | rospy.spin() 45 | -------------------------------------------------------------------------------- /topological_navigation/tests/topological_navigation_tester_critical.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import unittest 4 | import rospy 5 | from topological_navigation_msgs.srv import LoadTopoNavTestScenario, RunTopoNavTestScenario 6 | 7 | 8 | PKG = 'topological_navigation' 9 | 10 | 11 | class TestTopologicalNavigation(unittest.TestCase): 12 | _map_name = 'mb_test' 13 | 14 | def __init__(self, *args): 15 | super(self.__class__, self).__init__(*args) 16 | rospy.init_node('topological_navigation_tester') 17 | 18 | def _run(self, map_name): 19 | try: 20 | l = rospy.ServiceProxy("/scenario_server/load", LoadTopoNavTestScenario) 21 | l.wait_for_service(timeout=60.) 22 | l(map_name) 23 | s = rospy.ServiceProxy("/scenario_server/start", RunTopoNavTestScenario) 24 | s.wait_for_service(timeout=60.) 25 | res = s() 26 | except (rospy.ServiceException, rospy.ROSInterruptException) as e: 27 | rospy.logfatal(e) 28 | self.assertTrue(False) 29 | return res 30 | 31 | def test_static_l_shaped_corridor_2m(self): 32 | res = self._run(self._map_name+str(0)) 33 | rospy.loginfo(res) 34 | self.assertTrue(res.nav_success) 35 | 36 | 37 | if __name__ == '__main__': 38 | import rostest 39 | 40 | rostest.rosrun(PKG, 'topological_navigation_tester', TestTopologicalNavigation) 41 | 42 | -------------------------------------------------------------------------------- /topological_navigation/tests/travel_time_estimator.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /topological_navigation/tests/travel_time_tester.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import unittest 5 | import topological_navigation.testing 6 | import rospy 7 | import math 8 | from mongodb_store.message_store import MessageStoreProxy 9 | from topological_navigation.publisher import map_publisher 10 | from topological_navigation.publisher import map_publisher 11 | from topological_navigation_msgs.srv import EstimateTravelTime 12 | 13 | 14 | class TestTravelTimeEstimator(unittest.TestCase): 15 | # class TestTravelTimeEstimator(): 16 | def __init__(self, *args): 17 | super(TestTravelTimeEstimator, self).__init__(*args) 18 | rospy.init_node('test_travel_time_estimator') 19 | 20 | def test_travel_time_estimator(self): 21 | 22 | # create a test topological map 23 | width = 5 24 | height = 5 25 | nodeSeparation = 10.0 26 | 27 | test_nodes = topological_navigation.testing.create_cross_map(width = width, height = height, nodeSeparation = nodeSeparation) 28 | self.assertEqual(len(test_nodes), width + height - 1) 29 | startIndex = -(width/2) 30 | endIndex = (width/2) 31 | startNode = test_nodes['h_%s' % startIndex] 32 | self.assertIsNotNone(startNode) 33 | endNode = test_nodes['h_%s' % endIndex] 34 | self.assertIsNotNone(endNode) 35 | 36 | # locally check distance 37 | dist = math.hypot((startNode.pose.position.x-endNode.pose.position.x),(startNode.pose.position.y-endNode.pose.position.y)) 38 | self.assertEqual(dist, (width - 1) * nodeSeparation ) 39 | 40 | # now insert the map into the database 41 | msg_store = MessageStoreProxy(collection='topological_maps') 42 | 43 | meta = {} 44 | meta['map'] = 'test_travel_time_estimator_map' 45 | meta['pointset'] = 'test_travel_time_estimator_map' 46 | 47 | for (nodeName, node) in test_nodes.items(): 48 | meta["node"] = nodeName 49 | node.map = meta['map'] 50 | node.pointset = meta['pointset'] 51 | msg_store.insert(node,meta) 52 | 53 | # and publish the map 54 | ps = map_publisher('test_travel_time_estimator_map') 55 | 56 | # now wait for the distance service 57 | time_srv_name = 'topological_navigation/travel_time_estimator' 58 | rospy.wait_for_service(time_srv_name, timeout=10) 59 | time_srv = rospy.ServiceProxy(time_srv_name, EstimateTravelTime) 60 | time_estimate = time_srv(startNode.name, endNode.name).travel_time 61 | # the time should be (at least for now) at least as long as the straight-line distance at 1m/s 62 | self.assertGreaterEqual(time_estimate.to_sec(), (width - 1) * nodeSeparation) 63 | 64 | 65 | if __name__ == '__main__': 66 | import rostest 67 | PKG = 'topological_navigation' 68 | rostest.rosrun(PKG, 'test_travel_time_estimator', TestTravelTimeEstimator) 69 | 70 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_navigation/topological_navigation/__init__.py -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/navigation_stats.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from datetime import datetime 4 | 5 | class nav_stats(object): 6 | 7 | def __init__(self, origin, target, topol_map, edge_id): 8 | self.status= "active" 9 | self.origin=origin 10 | self.target=target 11 | self.topological_map = topol_map 12 | self.edge_id = edge_id 13 | #self.date_at_node = datetime.now() 14 | self.set_start() 15 | 16 | def set_start(self): 17 | self.date_started = datetime.now() 18 | self.date_at_node = self.date_started 19 | 20 | def set_ended(self, node): 21 | self.final_node=node 22 | self.date_finished = datetime.now() 23 | self.get_operation_time() 24 | self.get_time_to_wp() 25 | 26 | def set_at_node(self): 27 | self.date_at_node = datetime.now() 28 | 29 | def get_operation_time(self): 30 | operation_delta = self.date_finished - self.date_started 31 | self.operation_time = operation_delta.total_seconds() 32 | return self.operation_time 33 | 34 | def get_time_to_wp(self): 35 | if self.date_at_node != self.date_started : 36 | operation_delta = self.date_finished - self.date_at_node 37 | self.time_to_wp = operation_delta.total_seconds() 38 | else : 39 | self.time_to_wp = 0 40 | return self.time_to_wp 41 | 42 | def get_start_time_str(self): 43 | dt_text=self.date_started.strftime('%A, %B %d %Y, at %H:%M:%S hours') 44 | return dt_text 45 | 46 | def get_finish_time_str(self): 47 | dt_text=self.date_finished.strftime('%A, %B %d %Y, at %H:%M:%S hours') 48 | return dt_text -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/point2line.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ################################################################################################################### 3 | from __future__ import division 4 | import numpy as np 5 | 6 | 7 | def pnt2line(pnt, start, end): 8 | 9 | pnt = (pnt[:, 0], pnt[:, 1], pnt[:, 2]) 10 | start = (start[:, 0], start[:, 1], start[:, 2]) 11 | end = (end[:, 0], end[:, 1], end[:, 2]) 12 | 13 | line_vec = vector(start, end) 14 | pnt_vec = vector(start, pnt) 15 | line_unitvec = unit(line_vec) 16 | line_len = length(line_vec) 17 | pnt_vec_scaled = scale(pnt_vec, 1.0/line_len) 18 | 19 | t = dot(line_unitvec, pnt_vec_scaled) 20 | t[np.where(t < 0.0)[0]] = 0.0 21 | t[np.where(t > 1.0)[0]] = 1.0 22 | 23 | nearest = scale(line_vec, t) 24 | return distance(nearest, pnt_vec) 25 | 26 | 27 | def vector(b, e): 28 | return b[0]-e[0], b[1]-e[1], b[2]-e[2] 29 | 30 | 31 | def unit(v): 32 | mag = length(v) 33 | return v[0]/mag, v[1]/mag, v[2]/mag 34 | 35 | 36 | def length(v): 37 | return np.sqrt(v[0]**2 + v[1]**2 + v[2]**2) 38 | 39 | 40 | def scale(v, sc): 41 | return v[0]*sc, v[1]*sc, v[2]*sc 42 | 43 | 44 | def dot(v, w): 45 | return v[0]*w[0] + v[1]*w[1] + v[2]*w[2] 46 | 47 | 48 | def distance(p0, p1): 49 | return length(vector(p0, p1)) 50 | ################################################################################################################### 51 | 52 | 53 | ################################################################################################################### 54 | # pnt = [[x0, y0, z0], 55 | # [x1, y1, z1], 56 | # [..., ..., ...], 57 | # [xn, yn, zn]] 58 | 59 | if __name__ == '__main__': 60 | 61 | pnt = np.array([[5, 4, 0], 62 | [5, 4, 0]]) 63 | 64 | start = np.array([[3, 0, 0], 65 | [2, 3, 0]]) 66 | 67 | end = np.array([[8, 5, 0], 68 | [4, 7, 0]]) 69 | 70 | print(pnt2line(pnt, start, end)) 71 | ################################################################################################################### -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | import rospy 4 | import sys 5 | 6 | import std_msgs.msg 7 | from topological_navigation.topological_node import * 8 | from topological_navigation_msgs.msg import TopologicalNode 9 | from topological_navigation_msgs.msg import TopologicalMap 10 | from topological_navigation_msgs.srv import GetTopologicalMap 11 | 12 | from mongodb_store.message_store import MessageStoreProxy 13 | 14 | 15 | class map_publisher(object): 16 | 17 | def __init__(self, name) : 18 | self.name = name 19 | self.nodes = self.loadMap(name) 20 | self.map_pub = rospy.Publisher('/topological_map', TopologicalMap, latch=True, queue_size=1) 21 | self.last_updated = rospy.Time.now() 22 | self.map_pub.publish(self.nodes) 23 | rospy.Subscriber('/update_map', std_msgs.msg.Time, self.updateCallback) 24 | self.get_map_srv=rospy.Service('/topological_map_publisher/get_topological_map', GetTopologicalMap, self.get_topological_map_cb) 25 | 26 | 27 | 28 | def updateCallback(self, msg) : 29 | # if msg.data > self.last_updated : 30 | self.nodes = self.loadMap(self.name) 31 | self.last_updated = rospy.Time.now() 32 | self.map_pub.publish(self.nodes) 33 | 34 | 35 | def get_topological_map_cb(self, req): 36 | nodes = self.loadMap(req.pointset) 37 | print("Returning Map %s"%req.pointset) 38 | return nodes 39 | 40 | 41 | def loadMap(self, point_set) : 42 | msg_store = MessageStoreProxy(collection='topological_maps') 43 | 44 | query_meta = {} 45 | query_meta["pointset"] = point_set 46 | 47 | # waiting for the map to be in the mongodb_store 48 | ntries=1 49 | map_found=False 50 | 51 | while not map_found : 52 | available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 53 | #print available 54 | if available <= 0 : 55 | rospy.logerr("Desired pointset '"+point_set+"' not in datacentre, try :"+str(ntries)) 56 | #rospy.logerr("Available pointsets: "+str(available)) 57 | if ntries <=10 : 58 | ntries+=1 59 | rospy.sleep(rospy.Duration.from_sec(6)) 60 | else : 61 | raise Exception("Can't find waypoints.") 62 | sys.exit(2) 63 | else: 64 | map_found=True 65 | 66 | 67 | query_meta = {} 68 | query_meta["pointset"] = point_set 69 | 70 | message_list = msg_store.query(TopologicalNode._type, {}, query_meta) 71 | 72 | points = TopologicalMap() 73 | points.name = point_set 74 | points.map = point_set 75 | points.pointset = point_set 76 | #string last_updated 77 | for i in message_list: 78 | b = i[0] 79 | points.nodes.append(b) 80 | 81 | return points -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_navigation/topological_navigation/scripts/__init__.py -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/evaluate_top_pred.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | from topological_navigation_msgs.srv import * 6 | 7 | 8 | def predict_edges(seconds_from_now): 9 | rospy.wait_for_service('topological_prediction/predict_edges') 10 | try: 11 | get_prediction = rospy.ServiceProxy('topological_prediction/predict_edges', topological_navigation_msgs.srv.PredictEdgeState) 12 | now = rospy.Time.now() 13 | prediction_time = now + rospy.Duration(seconds_from_now) 14 | print("Requesting prediction for %f"%prediction_time.secs) 15 | resp1 = get_prediction(prediction_time) 16 | return resp1 17 | except rospy.ServiceException as e: 18 | print("Service call failed: %s"%e) 19 | 20 | def usage(): 21 | return "%s seconds_from_now"%sys.argv[0] 22 | 23 | if __name__ == "__main__": 24 | if len(sys.argv) == 2: 25 | seconds_from_now = int(sys.argv[1]) 26 | else: 27 | print(usage()) 28 | sys.exit(1) 29 | rospy.init_node('topological_prediction_test') 30 | est = predict_edges(seconds_from_now) 31 | print("set of repeated edges:") 32 | print(set([x for x in est.edge_ids if est.edge_ids.count(x) > 1])) 33 | #print est.edge_id 34 | 35 | if len(est.edge_ids) == len(est.probs) and len(est.edge_ids) == len(est.durations): 36 | print("Good Answer!!! %d" %len(est.probs)) 37 | for i in range(len(est.edge_ids)): 38 | print(est.edge_ids[i], est.probs[i], est.durations[i].secs) 39 | else: 40 | print("something failed") 41 | print(len(est.edge_ids), len(est.probs), len(est.durations)) 42 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/get_simple_policy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | from std_msgs.msg import String 6 | 7 | import topological_navigation_msgs.srv 8 | from topological_navigation_msgs.msg import TopologicalMap 9 | import topological_navigation.route_search 10 | #from topological_navigation.route_search import * 11 | 12 | 13 | class SearchPolicyServer(object): 14 | 15 | def __init__(self) : 16 | 17 | #Waiting for Topological Map 18 | self._map_received=False 19 | rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) 20 | rospy.loginfo("Waiting for Topological map ...") 21 | while not self._map_received : 22 | rospy.sleep(rospy.Duration.from_sec(0.05)) 23 | rospy.loginfo(" ...done") 24 | 25 | self._top_loc=False 26 | rospy.loginfo("Waiting for Topological localisation ...") 27 | rospy.Subscriber('closest_node', String, self.closestNodeCallback) 28 | while not self._top_loc : 29 | rospy.sleep(rospy.Duration.from_sec(0.05)) 30 | rospy.loginfo(" ...done") 31 | 32 | 33 | #This service returns any given map 34 | self.get_map_srv=rospy.Service('get_simple_policy/get_route_to', topological_navigation_msgs.srv.GetRouteTo, self.get_route_cb) 35 | self.get_map_srv=rospy.Service('get_simple_policy/get_route_between', topological_navigation_msgs.srv.GetRouteBetween, self.get_routeb_cb) 36 | rospy.loginfo("All Done ...") 37 | rospy.spin() 38 | 39 | 40 | def get_route_cb(self, req): 41 | rsearch = topological_navigation.route_search.TopologicalRouteSearch(self.top_map) 42 | route = rsearch.search_route(self.closest_node, req.goal) 43 | print(route) 44 | return route 45 | 46 | def get_routeb_cb(self, req): 47 | rsearch = topological_navigation.route_search.TopologicalRouteSearch(self.top_map) 48 | route = rsearch.search_route(req.origin, req.goal) 49 | print(route) 50 | return route 51 | 52 | 53 | """ 54 | Update Map CallBack 55 | 56 | This Function updates the Topological Map everytime it is called 57 | """ 58 | def MapCallback(self, msg) : 59 | self.top_map = msg 60 | self._map_received=True 61 | 62 | 63 | """ 64 | Closest Node CallBack 65 | 66 | """ 67 | def closestNodeCallback(self, msg): 68 | self.closest_node=msg.data 69 | self._top_loc=True 70 | 71 | 72 | 73 | 74 | 75 | if __name__ == '__main__': 76 | rospy.init_node('route_search') 77 | searcher = SearchPolicyServer() 78 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/get_simple_policy2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Created on Tue Nov 17 22:02:24 2023 5 | @author: Geesara Kulathunga (ggeesara@gmail.com) 6 | 7 | """ 8 | 9 | import sys 10 | import rclpy, json 11 | from std_msgs.msg import String 12 | from topological_navigation_msgs.srv import GetRouteTo, GetRouteBetween 13 | from topological_navigation.route_search2 import RouteChecker, TopologicalRouteSearch2, get_route_distance 14 | from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy 15 | from rclpy.executors import MultiThreadedExecutor 16 | 17 | class SearchPolicyServer(rclpy.node.Node): 18 | 19 | def __init__(self, name): 20 | super().__init__(name) 21 | self._map_received = False 22 | self.qos = QoSProfile(depth=1, 23 | reliability=ReliabilityPolicy.RELIABLE, 24 | history=HistoryPolicy.KEEP_LAST, 25 | durability=DurabilityPolicy.TRANSIENT_LOCAL) 26 | self.mapper_sub = self.create_subscription(String, '/topological_map_2', self.map_callback, qos_profile=self.qos) 27 | self.get_logger().info("Waiting for Topological map ...") 28 | while rclpy.ok(): 29 | rclpy.spin_once(self) 30 | if self._map_received: 31 | self.get_logger().info("Navigation received the Topological Map") 32 | break 33 | self._top_loc=False 34 | self.get_logger().info("Waiting for Topological localisation ...") 35 | self.closest_node_sub = self.create_subscription(String, 'closest_node', self.closest_node_callback, qos_profile=self.qos) 36 | while rclpy.ok(): 37 | rclpy.spin_once(self) 38 | if self._top_loc: 39 | self.get_logger().info("Localisation model is up...") 40 | break 41 | 42 | self.service_to_get_goal = self.create_service(GetRouteTo, 'get_simple_policy/get_route_to', self.get_route_cb) 43 | self.service_to_get_path = self.create_service(GetRouteBetween, 'get_simple_policy/get_route_between', self.get_routeb_cb) 44 | self.get_logger().info("All Done ...") 45 | 46 | 47 | def get_route_cb(self, req, res): 48 | route = self.rsearch.search_route(self.closest_node, req.goal) 49 | print(route) 50 | res.route = route 51 | return res 52 | 53 | def get_routeb_cb(self, req, res): 54 | route = self.rsearch.search_route(req.origin, req.goal) 55 | print(route) 56 | res.route = route 57 | return res 58 | 59 | def map_callback(self, msg) : 60 | self.lnodes = json.loads(msg.data) 61 | self.topol_map = self.lnodes["pointset"] 62 | self.rsearch = TopologicalRouteSearch2(self.lnodes) 63 | self._map_received = True 64 | 65 | def closest_node_callback(self, msg): 66 | self.closest_node=msg.data 67 | self._top_loc=True 68 | 69 | def main(args=None): 70 | rclpy.init(args=args) 71 | node = SearchPolicyServer('simple_policy_server') 72 | executor = MultiThreadedExecutor() 73 | executor.add_node(node) 74 | try: 75 | executor.spin() 76 | except KeyboardInterrupt: 77 | node.get_logger().info('shutting down simple_policy_server node\n') 78 | node.destroy_node() 79 | rclpy.shutdown() 80 | 81 | 82 | if __name__ == '__main__' : 83 | main() 84 | 85 | 86 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/manual_edge_predictions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | import yaml 6 | 7 | import actionlib 8 | from actionlib_msgs.msg import * 9 | 10 | from topological_navigation_msgs.srv import * 11 | from topological_navigation_msgs.msg import * 12 | 13 | 14 | def usage(): 15 | print("Run with a yaml file containing edge prediction information:") 16 | print("\t rosrun topological_navigation manual_edge_predictions.py ") 17 | 18 | # Examaple yaml format: 19 | # - edge_id: WayPoint0_WayPoint1 20 | # duration: 1.0 21 | # success_probability: 1.0 22 | # - edge_id: WayPoint0_WayPoint4 23 | # duration: 1.0 24 | # success_probability: 1.0 25 | # - edge_id: WayPoint1_WayPoint2 26 | # duration: 1.0 27 | # success_probability: 1.0 28 | 29 | 30 | class YamlConfiguredPredictor(object): 31 | 32 | def __init__(self, config) : 33 | 34 | self.response = PredictEdgeStateResponse() 35 | 36 | for edge in config: 37 | self.response.edge_ids.append(edge['edge_id']) 38 | self.response.probs.append(edge['success_probability']) 39 | self.response.durations.append(rospy.Duration(edge['duration'])) 40 | 41 | name= rospy.get_name() 42 | action_name = name+'/build_temporal_model' 43 | 44 | #Creating Action Server 45 | rospy.loginfo("Creating action server.") 46 | self._as = actionlib.SimpleActionServer(action_name, BuildTopPredictionAction, execute_cb = self.build_callback, auto_start = False) 47 | self._as.start() 48 | 49 | 50 | self.predict_srv=rospy.Service('topological_prediction/predict_edges', PredictEdgeState, self.predict_edge_cb) 51 | rospy.loginfo("All Done ...") 52 | rospy.spin() 53 | 54 | 55 | def predict_edge_cb(self, req): 56 | return self.response 57 | 58 | 59 | """ 60 | Build predictions from data. Or not. 61 | 62 | Does nothing for this version. 63 | """ 64 | def build_callback(self, goal): 65 | self._as.set_succeeded() 66 | 67 | 68 | if __name__ == '__main__': 69 | rospy.init_node('topological_prediction') 70 | 71 | if len(sys.argv) != 2: 72 | usage() 73 | sys.exit(1) 74 | config_file = sys.argv[1] 75 | 76 | 77 | with open(config_file, "r") as f: 78 | config = yaml.load(f.read()) 79 | 80 | server = YamlConfiguredPredictor(config) 81 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/map_manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | import rospy 4 | import sys 5 | 6 | import std_msgs.msg 7 | from topological_navigation.topological_node import * 8 | from topological_navigation_msgs.msg import TopologicalNode 9 | from topological_navigation_msgs.msg import TopologicalMap 10 | 11 | from mongodb_store.message_store import MessageStoreProxy 12 | 13 | from topological_navigation.manager import map_manager 14 | 15 | 16 | def usage(): 17 | print("\nPublishes Topological Maps:") 18 | print("\nFor loading a map from the mongodb:") 19 | print("\t rosrun topological_navigation map_manager.py map_name") 20 | print("\nFor loading a map from a tmap file:") 21 | print("\t rosrun topological_navigation map_manager.py -f map_filename") 22 | print("\nFor creating a new map:") 23 | print("\t rosrun topological_navigation map_manager.py -n map_name") 24 | print("\n\n") 25 | 26 | 27 | 28 | if __name__ == '__main__' : 29 | load=True 30 | load_from_file = False 31 | if '-h' in sys.argv or '--help' in sys.argv or len(sys.argv) < 2 : 32 | usage() 33 | sys.exit(1) 34 | else: 35 | if '-n' in sys.argv: 36 | ind = sys.argv.index('-n') 37 | _map=sys.argv[ind+1] 38 | print("Creating new Map (%s)" %_map) 39 | load=False 40 | elif '-f' in sys.argv: 41 | ind = sys.argv.index('-f') 42 | _map=sys.argv[ind+1] 43 | print("Loading map from tmap file (%s)" %_map) 44 | load_from_file=True 45 | else: 46 | _map=sys.argv[1] 47 | 48 | rospy.init_node("topological_map_manager") 49 | ps = map_manager() 50 | ps.init_map(_map,load,load_from_file) 51 | rospy.spin() 52 | ######################################################################################################### -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/map_manager2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Created on Thu Nov 5 10:41:24 2020 4 | 5 | @author: Adam Binch (abinch@sagarobotics.com) 6 | """ 7 | 8 | import sys 9 | import rclpy 10 | 11 | from topological_navigation.manager2 import map_manager_2 12 | 13 | 14 | def usage(): 15 | print("\nPublishes Topological Maps:") 16 | print("\nFor loading a map:") 17 | print("\t rosrun topological_navigation map_manager2.py map_filename") 18 | print("\nFor creating a new map:") 19 | print("\t rosrun topological_navigation map_manager2.py -n map_filename") 20 | print("\n\n") 21 | 22 | 23 | def main(args=None): 24 | 25 | load=True 26 | if '-h' in sys.argv or '--help' in sys.argv or len(sys.argv) < 2: 27 | usage() 28 | sys.exit(1) 29 | else: 30 | if '-n' in sys.argv: 31 | ind = sys.argv.index('-n') 32 | _map=sys.argv[ind+1] 33 | print("Creating new Map (%s)" %_map) 34 | load=False 35 | else: 36 | _map=sys.argv[1] 37 | 38 | 39 | rclpy.init(args=args) 40 | node = map_manager_2() 41 | 42 | node.init_map(filename=_map, load=load) 43 | 44 | rclpy.spin(node) 45 | rclpy.shutdown() 46 | 47 | 48 | if __name__ == '__main__' : 49 | main() 50 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/map_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | import rospy 4 | import sys 5 | 6 | import std_msgs.msg 7 | from topological_navigation.topological_node import * 8 | from topological_navigation_msgs.msg import TopologicalNode 9 | from topological_navigation_msgs.msg import TopologicalMap 10 | 11 | from mongodb_store.message_store import MessageStoreProxy 12 | 13 | from topological_navigation.publisher import map_publisher 14 | 15 | 16 | if __name__ == '__main__' : 17 | point_set = sys.argv[1] 18 | rospy.init_node("topological_map_publisher") 19 | ps = map_publisher(point_set) 20 | rospy.spin() -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/nav_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import sys 5 | # Brings in the SimpleActionClient 6 | import actionlib 7 | import topological_navigation_msgs.msg 8 | 9 | 10 | class topol_nav_client(object): 11 | 12 | def __init__(self, targ) : 13 | 14 | rospy.on_shutdown(self._on_node_shutdown) 15 | self.client = actionlib.SimpleActionClient('topological_navigation', topological_navigation_msgs.msg.GotoNodeAction) 16 | 17 | self.client.wait_for_server() 18 | rospy.loginfo(" ... Init done") 19 | 20 | navgoal = topological_navigation_msgs.msg.GotoNodeGoal() 21 | 22 | print("Requesting Navigation to %s" %targ) 23 | 24 | navgoal.target = targ 25 | #navgoal.origin = orig 26 | 27 | # Sends the goal to the action server. 28 | self.client.send_goal(navgoal)#,self.done_cb, self.active_cb, self.feedback_cb) 29 | 30 | # Waits for the server to finish performing the action. 31 | self.client.wait_for_result() 32 | 33 | # Prints out the result of executing the action 34 | ps = self.client.get_result() # A FibonacciResult 35 | print(ps) 36 | 37 | def _on_node_shutdown(self): 38 | self.client.cancel_all_goals() 39 | #sleep(2) 40 | 41 | 42 | if __name__ == '__main__': 43 | print('Argument List:',str(sys.argv)) 44 | if len(sys.argv) < 2 : 45 | sys.exit(2) 46 | rospy.init_node('topol_nav_test') 47 | ps = topol_nav_client(sys.argv[1]) 48 | 49 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/navstats_logger.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | 4 | import calendar 5 | from datetime import datetime 6 | 7 | import rospy 8 | 9 | from topological_navigation_msgs.msg import NavStatistics 10 | from mongodb_store.message_store import MessageStoreProxy 11 | 12 | class TopologicalNavStatsSaver(object): 13 | """ 14 | Class for Topological Navigation stats logging 15 | 16 | """ 17 | def __init__(self) : 18 | rospy.Subscriber('topological_navigation/Statistics', NavStatistics, self.statsCallback) 19 | 20 | rospy.spin() 21 | 22 | def statsCallback(self, msg): 23 | #print "---------------------------------------------------" 24 | meta = {} 25 | meta["type"] = "Topological Navigation Stat" 26 | epoch = datetime.strptime(msg.date_at_node, '%A, %B %d %Y, at %H:%M:%S hours').timetuple()# .time() 27 | #print calendar.timegm(epoch) 28 | meta["epoch"] = calendar.timegm(epoch)#calendar.timegm(self.stat.date_at_node.timetuple()) 29 | meta["date"] = msg.date_at_node 30 | meta["pointset"] = msg.topological_map 31 | 32 | #print meta 33 | 34 | msg_store = MessageStoreProxy(collection='nav_stats') 35 | msg_store.insert(msg,meta) 36 | 37 | 38 | if __name__ == '__main__': 39 | mode="normal" 40 | rospy.init_node('topological_navstats_logger') 41 | server = TopologicalNavStatsSaver() -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/reconf_at_edges_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy, dynamic_reconfigure.client 3 | from topological_navigation_msgs.srv import ReconfAtEdges, ReconfAtEdgesResponse 4 | 5 | 6 | class reconf_at_edges_server(object): 7 | 8 | 9 | def __init__(self): 10 | self.edge_groups = rospy.get_param("/edge_nav_reconfig_groups", {}) 11 | rospy.Service('reconf_at_edges', ReconfAtEdges, self.handle_reconf_at_edges) 12 | 13 | 14 | def handle_reconf_at_edges(self, req): 15 | 16 | self.success = False 17 | 18 | for group in self.edge_groups: 19 | 20 | if req.edge_id in self.edge_groups[group]["edges"]: 21 | rospy.loginfo("\nSetting parameters for edge group {} ...".format(group)) 22 | 23 | self.set_parameters(group) 24 | break 25 | 26 | return ReconfAtEdgesResponse(self.success) 27 | 28 | 29 | def set_parameters(self, group): 30 | 31 | try: 32 | for param in self.edge_groups[group]["parameters"]: 33 | print("Setting {} = {}".format("/".join((param["ns"], param["name"])), param["value"])) 34 | 35 | rcnfclient = dynamic_reconfigure.client.Client(param["ns"], timeout=5.0) 36 | rcnfclient.update_configuration({param["name"]: param["value"]}) 37 | 38 | self.success = True 39 | 40 | except rospy.ROSException as e: 41 | rospy.logerr(e) 42 | 43 | 44 | 45 | 46 | if __name__ == "__main__": 47 | 48 | rospy.init_node('reconf_at_edges_server', anonymous=True) 49 | reconf_at_edges_server() 50 | rospy.spin() 51 | ##################################################################################### -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/search_route.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | #import actionlib 6 | #import pymongo 7 | #import json 8 | import sys 9 | import math 10 | 11 | 12 | #from geometry_msgs.msg import Pose 13 | from std_msgs.msg import String 14 | #import scitos_apps_msgs.msg 15 | 16 | #from topological_navigation_msgs.msg import TopologicalNode 17 | #from mongodb_store.message_store import MessageStoreProxy 18 | 19 | from topological_navigation_msgs.msg import TopologicalMap 20 | from topological_navigation.tmap_utils import * 21 | from topological_navigation.route_search import * 22 | 23 | 24 | class SearchRoute(object): 25 | 26 | def __init__(self, goal) : 27 | rospy.loginfo("Waiting for Topological map ...") 28 | 29 | try: 30 | msg = rospy.wait_for_message('topological_map', TopologicalMap, timeout=10.0) 31 | self.top_map = msg 32 | self.lnodes = msg.nodes 33 | except rospy.ROSException : 34 | rospy.logwarn("Failed to get topological map") 35 | return 36 | 37 | rospy.loginfo("Waiting for Current Node ...") 38 | 39 | try: 40 | msg = rospy.wait_for_message('closest_node', String, timeout=10.0) 41 | cnode = msg.data 42 | except rospy.ROSException : 43 | rospy.logwarn("Failed to get closest node") 44 | return 45 | 46 | 47 | rsearch = TopologicalRouteSearch(self.top_map) 48 | route = rsearch.search_route(cnode, goal) 49 | print(route) 50 | 51 | rospy.loginfo("All Done ...") 52 | #rospy.spin() 53 | 54 | 55 | if __name__ == '__main__': 56 | rospy.init_node('route_search') 57 | goal = str(sys.argv[1]) 58 | searcher = SearchRoute(goal) -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/topological_transform_publisher.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | #!/usr/bin/env python3 3 | # ---------------------------------- 4 | # @author: Adam Binch 5 | # @email: abinch@sagarobotics.com 6 | # @date: Fri Feb 26 10:37:50 2021 7 | # ---------------------------------- 8 | 9 | import json 10 | 11 | import rclpy 12 | from rclpy.node import Node 13 | from rclpy.qos import QoSProfile, DurabilityPolicy 14 | import tf2_ros 15 | 16 | from std_msgs.msg import String 17 | from geometry_msgs.msg import Vector3, Quaternion, TransformStamped 18 | 19 | 20 | class TopologicalTransformPublisher(Node): 21 | 22 | def __init__(self): 23 | super().__init__("topological_transform_publisher") 24 | self.tf_broadcaster = tf2_ros.static_transform_broadcaster.StaticTransformBroadcaster(self) 25 | qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) 26 | self.create_subscription(String, '/topological_map_2', self.map_callback, qos) 27 | self.get_logger().info("Transform Publisher waiting for the Topological Map...") 28 | 29 | def map_callback(self, msg): 30 | tmap = json.loads(msg.data) 31 | self.get_logger().info("Transform Publisher received the Topological Map") 32 | transformation = tmap["transformation"] 33 | msg = TransformStamped() 34 | msg.header.stamp = self.get_clock().now().to_msg() 35 | msg.header.frame_id = transformation["parent"] 36 | msg.child_frame_id = transformation["child"] 37 | msg.transform.translation.x = transformation["translation"]["x"] 38 | msg.transform.translation.y = transformation["translation"]["y"] 39 | msg.transform.translation.z = transformation["translation"]["z"] 40 | msg.transform.rotation.x = transformation["rotation"]["x"] 41 | msg.transform.rotation.y = transformation["rotation"]["y"] 42 | msg.transform.rotation.z = transformation["rotation"]["z"] 43 | msg.transform.rotation.w = transformation["rotation"]["w"] 44 | self.tf_broadcaster.sendTransform(msg) 45 | 46 | 47 | def main(args=None): 48 | rclpy.init(args=args) 49 | 50 | TTP = TopologicalTransformPublisher() 51 | rclpy.spin(TTP) 52 | 53 | TTP.destroy_node() 54 | rclpy.shutdown() 55 | 56 | if __name__ == '__main__': 57 | main() 58 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/travel_time_estimator.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | 4 | import rospy 5 | from topological_navigation_msgs.msg import TopologicalMap 6 | from topological_navigation_msgs.srv import EstimateTravelTime 7 | from std_msgs.msg import Duration 8 | import math 9 | import threading 10 | 11 | class TravelTimeEstimator(object): 12 | """docstring for TravelTimeEstimator""" 13 | def __init__(self): 14 | super(TravelTimeEstimator, self).__init__() 15 | self.service_lock = threading.Lock() 16 | self.nodes = dict() 17 | rospy.Subscriber('topological_map', TopologicalMap, self.map_callback) 18 | 19 | while(not rospy.is_shutdown() and len(self.nodes) == 0): 20 | rospy.sleep(1) 21 | 22 | rospy.Service('topological_navigation/travel_time_estimator', EstimateTravelTime, self.estimate_travel_time) 23 | 24 | 25 | def map_callback(self, msg): 26 | self.nodes.clear() 27 | for node in msg.nodes: 28 | self.nodes[node.name] = node 29 | 30 | 31 | def estimate_travel_time(self, req): 32 | self.service_lock.acquire() 33 | 34 | print(req) 35 | 36 | if req.start not in self.nodes: 37 | raise Exception('Unknown start node: %s' % req.start) 38 | if req.target not in self.nodes: 39 | raise Exception('Unknown target node: %s' % req.target) 40 | 41 | startNode = self.nodes[req.start] 42 | endNode = self.nodes[req.target] 43 | 44 | travel_distance = math.hypot((startNode.pose.position.x-endNode.pose.position.x),(startNode.pose.position.y-endNode.pose.position.y)) 45 | 46 | # this is the default scitos speed 47 | travel_speed_ms = 0.5 48 | travel_time = rospy.Duration(travel_distance/travel_speed_ms) 49 | self.service_lock.release() 50 | return travel_time 51 | 52 | if __name__ == '__main__': 53 | rospy.init_node('travel_time_estimator') 54 | tte = TravelTimeEstimator() 55 | rospy.spin() 56 | 57 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/scripts/visualise_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | 6 | import math 7 | 8 | 9 | from time import sleep 10 | 11 | from threading import Timer 12 | from geometry_msgs.msg import Pose 13 | from geometry_msgs.msg import Point 14 | import std_msgs.msg 15 | 16 | #from mongodb_store.message_store import MessageStoreProxy 17 | 18 | from interactive_markers.interactive_marker_server import * 19 | #from interactive_markers.menu_handler import * 20 | from visualization_msgs.msg import * 21 | 22 | from topological_navigation_msgs.msg import TopologicalNode 23 | from topological_navigation.topological_map import * 24 | from topological_navigation.marker_arrays import * 25 | from topological_navigation.node_controller import * 26 | from topological_navigation.edge_controller import * 27 | from topological_navigation.vertex_controller import * 28 | from topological_navigation.node_manager import * 29 | from topological_navigation.edge_std import * 30 | 31 | 32 | 33 | from topological_navigation.goto import * 34 | 35 | 36 | from topological_navigation_msgs.msg import NavRoute 37 | from topological_navigation_msgs.msg import TopologicalMap 38 | import topological_navigation.policies 39 | import topological_navigation.map_marker 40 | 41 | 42 | class VisualiseMap(object): 43 | _killall_timers=False 44 | 45 | def __init__(self, name, edit_mode, noedit_mode) : 46 | rospy.on_shutdown(self._on_node_shutdown) 47 | 48 | self.update_needed=False 49 | self.in_feedback=False 50 | #self._point_set=filename 51 | self._edit_mode = edit_mode 52 | self._noedit_mode = noedit_mode 53 | 54 | self.map_markers = topological_navigation.map_marker.TopologicalVis() 55 | self.pol_markers = topological_navigation.policies.PoliciesVis() 56 | 57 | if not noedit_mode: 58 | rospy.loginfo("Edge Controllers ...") 59 | self.edge_cont = edge_controllers() 60 | rospy.loginfo("Vertex Controllers ...") 61 | self.vert_cont = VertexControllers() 62 | rospy.loginfo("Waypoint Controllers ...") 63 | self.node_cont = WaypointControllers() 64 | rospy.loginfo("Node Manager Controllers ...") 65 | self.add_rm_node = node_manager() 66 | else: 67 | rospy.logwarn("No edit Visualisation mode ...") 68 | 69 | if not self._edit_mode : 70 | rospy.loginfo("Go To Controllers ...") 71 | self.goto_cont = go_to_controllers() 72 | else: 73 | rospy.logwarn("Edit only Visualisation mode ...") 74 | 75 | rospy.loginfo("Done ...") 76 | 77 | 78 | rospy.loginfo("All Done ...") 79 | 80 | 81 | def _on_node_shutdown(self): 82 | print("bye") 83 | 84 | 85 | 86 | if __name__ == '__main__': 87 | edit_mode=False 88 | noedit_mode=False 89 | #mapname=str(sys.argv[1]) 90 | argc = len(sys.argv) 91 | print(argc) 92 | if argc > 1: 93 | if '-edit' in sys.argv or '-e' in sys.argv : 94 | edit_mode = True 95 | if '-noedit' in sys.argv or '-n' in sys.argv : 96 | noedit_mode=True 97 | rospy.init_node('topological_visualisation') 98 | server = VisualiseMap(rospy.get_name(), edit_mode, noedit_mode) 99 | rospy.spin() 100 | -------------------------------------------------------------------------------- /topological_navigation/topological_navigation/topological_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | from geometry_msgs.msg import Pose 4 | 5 | def findInList(name,List): 6 | found = name in List 7 | if found : 8 | pos = List.index(name) 9 | else : 10 | pos = -1 11 | return pos 12 | 13 | 14 | def update_to_expand(to_expand, new_nodes, maptree, father) : 15 | for i in new_nodes : 16 | found_el=False 17 | for j in to_expand : 18 | if i == j.name : 19 | found_el=True 20 | if not found_el : 21 | for k in maptree : 22 | if i == k.name : 23 | new_el=k 24 | new_el._set_Father(father) 25 | to_expand.append(new_el) 26 | return to_expand 27 | 28 | 29 | def get_node(name, maptree) : 30 | for i in maptree : 31 | if i.name == name : 32 | return i 33 | return None 34 | 35 | 36 | class topological_node(object): 37 | def __init__(self, name): 38 | self.name = name 39 | self.expanded=False 40 | self.father='none' 41 | self.px=0.0 42 | self.py=0.0 43 | self.influence_radius=1.5 44 | 45 | def _insert_waypoint(self, waypoint): 46 | self.waypoint=waypoint 47 | self._get_coords() 48 | 49 | def _get_coords(self): 50 | self.px=float(self.waypoint[0]) 51 | self.py=float(self.waypoint[1]) 52 | 53 | def _get_distance(self, cx, cy): 54 | dist=math.hypot((cx-self.px),(cy-self.py)) 55 | return dist 56 | 57 | def _insert_edges(self, edges): 58 | self.edges=edges 59 | 60 | def _insert_vertices(self, vertices): 61 | self.vertices=vertices 62 | a=0 63 | for i in self.vertices : 64 | dist=math.hypot(i[0],i[1]) 65 | if dist > a : 66 | a=dist 67 | self.influence_radius=a 68 | 69 | def _get_Children(self) : 70 | self.expanded=True 71 | a=['none'] 72 | for k in self.edges : 73 | a.append(k['node']) 74 | a.pop(0) 75 | return a 76 | 77 | def _get_action(self, node) : 78 | for k in self.edges : 79 | if k['node'] == node : 80 | return k['action'] 81 | 82 | def _set_Father(self,father) : 83 | self.father=father 84 | 85 | def _get_pose(self): 86 | p = Pose() 87 | p.position.x=float(self.waypoint[0]) 88 | p.position.y=float(self.waypoint[1]) 89 | p.position.z=float(self.waypoint[2]) 90 | p.orientation.x=float(self.waypoint[3]) 91 | p.orientation.y=float(self.waypoint[4]) 92 | p.orientation.z=float(self.waypoint[5]) 93 | p.orientation.w=float(self.waypoint[6]) 94 | return p -------------------------------------------------------------------------------- /topological_navigation_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(topological_navigation_msgs) 3 | 4 | 5 | 6 | 7 | ############################ 8 | # 9 | # DEPENDENCY DEFINITION 10 | # 11 | 12 | # Define build tool dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rosidl_default_generators REQUIRED) 15 | 16 | # Define dependencies for messages 17 | find_package(builtin_interfaces REQUIRED) 18 | find_package(std_msgs REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(actionlib_msgs REQUIRED) 21 | set(msg_deps 22 | builtin_interfaces 23 | std_msgs 24 | geometry_msgs 25 | actionlib_msgs 26 | ) 27 | 28 | 29 | 30 | 31 | ######################## 32 | # 33 | # FILE DECLARATION 34 | # 35 | 36 | # Declare the custom message files 37 | set(msg_files 38 | "msg/ExecutePolicyModeGoal.msg" 39 | "msg/ClosestEdges.msg" 40 | "msg/TopologicalMap.msg" 41 | "msg/TopologicalNode.msg" 42 | "msg/Vertex.msg" 43 | "msg/Edge.msg" 44 | "msg/NavRoute.msg" 45 | "msg/CurrentEdge.msg" 46 | "msg/NavStatistics.msg" 47 | "msg/TopologicalRoute.msg" 48 | "msg/AddEdgeReq.msg" 49 | "msg/AddNodeReq.msg" 50 | "msg/SetInfluenceZoneReq.msg" 51 | "msg/UpdateEdgeConfigReq.msg" 52 | "msg/GotoNodeFeedback.msg" 53 | "msg/ExecutePolicyModeFeedback.msg" 54 | ) 55 | 56 | 57 | # Declare the custom service files 58 | set(srv_files 59 | "srv/WriteTopologicalMap.srv" 60 | "srv/UpdateEdgeConfig.srv" 61 | "srv/UpdateRestrictions.srv" 62 | "srv/UpdateEdge.srv" 63 | "srv/UpdateAction.srv" 64 | "srv/RestrictMap.srv" 65 | "srv/EvaluateNode.srv" 66 | "srv/EvaluateEdge.srv" 67 | "srv/AddDatum.srv" 68 | "srv/GetTaggedNodes.srv" 69 | "srv/GetTags.srv" 70 | "srv/GetNodeTags.srv" 71 | "srv/NodeMetadata.srv" 72 | "srv/GetEdgesBetweenNodes.srv" 73 | "srv/AddNode.srv" 74 | "srv/RmvNode.srv" 75 | "srv/AddEdge.srv" 76 | "srv/AddEdgeRviz.srv" 77 | "srv/AddContent.srv" 78 | "srv/UpdateNodeName.srv" 79 | "srv/UpdateNodeTolerance.srv" 80 | "srv/ModifyTag.srv" 81 | "srv/AddTag.srv" 82 | "srv/GetTopologicalMap.srv" 83 | "srv/UpdateEdgeLegacy.srv" 84 | "srv/LocalisePose.srv" 85 | "srv/GetRouteTo.srv" 86 | "srv/GetRouteBetween.srv" 87 | "srv/EstimateTravelTime.srv" 88 | "srv/PredictEdgeState.srv" 89 | "srv/UpdateFailPolicy.srv" 90 | "srv/ReconfAtEdges.srv" 91 | "srv/LoadTopoNavTestScenario.srv" 92 | "srv/RunTopoNavTestScenario.srv" 93 | "srv/SetInfluenceZone.srv" 94 | "srv/AddEdgeArray.srv" 95 | "srv/AddNodeArray.srv" 96 | "srv/SetInfluenceZoneArray.srv" 97 | "srv/UpdateEdgeConfigArray.srv" 98 | ) 99 | 100 | 101 | 102 | #Generate actions in the 'action' folder 103 | # Declare the custom service files 104 | set(action_files 105 | "action/GotoNode.action" 106 | "action/ExecutePolicyMode.action" 107 | "action/BuildTopPrediction.action" 108 | ) 109 | 110 | 111 | 112 | ####################### 113 | # 114 | # FILE GENERATION 115 | # 116 | 117 | # Generate the files 118 | rosidl_generate_interfaces(${PROJECT_NAME} 119 | ${msg_files} 120 | ${srv_files} 121 | ${action_files} 122 | DEPENDENCIES ${msg_deps} 123 | ) 124 | 125 | 126 | ################################### 127 | # 128 | # DOWNSTREAM DEPENDENCY EXPORT 129 | # 130 | 131 | # Export information to downstream packages 132 | ament_export_dependencies(rosidl_default_runtime) 133 | ament_package() 134 | -------------------------------------------------------------------------------- /topological_navigation_msgs/action/BuildTopPrediction.action: -------------------------------------------------------------------------------- 1 | # These define the epoch range for the model to cover. 2 | # If left blank the component will use the range it was 3 | # previously configured with, either at start-up or in the last call 4 | builtin_interfaces/Time start_range 5 | builtin_interfaces/Time end_range 6 | --- 7 | bool success 8 | --- 9 | string result 10 | -------------------------------------------------------------------------------- /topological_navigation_msgs/action/ExecutePolicyMode.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | topological_navigation_msgs/NavRoute route 3 | --- 4 | #result definition 5 | bool success 6 | --- 7 | #feedback 8 | string current_wp 9 | uint8 status # interpert using action_msgs.msg GoalStatus 10 | -------------------------------------------------------------------------------- /topological_navigation_msgs/action/GotoNode.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | string target 3 | bool no_orientation #Do not care about the final orientation 4 | --- 5 | #result definition 6 | bool success 7 | --- 8 | #feedback 9 | string route 10 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/AddEdgeReq.msg: -------------------------------------------------------------------------------- 1 | string origin 2 | string destination 3 | string action 4 | string action_type 5 | string edge_id 6 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/AddNodeReq.msg: -------------------------------------------------------------------------------- 1 | string name 2 | geometry_msgs/Pose pose 3 | # Set to true to add nodes which are in close proximity (currently hardcoded to 8m) 4 | bool add_close_nodes 5 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/ClosestEdges.msg: -------------------------------------------------------------------------------- 1 | string[] edge_ids 2 | float32[] distances 3 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/CurrentEdge.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | string edge_id 3 | bool active 4 | bool result 5 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/Edge.msg: -------------------------------------------------------------------------------- 1 | string edge_id 2 | string node 3 | string action 4 | float64 top_vel 5 | string map_2d 6 | float64 inflation_radius 7 | string recovery_behaviours_config -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/ExecutePolicyModeFeedback.msg: -------------------------------------------------------------------------------- 1 | string current_wp 2 | uint8 status # interpert using action_msgs.msg/GoalStatus 3 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/ExecutePolicyModeGoal.msg: -------------------------------------------------------------------------------- 1 | #goal definition 2 | topological_navigation_msgs/NavRoute route 3 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/GotoNodeFeedback.msg: -------------------------------------------------------------------------------- 1 | #feedback 2 | string route 3 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/NavRoute.msg: -------------------------------------------------------------------------------- 1 | string[] source 2 | string[] edge_id 3 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/NavStatistics.msg: -------------------------------------------------------------------------------- 1 | string edge_id 2 | string status 3 | string origin 4 | string target 5 | string final_node 6 | string date_started 7 | string date_at_node 8 | string date_finished 9 | float64 time_to_waypoint 10 | float64 operation_time 11 | string topological_map 12 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/SetInfluenceZoneReq.msg: -------------------------------------------------------------------------------- 1 | string name 2 | float64[] vertices_x 3 | float64[] vertices_y 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/TopologicalMap.msg: -------------------------------------------------------------------------------- 1 | string name 2 | string map 3 | string pointset 4 | string last_updated 5 | topological_navigation_msgs/TopologicalNode[] nodes 6 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/TopologicalNode.msg: -------------------------------------------------------------------------------- 1 | string name 2 | string map 3 | string pointset 4 | geometry_msgs/Pose pose 5 | float64 yaw_goal_tolerance #The tolerance in radians for the waypoint position 6 | float64 xy_goal_tolerance #The tolerance in meters for the waypoint position 7 | Vertex[] verts 8 | Edge[] edges 9 | string localise_by_topic #The configuration for localisation by topic -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/TopologicalRoute.msg: -------------------------------------------------------------------------------- 1 | string[] nodes 2 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/UpdateEdgeConfigReq.msg: -------------------------------------------------------------------------------- 1 | string edge_id 2 | string namespace_name 3 | string name 4 | string value # python uses its eval function to determine the type unless it is a string 5 | bool value_is_string 6 | bool not_reset # if false then the param is reconfigured back to its original value after the edge is traversed 7 | 8 | -------------------------------------------------------------------------------- /topological_navigation_msgs/msg/Vertex.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | -------------------------------------------------------------------------------- /topological_navigation_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | topological_navigation_msgs 5 | 3.0.5 6 | The ros2 topological_navigation_msgs package 7 | 8 | 9 | James R Heselden 10 | Marc Hanheide 11 | 12 | James R Heselden 13 | Adam Binch 14 | Jaime Pulido Fentanes 15 | 16 | Apache License 2.0 17 | 18 | ament_copyright 19 | ament_flake8 20 | ament_pep257 21 | python3-pytest 22 | 23 | ament_cmake 24 | 25 | rosidl_default_generators 26 | rosidl_default_runtime 27 | rosidl_interface_packages 28 | 29 | builtin_interfaces 30 | std_msgs 31 | actionlib_msgs 32 | action_msgs 33 | geometry_msgs 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/AddContent.srv: -------------------------------------------------------------------------------- 1 | string node 2 | string content 3 | --- 4 | bool success 5 | string content -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/AddDatum.srv: -------------------------------------------------------------------------------- 1 | float64 latitude 2 | float64 longitude 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/AddEdge.srv: -------------------------------------------------------------------------------- 1 | string origin 2 | string destination 3 | string action 4 | string action_type 5 | string edge_id 6 | --- 7 | bool success 8 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/AddEdgeArray.srv: -------------------------------------------------------------------------------- 1 | topological_navigation_msgs/AddEdgeReq[] data 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/AddEdgeRviz.srv: -------------------------------------------------------------------------------- 1 | # This service allows you to add an edge by providing two poses on the map, and 2 | # will add an edge to the topological map based on the closest nodes to those 3 | # clicked locations. 4 | 5 | # Pose of where the first mouse click happened 6 | geometry_msgs/Pose first 7 | # Pose of where the second mouse click happened 8 | geometry_msgs/Pose second 9 | 10 | # If there are nodes within this distance of a click location (for either the 11 | # start or end node), the edge will not be created 12 | float32 max_distance 13 | 14 | # If true, add two edges - one from first->second, one from second->first 15 | bool bidirectional 16 | --- 17 | bool success 18 | string message -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/AddNode.srv: -------------------------------------------------------------------------------- 1 | string name 2 | geometry_msgs/Pose pose 3 | # Set to true to add nodes which are in close proximity (currently hardcoded to 8m) 4 | bool add_close_nodes 5 | --- 6 | bool success 7 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/AddNodeArray.srv: -------------------------------------------------------------------------------- 1 | topological_navigation_msgs/AddNodeReq[] data 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/AddTag.srv: -------------------------------------------------------------------------------- 1 | string tag 2 | string[] node 3 | --- 4 | bool success 5 | string meta -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/EstimateTravelTime.srv: -------------------------------------------------------------------------------- 1 | string start 2 | string target 3 | --- 4 | builtin_interfaces/Duration travel_time 5 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/EvaluateEdge.srv: -------------------------------------------------------------------------------- 1 | string state # the state of the robot, e.g. '{"robot": "tall", "task": "uv"}' 2 | string edge # the name of the edge 3 | bool runtime # if True, evaluates runtime restrictions otherwise, planning restrictions 4 | --- 5 | 6 | bool success 7 | bool evaluation # the result of the evaluation of restrictions for the edge (or eventually the and-concatenation) -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/EvaluateNode.srv: -------------------------------------------------------------------------------- 1 | string state # the state of the robot, e.g. '{"robot": "tall", "task": "uv"}' 2 | string node # the name of the node 3 | bool runtime # if True, evaluates runtime restrictions otherwise, planning restrictions 4 | --- 5 | 6 | bool success 7 | bool evaluation # the result of the evaluation of restrictions for the node -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/GetEdgesBetweenNodes.srv: -------------------------------------------------------------------------------- 1 | string nodea 2 | string nodeb 3 | --- 4 | string[] ids_a_to_b 5 | string[] ids_b_to_a -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/GetNodeTags.srv: -------------------------------------------------------------------------------- 1 | string node_name 2 | --- 3 | bool success 4 | string[] tags -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/GetRouteBetween.srv: -------------------------------------------------------------------------------- 1 | string origin 2 | string goal 3 | --- 4 | topological_navigation_msgs/NavRoute route 5 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/GetRouteTo.srv: -------------------------------------------------------------------------------- 1 | string goal 2 | --- 3 | topological_navigation_msgs/NavRoute route 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/GetTaggedNodes.srv: -------------------------------------------------------------------------------- 1 | string tag 2 | --- 3 | string[] nodes 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/GetTags.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string[] tags 3 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/GetTopologicalMap.srv: -------------------------------------------------------------------------------- 1 | string pointset #Pointset to retrieve 2 | --- 3 | topological_navigation_msgs/TopologicalMap map 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/LoadTopoNavTestScenario.srv: -------------------------------------------------------------------------------- 1 | string pointset 2 | --- 3 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/LocalisePose.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose pose 2 | --- 3 | string current_node 4 | string closest_node -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/ModifyTag.srv: -------------------------------------------------------------------------------- 1 | string tag 2 | string new_tag 3 | string[] node 4 | --- 5 | bool success 6 | string meta -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/NodeMetadata.srv: -------------------------------------------------------------------------------- 1 | #string map_name 2 | string pointset 3 | string meta_category 4 | --- 5 | string[] name 6 | string[] description 7 | string[] goto_node 8 | string[] node_type 9 | bool[] at_node 10 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/PredictEdgeState.srv: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time epoch 2 | --- 3 | string[] edge_ids 4 | float64[] probs 5 | builtin_interfaces/Duration[] durations 6 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/ReconfAtEdges.srv: -------------------------------------------------------------------------------- 1 | string edge_id 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/RestrictMap.srv: -------------------------------------------------------------------------------- 1 | string state # the state of the robot, e.g. '{"robot": "tall", "task": "uv"}' 2 | 3 | --- 4 | 5 | bool success 6 | string restricted_tmap # the map without the nodes/edges which does not satisfy the restrictions according to the state -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/RmvNode.srv: -------------------------------------------------------------------------------- 1 | string name 2 | --- 3 | bool success -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/RunTopoNavTestScenario.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool nav_success 3 | bool nav_timeout 4 | bool graceful_fail 5 | bool human_success 6 | float64 min_distance_to_human 7 | float64 mean_speed 8 | float64 distance_travelled 9 | float64 travel_time 10 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/SetInfluenceZone.srv: -------------------------------------------------------------------------------- 1 | string name 2 | float64[] vertices_x 3 | float64[] vertices_y 4 | --- 5 | bool success 6 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/SetInfluenceZoneArray.srv: -------------------------------------------------------------------------------- 1 | topological_navigation_msgs/SetInfluenceZoneReq[] data 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateAction.srv: -------------------------------------------------------------------------------- 1 | string action_name # the action for which you are setting the type and goal for every edge in map 2 | string action_type # e.g. move_base_msgs/MoveBaseGoal 3 | string goal # json string setting the args of the goal 4 | --- 5 | bool success 6 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateEdge.srv: -------------------------------------------------------------------------------- 1 | string edge_id 2 | string action_name 3 | string action_type # e.g. move_base_msgs/MoveBaseGoal 4 | string goal # json string setting the args of the goal 5 | string fail_policy 6 | bool not_fluid # if true then the robot gets to the exact pose of the edge's origin node before it is traversed 7 | --- 8 | bool success 9 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateEdgeConfig.srv: -------------------------------------------------------------------------------- 1 | string edge_id 2 | string namespace_name 3 | string name 4 | string value # python uses its eval function to determine the type unless it is a string 5 | bool value_is_string 6 | bool not_reset # if false then the param is reconfigured back to its original value after the edge is traversed 7 | --- 8 | bool success 9 | string message 10 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateEdgeConfigArray.srv: -------------------------------------------------------------------------------- 1 | topological_navigation_msgs/UpdateEdgeConfigReq[] data 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateEdgeLegacy.srv: -------------------------------------------------------------------------------- 1 | string edge_id 2 | string action 3 | float32 top_vel 4 | --- 5 | bool success 6 | string message -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateFailPolicy.srv: -------------------------------------------------------------------------------- 1 | string fail_policy 2 | --- 3 | bool success 4 | 5 | 6 | # "retry_N" with N the number of retries, tries again to execute the same action 7 | # "wait_S_N" with S the number of seconds and N the number of waits, wait action 8 | # "replan_N" with N the number of replans, generates a new plan which does not pass through the same edge where the fail occurred 9 | # "fail" fails the entire plan 10 | 11 | # can combine fail policy actions e.g "replan_2, fail" 12 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateNodeName.srv: -------------------------------------------------------------------------------- 1 | string node_name 2 | string new_name 3 | --- 4 | bool success 5 | string message -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateNodeTolerance.srv: -------------------------------------------------------------------------------- 1 | string node_name 2 | float64 xy_tolerance 3 | float64 yaw_tolerance 4 | --- 5 | bool success 6 | string message -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/UpdateRestrictions.srv: -------------------------------------------------------------------------------- 1 | string name # node name or edge id 2 | string restrictions_planning # boolean sentence 3 | string restrictions_runtime # boolean sentence 4 | bool update_edges # if updating node restrictions then apply planning restrictions to edges involving the node 5 | --- 6 | bool success 7 | string message 8 | -------------------------------------------------------------------------------- /topological_navigation_msgs/srv/WriteTopologicalMap.srv: -------------------------------------------------------------------------------- 1 | string filename # map filename (leave empty to overwrite) 2 | bool no_alias # Disables anchors and aliases in generated topological map yaml file 3 | --- 4 | bool success 5 | string message 6 | -------------------------------------------------------------------------------- /topological_rviz_tools/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_rviz_tools/COLCON_IGNORE -------------------------------------------------------------------------------- /topological_rviz_tools/README.md: -------------------------------------------------------------------------------- 1 | # topological_rviz_tools 2 | Rviz tool for creating a STRANDS topological map 3 | 4 | ## Usage 5 | 6 | This rviz toolset can be launched using 7 | 8 | ```sh 9 | roslaunch topological_rviz_tools strands_rviz_topmap.launch map:=/path/to/map.yaml topmap:=topmap_pointset db_path:=/path/to/db standalone:=true 10 | ``` 11 | 12 | `map` specifies the `yaml` file for the map you are using, and `topmap` the 13 | corresponding pointset that exists in the database. `db_path` is used to point 14 | to the database you want to use. If `standalone` is true, everything needed will 15 | be run automatically. If false, it is assumed that other parts of the strands 16 | system are running (navigation and mongodb_store in particular), and only run 17 | a few additional things. 18 | 19 | You can also add the node tool, edge tool and topological map panel to rviz 20 | individually by using the buttons for adding tools or panels. You will need to 21 | run the above launch file with `standalone=false` for this to work correctly. 22 | 23 | When you launch with a database which contains a topological map, you should see 24 | something like the following: 25 | 26 | ![Annotated rviz_tools image](images/00_annotated.png) 27 | 28 | You can move nodes around by clicking the arrow at the centre of topological 29 | nodes and dragging. The ring around the node allows you to change the 30 | orientation of the node. You can delete edges using the red arrows in the middle 31 | of edges. 32 | 33 | The following sections give a little more detail about to tools and panel 34 | labelled in the image above. 35 | 36 | ### 1. The edge tool 37 | 38 | Use this tool to create edges between nodes. Left click to select the start point of 39 | the edge, then click again to create an edge. The edge will be created between 40 | the nodes closest to the two clicks, but only if there are nodes within some 41 | distance of the clicks. Left clicking will create a bidirectional edge, whereas 42 | right clicking will create an edge only from the node closest to the first click 43 | to the second one. This tool stays on until you press escape. 44 | 45 | The shortcut is `e`. 46 | 47 | ### 2. The node tool 48 | 49 | This tool allows you to add nodes to the topological map. Click the tool and 50 | then click on the map to add a node in that location. Edges will automatically 51 | be added between the new node and any nodes in close proximity. 52 | 53 | The shortcut is `n`. 54 | 55 | ### 3. Add tag button 56 | 57 | This button allows you to add tags to nodes. You can select multiple nodes, and 58 | the tag you enter in the dialog box will be added to all of them. 59 | 60 | ### 4. Remove button 61 | 62 | With this button, you can remove edges, tags, and nodes from the topological 63 | map. You can select multiple elements and they will all be removed at once. 64 | 65 | ### 5. Topological map panel 66 | 67 | You can see all the elements of the topological map here. You can edit the 68 | following elements: 69 | 70 | - Node name 71 | - Node pose 72 | - Node tags 73 | - Node yaw tolerance 74 | - Node xy tolerance 75 | - Edge action 76 | - Edge velocity 77 | 78 | Ctrl-click allows you to select multiple distinct elements. Shift-click will 79 | select elements between the previously selected element and the current one. 80 | -------------------------------------------------------------------------------- /topological_rviz_tools/images/00_annotated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_rviz_tools/images/00_annotated.png -------------------------------------------------------------------------------- /topological_rviz_tools/images/00_annotated.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_rviz_tools/images/00_annotated.xcf -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/edge_controller.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPMAP_EDGE_CONTROLLER_H 2 | #define TOPMAP_EDGE_CONTROLLER_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "rviz_common/config.hpp" 12 | #include "rviz_common/display_context.hpp" 13 | #include "rviz_common/frame_manager_iface.hpp" 14 | #include "rviz_common/load_resource.hpp" 15 | #include "rviz_rendering/render_system.hpp" 16 | #include "rviz_common/properties/bool_property.hpp" 17 | #include "rviz_common/properties/enum_property.hpp" 18 | #include "rviz_common/properties/float_property.hpp" 19 | #include "rviz_common/properties/property.hpp" 20 | #include "rviz_common/render_panel.hpp" 21 | #include "rviz_common/interaction/selection_manager.hpp" 22 | #include "rviz_common/viewport_mouse_event.hpp" 23 | #include "rviz_common/window_manager_interface.hpp" 24 | #include "topological_navigation_msgs/msg/edge.hpp" 25 | #include "geometry_msgs/msg/pose.hpp" 26 | #include "edge_property.hpp" 27 | 28 | class QKeyEvent; 29 | 30 | namespace topological_rviz_tools { 31 | class EdgeController: public rviz_common::properties::Property 32 | { 33 | Q_OBJECT 34 | public: 35 | EdgeController(const QString& name = QString(), 36 | const std::vector& default_values = std::vector(), 37 | const QString& description = QString(), 38 | rviz_common::properties::Property* parent = 0, 39 | const char *changed_slot = 0, 40 | QObject* receiver = 0); 41 | virtual ~EdgeController(); 42 | 43 | void initialize(); 44 | 45 | /** @brief Subclasses should call this whenever a change is made which would change the results of toString(). */ 46 | void emitConfigChanged(); 47 | 48 | // required by something that initialises this class 49 | QString formatClassId(const QString& class_id); 50 | 51 | /** @brief Return the class identifier which was used to create this 52 | * instance. This version just returns whatever was set with 53 | * setClassId(). */ 54 | virtual QString getClassId() const { return class_id_; } 55 | 56 | /** @brief Set the class identifier used to create this instance. 57 | * Typically this will be set by the factory object which created it. */ 58 | virtual void setClassId( const QString& class_id ) { class_id_ = class_id; } 59 | 60 | virtual void load(const rviz_common::Config& config); 61 | virtual void save(rviz_common::Config config) const; 62 | bool addEdge(const topological_navigation_msgs::msg::Edge& edge); 63 | Q_SIGNALS: 64 | void configChanged(); 65 | 66 | protected: 67 | /** @brief Do subclass-specific initialization. Called by 68 | * EdgeController::initialize after context_ and camera_ are set. 69 | * Default implementation does nothing. */ 70 | virtual void onInitialize() {} 71 | private: 72 | QString class_id_; 73 | std::vector edges_; 74 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 75 | }; 76 | 77 | } // end namespace topological_rviz_tools 78 | 79 | #endif // TOPMAP_EDGE_CONTROLLER_H 80 | -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/edge_property.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EDGE_PROPERTY_H 2 | #define EDGE_PROPERTY_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "rviz_common/properties/property.hpp" 6 | #include "rviz_common/properties/string_property.hpp" 7 | #include "rviz_common/properties/float_property.hpp" 8 | #include "topological_navigation_msgs/msg/edge.hpp" 9 | #include "topological_navigation_msgs/srv/update_edge_legacy.hpp" 10 | 11 | namespace topological_rviz_tools 12 | { 13 | 14 | /** @brief Property specialized to provide getter for booleans. */ 15 | class EdgeProperty: public rviz_common::properties::Property 16 | { 17 | Q_OBJECT 18 | public: 19 | EdgeProperty(const QString& name = QString(), 20 | const topological_navigation_msgs::msg::Edge& default_value = topological_navigation_msgs::msg::Edge(), 21 | const QString& description = QString(), 22 | Property* parent = 0, 23 | const char *changed_slot = 0, 24 | QObject* receiver = 0); 25 | 26 | virtual ~EdgeProperty(); 27 | 28 | std::string getEdgeId() { return edge_id_->getString().toStdString(); } 29 | public Q_SLOTS: 30 | void updateAction(); 31 | void updateTopvel(); 32 | 33 | Q_SIGNALS: 34 | void edgeModified(); 35 | 36 | private: 37 | const topological_navigation_msgs::msg::Edge& edge_; 38 | 39 | // keep track of changing values to ensure that they are redisplayed correctly 40 | // when we fail to update. 41 | std::string action_value_; 42 | float topvel_value_; 43 | 44 | bool reset_value_; 45 | ros::ServiceClient edgeUpdate_; 46 | 47 | rviz_common::properties::StringProperty* edge_id_; 48 | rviz_common::properties::StringProperty* node_; 49 | rviz_common::properties::StringProperty* action_; 50 | rviz_common::properties::StringProperty* map_2d_; 51 | rviz_common::properties::FloatProperty* inflation_radius_; 52 | rviz_common::properties::FloatProperty* top_vel_; 53 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 54 | }; 55 | 56 | } // end namespace topological_rviz_tools 57 | 58 | #endif // EDGE_PROPERTY_H 59 | -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/node_property.hpp: -------------------------------------------------------------------------------- 1 | #ifndef NODE_PROPERTY_H 2 | #define NODE_PROPERTY_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "rviz_common/properties/property.hpp" 6 | #include "rviz_common/properties/string_property.hpp" 7 | #include "rviz_common/properties/float_property.hpp" 8 | #include "topological_navigation_msgs/msg/topological_node.hpp" 9 | #include "topological_navigation_msgs/srv/get_node_tags.hpp" 10 | #include "topological_navigation_msgs/srv/update_node_name.hpp" 11 | #include "topological_navigation_msgs/srv/update_node_tolerance.hpp" 12 | #include "pose_property.hpp" 13 | #include "edge_controller.hpp" 14 | #include "tag_controller.hpp" 15 | 16 | namespace topological_rviz_tools 17 | { 18 | 19 | class TagController; 20 | 21 | /** @brief Property specialized to provide getter for booleans. */ 22 | class NodeProperty: public rviz_common::properties::Property 23 | { 24 | Q_OBJECT 25 | public: 26 | NodeProperty(const QString& name = QString(), 27 | const topological_navigation_msgs::msg::TopologicalNode& default_value = topological_navigation_msgs::msg::TopologicalNode(), 28 | const QString& description = QString(), 29 | Property* parent = 0, 30 | const char *changed_slot = 0, 31 | QObject* receiver = 0); 32 | 33 | virtual ~NodeProperty(); 34 | 35 | std::string getNodeName() { return name_; } 36 | TagController* getTagController() { return tag_controller_; } 37 | public Q_SLOTS: 38 | void updateYawTolerance(); 39 | void updateXYTolerance(); 40 | void updateNodeName(); 41 | void nodePropertyUpdated(); 42 | 43 | Q_SIGNALS: 44 | void nodeModified(Property* node); 45 | 46 | private: 47 | const topological_navigation_msgs::msg::TopologicalNode& node_; 48 | 49 | ros::ServiceClient nameUpdate_; 50 | ros::ServiceClient toleranceUpdate_; 51 | 52 | rviz_common::properties::StringProperty* node_name_; 53 | rviz_common::properties::StringProperty* map_; 54 | rviz_common::properties::StringProperty* pointset_; 55 | rviz_common::properties::StringProperty* localise_; 56 | rviz_common::properties::FloatProperty* yaw_tolerance_; 57 | rviz_common::properties::FloatProperty* xy_tolerance_; 58 | // Store the name so that we can refer to it to change the node name in the 59 | // map - once it changes in the property we won't know its previous value 60 | // otherwise. 61 | std::string name_; 62 | // Also store the editable values, in case the service call fails. We then 63 | // reset the property value to its original value. 64 | float xy_tol_value_; 65 | float yaw_tol_value_; 66 | bool reset_value_; 67 | PoseProperty* pose_; 68 | EdgeController* edge_controller_; 69 | TagController* tag_controller_; 70 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 71 | }; 72 | 73 | } // end namespace topological_rviz_tools 74 | 75 | #endif // NODE_PROPERTY_H 76 | -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/pose_property.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSE_PROPERTY_H 2 | #define POSE_PROPERTY_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "topological_navigation_msgs/srv/add_node.hpp" 6 | #include "geometry_msgs/msg/pose.hpp" 7 | #include "rviz_common/properties/property.hpp" 8 | #include "rviz_common/properties/float_property.hpp" 9 | #include "rviz_common/properties/string_property.hpp" 10 | 11 | namespace topological_rviz_tools 12 | { 13 | 14 | /** @brief Property specialized to provide getter for booleans. */ 15 | class PoseProperty: public rviz_common::properties::Property 16 | { 17 | 18 | public: 19 | PoseProperty(const QString& name = QString(), 20 | const geometry_msgs::msg::Pose& default_value = geometry_msgs::msg::Pose(), 21 | const QString& description = QString(), 22 | rviz_common::properties::Property* parent = 0, 23 | const char *changed_slot = 0, 24 | QObject* receiver = 0); 25 | 26 | virtual ~PoseProperty(); 27 | 28 | public Q_SLOTS: 29 | void positionUpdated(); 30 | 31 | Q_SIGNALS: 32 | void poseModified(); 33 | 34 | private: 35 | const geometry_msgs::msg::Pose& pose_; 36 | rviz_common::properties::StringProperty* orientation_; 37 | rviz_common::properties::FloatProperty* orientation_w_; 38 | rviz_common::properties::FloatProperty* orientation_x_; 39 | rviz_common::properties::FloatProperty* orientation_y_; 40 | rviz_common::properties::FloatProperty* orientation_z_; 41 | rviz_common::properties::StringProperty* position_; 42 | rviz_common::properties::FloatProperty* position_x_; 43 | rviz_common::properties::FloatProperty* position_y_; 44 | rviz_common::properties::FloatProperty* position_z_; 45 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 46 | 47 | ros::ServiceClient poseUpdate_; 48 | }; 49 | 50 | } // end namespace topological_rviz_tools 51 | 52 | #endif // POSE_PROPERTY_H 53 | -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/tag_controller.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TAG_CONTROLLER_H 2 | #define TAG_CONTROLLER_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "rviz_common/properties/property.hpp" 6 | #include "rviz_common/properties/string_property.hpp" 7 | #include "tag_property.hpp" 8 | #include "node_property.hpp" 9 | 10 | namespace topological_rviz_tools 11 | { 12 | 13 | class NodeProperty; 14 | 15 | /** @brief Property specialized to provide getter for booleans. */ 16 | class TagController: public rviz_common::properties::Property 17 | { 18 | 19 | public: 20 | TagController(const QString& name = QString(), 21 | const std::vector& default_value = std::vector(), 22 | const QString& description = QString(), 23 | NodeProperty* parent = 0, 24 | const char *changed_slot = 0, 25 | QObject* receiver = 0); 26 | 27 | virtual ~TagController(); 28 | /** @brief Do all setup that can't be done in the constructor. 29 | * 30 | * 31 | * Calls onInitialize() just before returning. */ 32 | void initialize(); 33 | 34 | /** @brief Subclasses should call this whenever a change is made which would change the results of toString(). */ 35 | void emitConfigChanged(); 36 | 37 | virtual void load(const rviz_common::Config& config); 38 | virtual void save(rviz_common::Config config) const; 39 | 40 | // required by something that initialises this class 41 | QString formatClassId(const QString& class_id); 42 | 43 | /** @brief Return the class identifier which was used to create this 44 | * instance. This version just returns whatever was set with 45 | * setClassId(). */ 46 | virtual QString getClassId() const { return class_id_; } 47 | 48 | /** @brief Set the class identifier used to create this instance. 49 | * Typically this will be set by the factory object which created it. */ 50 | virtual void setClassId( const QString& class_id ) { class_id_ = class_id; } 51 | 52 | Q_SIGNALS: 53 | void configChanged(); 54 | 55 | private: 56 | 57 | QString class_id_; 58 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 59 | }; 60 | 61 | } // end namespace topological_rviz_tools 62 | 63 | #endif // TAG_CONTROLLER_H 64 | -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/tag_property.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TAG_PROPERTY_H 2 | #define TAG_PROPERTY_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "rviz_common/properties/property.hpp" 6 | #include "rviz_common/properties/string_property.hpp" 7 | #include "topological_navigation_msgs/srv/modify_tag.hpp" 8 | 9 | namespace topological_rviz_tools 10 | { 11 | 12 | class TagProperty: public rviz_common::properties::StringProperty 13 | { 14 | 15 | public: 16 | TagProperty(const QString& name = QString(), 17 | const QString& default_value = QString(), 18 | const QString& description = QString(), 19 | const QString& node_name = QString(), 20 | Property* parent = 0, 21 | const char *changed_slot = 0, 22 | QObject* receiver = 0); 23 | 24 | 25 | virtual ~TagProperty(); 26 | void addTag(const QString& tag); 27 | 28 | public Q_SLOTS: 29 | void updateTag(); 30 | 31 | Q_SIGNALS: 32 | void tagModified(); 33 | private: 34 | ros::ServiceClient tagUpdate_; 35 | std::string tag_value_; // keep value so it's not lost if we fail to update 36 | bool reset_value_; 37 | std::string node_name_; 38 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 39 | }; 40 | 41 | } // end namespace topological_rviz_tools 42 | 43 | #endif // TAG_PROPERTY_H 44 | -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/topological_edge_tool.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPMAP_EDGE_TOOL_H 2 | #define TOPMAP_EDGE_TOOL_H 3 | 4 | #include 5 | #include "rviz_common/tool.hpp" 6 | #include 7 | #include 8 | #include "topological_navigation_msgs/srv/add_edge_rviz.hpp" 9 | #include "std_msgs/msg/header.hpp" 10 | 11 | namespace rviz 12 | { 13 | class VectorProperty; 14 | class VisualizationManager; 15 | class ViewportMouseEvent; 16 | } 17 | 18 | namespace topological_rviz_tools 19 | { 20 | 21 | // BEGIN_TUTORIAL 22 | // Here we declare our new subclass of rviz::Tool. Every tool 23 | // which can be added to the tool bar is a subclass of 24 | // rviz::Tool. 25 | class TopmapEdgeTool: public rviz::Tool 26 | { 27 | public: 28 | TopmapEdgeTool(); 29 | ~TopmapEdgeTool(); 30 | 31 | virtual void onInitialize(); 32 | 33 | virtual void activate(); 34 | virtual void deactivate(); 35 | 36 | virtual int processMouseEvent(rviz::ViewportMouseEvent& event); 37 | private: 38 | rclcpp::Publisher::SharedPtr markerPub_; 39 | ros::Publisher update_map_; 40 | ros::ServiceClient addEdgeSrv_; 41 | bool noClick_; // true if nothing clicked yet 42 | geometry_msgs::msg::Pose firstClick_; 43 | visualization_msgs::msg::Marker edgeMarker_; 44 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 45 | 46 | }; 47 | } // end namespace topological_rviz_tools 48 | 49 | #endif // TOPMAP_EDGE_TOOL_H 50 | -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/topological_map_panel.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPMAP_PANEL_H 2 | #define TOPMAP_PANEL_H 3 | 4 | #include 5 | 6 | #include "rviz_common/panel.hpp" 7 | #include "topmap_manager.hpp" 8 | #include "tag_property.hpp" 9 | #include "edge_property.hpp" 10 | #include "node_property.hpp" 11 | #include "rclcpp/rclcpp.hpp" 12 | 13 | #include "rviz_common/properties/property_tree_widget.hpp" 14 | #include "std_msgs/msg/header.hpp" 15 | 16 | #include "topological_navigation_msgs/srv/add_tag.hpp" 17 | #include "topological_navigation_msgs/srv/add_edge.hpp" 18 | #include "topological_navigation_msgs/srv/rmv_node.hpp" 19 | 20 | class QComboBox; 21 | class QMessageBox; 22 | class QModelIndex; 23 | class QPushButton; 24 | class QInputDialog; 25 | 26 | namespace topological_rviz_tools { 27 | /** 28 | * @brief Panel for choosing the view controller and saving and restoring 29 | * viewpoints. 30 | */ 31 | class TopologicalMapPanel: public rviz::Panel 32 | { 33 | public: 34 | TopologicalMapPanel(QWidget* parent = 0); 35 | virtual ~TopologicalMapPanel() {} 36 | 37 | /** @brief Overridden from TopologicalMapPanel. Just calls setMan() with vis_manager_->getTopmapManager(). */ 38 | virtual void onInitialize(); 39 | 40 | /** @brief Set the TopmapManager which this panel should display and edit. 41 | * 42 | * If this TopologicalMapPanel is to be used with a TopmapManager other than 43 | * the one in the TopmapManager sent in through 44 | * TopologicalMapPanel::initialize(), either TopologicalMapPanelel::initialize() must not be 45 | * called or setTopmapManager() must be called after 46 | * TopologicalMapPanel::initialize(). */ 47 | void setTopmapManager(TopmapManager* topmap_man); 48 | 49 | /** @brief Returns the current TopmapManager. */ 50 | TopmapManager* getTopmapManager() const { return topmap_man_; } 51 | 52 | /** @brief Load configuration data, specifically the PropertyTreeWidget view settings. */ 53 | virtual void load(const rviz_common::Config& config); 54 | 55 | /** @brief Save configuration data, specifically the PropertyTreeWidget view settings. */ 56 | virtual void save(rviz_common::Config config) const; 57 | 58 | private Q_SLOTS: 59 | void onDeleteClicked(); 60 | void onAddTagClicked(); 61 | void renameSelected(); 62 | void onCurrentChanged(); 63 | void updateTopMap(); 64 | private: 65 | ros::ServiceClient delNodeSrv_; 66 | ros::ServiceClient delTagSrv_; 67 | ros::ServiceClient delEdgeSrv_; 68 | ros::ServiceClient addTagSrv_; 69 | ros::Publisher update_map_; 70 | 71 | TopmapManager* topmap_man_; 72 | rviz::PropertyTreeWidget* properties_view_; 73 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 74 | }; 75 | 76 | } // namespace topological_rviz_tools 77 | 78 | #endif // TOPMAP_PANEL_H 79 | -------------------------------------------------------------------------------- /topological_rviz_tools/include/topological_rviz_tools/topological_node_tool.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPMAP_NODE_TOOL_H 2 | #define TOPMAP_NODE_TOOL_H 3 | 4 | #include 5 | #include "rviz_common/tool.hpp" 6 | #include "geometry_msgs/msg/pose.hpp" 7 | #include "std_msgs/msg/header.hpp" 8 | #include "topological_navigation_msgs/srv/add_node.hpp" 9 | 10 | namespace rviz 11 | { 12 | class VectorProperty; 13 | class VisualizationManager; 14 | class ViewportMouseEvent; 15 | } 16 | 17 | namespace topological_rviz_tools 18 | { 19 | 20 | // BEGIN_TUTORIAL 21 | // Here we declare our new subclass of rviz::Tool. Every tool 22 | // which can be added to the tool bar is a subclass of 23 | // rviz::Tool. 24 | class TopmapNodeTool: public rviz::Tool 25 | { 26 | public: 27 | TopmapNodeTool(); 28 | ~TopmapNodeTool(); 29 | 30 | virtual void onInitialize(); 31 | 32 | virtual void activate(); 33 | virtual void deactivate(); 34 | 35 | virtual int processMouseEvent(rviz::ViewportMouseEvent& event); 36 | private: 37 | ros::ServiceClient addNodeSrv_; 38 | ros::Publisher update_map_; 39 | rclcpp::Logger logger_{rclcpp::get_logger("rviz2")}; 40 | }; 41 | } // end namespace topological_rviz_tools 42 | 43 | #endif // TOPMAP_NODE_TOOL_H 44 | -------------------------------------------------------------------------------- /topological_rviz_tools/launch/strands_rviz_topmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /topological_rviz_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | topological_rviz_tools 5 | 3.0.0 6 | The ros2 rviz-based topological map construction package 7 | 8 | 9 | Marc Hanheide 10 | Marc Hanheide 11 | Michal Staniaszek 12 | 13 | Apache-2.0 14 | 15 | ament_copyright 16 | ament_flake8 17 | ament_pep257 18 | python3-pytest 19 | ament_cmake 20 | 21 | rosidl_default_generators 22 | rosidl_default_runtime 23 | rosidl_interface_packages 24 | 25 | 26 | std_msgs 27 | diagnostic_msgs 28 | geometry_msgs 29 | topological_navigation_msgs 30 | rclcpp 31 | libogre-dev 32 | rviz_common 33 | rviz_default_plugins 34 | rviz_ogre_vendor 35 | rviz_rendering 36 | 37 | 38 | ament_cmake 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /topological_rviz_tools/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Tool for manipulating a topological map 7 | 8 | 9 | 10 | 11 | Tool for manipulating edges in the topological map 12 | 13 | 14 | 15 | 16 | Tool for adding nodes in the topological map 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /topological_rviz_tools/scripts/python_topmap_interface.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import math 5 | import operator 6 | from std_msgs.msg import Time 7 | from topological_navigation_msgs.msg import TopologicalMap 8 | from topological_navigation_msgs.srv import * 9 | from geometry_msgs.msg import Pose 10 | 11 | class TopmapInterface(object): 12 | """Creates some topics which can be used by c++ code in the rviz portion of this 13 | package to call into python functions to modify the topological map 14 | 15 | """ 16 | 17 | def __init__(self): 18 | """ 19 | name: the name of the map as recorded in the database. Use 20 | topological_util/list_maps to see which ones are available 21 | 22 | """ 23 | rospy.init_node("topmap_interface") 24 | self.name = rospy.get_param('~map_name') 25 | self.topmap = None 26 | self.new_nodes = 0 27 | 28 | self.topmap_sub = rospy.Subscriber("/topological_map", TopologicalMap, self.topmap_cb) 29 | self.add_edge_srv = rospy.Service("~add_edge", topological_navigation_msgs.srv.AddEdgeRviz, self.add_edge) 30 | 31 | self.manager_add_edge = rospy.ServiceProxy("/topological_map_manager/add_edges_between_nodes", topological_navigation_msgs.srv.AddEdge) 32 | 33 | rospy.spin() 34 | 35 | def add_edge(self, req): 36 | def tuple_dist(pose_tuple): 37 | return math.sqrt(pow(pose_tuple[0].position.x - pose_tuple[1].position.x, 2) 38 | + pow(pose_tuple[0].position.y - pose_tuple[1].position.y, 2)) 39 | 40 | # Find the nodes closest to the start and end point of the line given in the request 41 | poses = [node.pose for node in self.topmap.nodes] 42 | 43 | from_dists = map(tuple_dist, zip([req.first]*len(poses), poses)) 44 | to_dists = map(tuple_dist, zip([req.second]*len(poses), poses)) 45 | closest_from_ind, from_dist = min(enumerate(from_dists), key=operator.itemgetter(1)) 46 | closest_to_ind, to_dist = min(enumerate(to_dists), key=operator.itemgetter(1)) 47 | 48 | if req.max_distance > 0 and (from_dist > req.max_distance or to_dist > req.max_distance): 49 | return AddEdgeResponse(True, "Click locations were not close enough to a node") 50 | 51 | from_name = self.topmap.nodes[closest_from_ind].name 52 | to_name = self.topmap.nodes[closest_to_ind].name 53 | 54 | if from_name == to_name: 55 | return AddEdgeResponse(True, "Can't have an edge from a node to itself.") 56 | 57 | message = "Added edge from {0} to {1}".format(from_name, to_name) 58 | 59 | self.manager_add_edge(origin=from_name, destination=to_name, action="move_base") 60 | if req.bidirectional: 61 | self.manager_add_edge(origin=to_name, destination=from_name, action="move_base") 62 | message += " (bidirectional)" 63 | 64 | return topological_navigation_msgs.srv.AddEdgeRvizResponse(True, message) 65 | 66 | def topmap_cb(self, msg): 67 | rospy.loginfo("Topological map was updated via callback.") 68 | self.topmap = msg 69 | 70 | if __name__ == '__main__': 71 | TopmapInterface() 72 | -------------------------------------------------------------------------------- /topological_rviz_tools/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | package_name = 'topological_rviz_tools' 3 | 4 | setup( 5 | name=package_name, 6 | version='3.0.0', 7 | packages=[package_name], 8 | data_files=[ 9 | ('share/ament_index/resource_index/packages', 10 | ['resource/' + package_name]), 11 | ('share/' + package_name, ['package.xml']), 12 | ], 13 | install_requires=[''], 14 | zip_safe=True, 15 | maintainer='Michal Staniaszek', 16 | maintainer_email='staniasm@cs.bham.ac.uk', 17 | description='The ros2 rviz-based topological map construction package', 18 | license='BSD', 19 | tests_require=['pytest'], 20 | entry_points={ 21 | 'console_scripts': [ 22 | 'python_topmap_interface.py = topological_rviz_tools.scripts.python_topmap_interface.py' 23 | ], 24 | }, 25 | 26 | ) 27 | 28 | 29 | -------------------------------------------------------------------------------- /topological_rviz_tools/src/edge_controller.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | namespace topological_rviz_tools 5 | { 6 | EdgeController::EdgeController(const QString& name, 7 | const std::vector& default_values, 8 | const QString& description, 9 | rviz_common::properties::Property* parent, 10 | const char *changed_slot, 11 | QObject* receiver) 12 | : rviz_common::properties::Property(name, "", description, parent, changed_slot, receiver) 13 | { 14 | for (int i = 0; i < default_values.size(); i++) { 15 | // RCLCPP_INFO("ADDING EDGE %s", default_values[i].edge_id.c_str()); 16 | EdgeProperty* newEdge = new EdgeProperty("Edge", default_values[i], ""); 17 | addChild(newEdge); 18 | connect(newEdge, SIGNAL(edgeModified()), parent, SLOT(nodePropertyUpdated())); 19 | } 20 | } 21 | 22 | void EdgeController::initialize() 23 | { 24 | 25 | std::stringstream ss; 26 | static int count = 0; 27 | ss << "EdgeControllerCamera" << count++; 28 | 29 | // Do subclass initialization. 30 | onInitialize(); 31 | } 32 | 33 | EdgeController::~EdgeController() 34 | { 35 | for (;numChildren() != 0;) { 36 | delete takeChildAt(0); 37 | } 38 | } 39 | 40 | QString EdgeController::formatClassId(const QString& class_id) 41 | { 42 | QStringList id_parts = class_id.split("/"); 43 | if(id_parts.size() != 2) 44 | { 45 | // Should never happen with pluginlib class ids, which are 46 | // formatted like "package_name/class_name". Not worth crashing 47 | // over though. 48 | return class_id; 49 | } 50 | else 51 | { 52 | return id_parts[ 1 ] + " (" + id_parts[ 0 ] + ")"; 53 | } 54 | } 55 | 56 | 57 | void EdgeController::emitConfigChanged() 58 | { 59 | Q_EMIT configChanged(); 60 | } 61 | 62 | void EdgeController::load(const rviz_common::Config& config) 63 | { 64 | // // Load the name by hand. 65 | // QString name; 66 | // if(config.mapGetString("Name", &name)) 67 | // { 68 | // setName(name); 69 | // } 70 | // // Load all sub-properties the same way the base class does. 71 | // rviz_common::properties::Property::load(config); 72 | } 73 | 74 | void EdgeController::save(rviz_common::Config config) const 75 | { 76 | // config.mapSetValue("Class", getClassId()); 77 | // config.mapSetValue("Name", getName()); 78 | 79 | // rviz_common::properties::Property::save(config); 80 | } 81 | 82 | } // end namespace topological_rviz_tools 83 | -------------------------------------------------------------------------------- /topological_rviz_tools/src/pose_property.cpp: -------------------------------------------------------------------------------- 1 | #include "topological_rviz_tools/pose_property.hpp" 2 | 3 | namespace topological_rviz_tools 4 | { 5 | 6 | PoseProperty::PoseProperty(const QString& name, 7 | const geometry_msgs::msg::Pose& default_value, 8 | const QString& description, 9 | rviz_common::properties::Property* parent, 10 | const char *changed_slot, 11 | QObject* receiver) 12 | // We set the default value sent to the base property to the empty string, 13 | // rather than trying to put in the geometry msgs pose 14 | : rviz_common::properties::Property(name, "", description, parent, changed_slot, receiver), 15 | pose_(default_value) 16 | { 17 | connect(this, SIGNAL(poseModified()), parent, SLOT(nodePropertyUpdated())); 18 | setReadOnly(true); // can't change the name of this pose 19 | 20 | ros::NodeHandle nh; 21 | // Use addnode because it has the fields we need, so we don't have to write a 22 | // new message 23 | poseUpdate_ = nh.serviceClient("/topological_map_manager/update_node_pose", true); 24 | 25 | orientation_ = new rviz_common::properties::StringProperty("Orientation", "", "", this); 26 | orientation_w_ = new rviz_common::properties::FloatProperty("w", pose_.orientation.w, "", orientation_); 27 | orientation_x_ = new rviz_common::properties::FloatProperty("x", pose_.orientation.x, "", orientation_); 28 | orientation_y_ = new rviz_common::properties::FloatProperty("y", pose_.orientation.y, "", orientation_); 29 | orientation_z_ = new rviz_common::properties::FloatProperty("z", pose_.orientation.z, "", orientation_); 30 | // Don't allow messing around with the quaternion from here - can be done 31 | // using the interactive marker. 32 | orientation_->setReadOnly(true); 33 | orientation_w_->setReadOnly(true); 34 | orientation_x_->setReadOnly(true); 35 | orientation_y_->setReadOnly(true); 36 | orientation_z_->setReadOnly(true); 37 | 38 | position_ = new rviz_common::properties::StringProperty("Position", "", "", this); 39 | position_x_ = new rviz_common::properties::FloatProperty("x", pose_.position.x, "", position_, SLOT(positionUpdated()), this); 40 | position_y_ = new rviz_common::properties::FloatProperty("y", pose_.position.y, "", position_, SLOT(positionUpdated()), this); 41 | position_z_ = new rviz_common::properties::FloatProperty("z", pose_.position.z, "", position_); 42 | 43 | // Don't allow modification of z position of the node 44 | position_->setReadOnly(true); 45 | position_z_->setReadOnly(true); 46 | } 47 | 48 | PoseProperty::~PoseProperty() 49 | { 50 | delete orientation_w_; 51 | delete orientation_x_; 52 | delete orientation_y_; 53 | delete orientation_z_; 54 | delete orientation_; 55 | delete position_x_; 56 | delete position_y_; 57 | delete position_z_; 58 | delete position_; 59 | } 60 | 61 | void PoseProperty::positionUpdated() 62 | { 63 | topological_navigation_msgs::AddNode srv; 64 | srv.request.name = getParent()->getValue().toString().toStdString().c_str(); 65 | srv.request.pose.position.x = position_x_->getFloat(); 66 | srv.request.pose.position.y = position_y_->getFloat(); 67 | srv.request.pose.position.z = position_z_->getFloat(); 68 | srv.request.pose.orientation.x = orientation_x_->getFloat(); 69 | srv.request.pose.orientation.y = orientation_y_->getFloat(); 70 | srv.request.pose.orientation.z = orientation_z_->getFloat(); 71 | srv.request.pose.orientation.w = orientation_w_->getFloat(); 72 | 73 | if (poseUpdate_.call(srv)) { 74 | RCLCPP_INFO(logger_, "Successfully updated pose for node %s", srv.request.name.c_str()); 75 | Q_EMIT poseModified(); 76 | } else { 77 | RCLCPP_WARN(logger_, "Failed to update pose for node %s", srv.request.name.c_str()); 78 | } 79 | } 80 | 81 | } // end namespace topological_rviz_tools 82 | -------------------------------------------------------------------------------- /topological_rviz_tools/src/tag_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "topological_rviz_tools/tag_controller.hpp" 2 | 3 | namespace topological_rviz_tools 4 | { 5 | TagController::TagController(const QString& name, 6 | const std::vector& default_values, 7 | const QString& description, 8 | NodeProperty* parent, 9 | const char *changed_slot, 10 | QObject* receiver) 11 | : rviz_common::properties::Property(name, "", description, parent, changed_slot, receiver) 12 | { 13 | for (int i = 0; i < default_values.size(); i++) { 14 | TagProperty* newTag = new TagProperty("Tag", QString(QString::fromStdString(default_values[i])), "", QString::fromStdString(parent->getNodeName())); 15 | addChild(newTag); 16 | connect(newTag, SIGNAL(tagModified()), parent, SLOT(nodePropertyUpdated())); 17 | } 18 | } 19 | 20 | void TagController::initialize() 21 | { 22 | 23 | std::stringstream ss; 24 | static int count = 0; 25 | ss << "TagController" << count++; 26 | } 27 | 28 | TagController::~TagController() 29 | { 30 | for (;numChildren() != 0;) { 31 | delete takeChildAt(0); 32 | } 33 | } 34 | 35 | QString TagController::formatClassId(const QString& class_id) 36 | { 37 | QStringList id_parts = class_id.split("/"); 38 | if(id_parts.size() != 2) 39 | { 40 | // Should never happen with pluginlib class ids, which are 41 | // formatted like "package_name/class_name". Not worth crashing 42 | // over though. 43 | return class_id; 44 | } 45 | else 46 | { 47 | return id_parts[ 1 ] + " (" + id_parts[ 0 ] + ")"; 48 | } 49 | } 50 | 51 | void TagController::emitConfigChanged() 52 | { 53 | Q_EMIT configChanged(); 54 | } 55 | 56 | void TagController::load(const rviz_common::Config& config) 57 | { 58 | // // Load the name by hand. 59 | // QString name; 60 | // if(config.mapGetString("Name", &name)) 61 | // { 62 | // setName(name); 63 | // } 64 | // // Load all sub-properties the same way the base class does. 65 | // rviz_common::properties::Property::load(config); 66 | } 67 | 68 | void TagController::save(rviz_common::Config config) const 69 | { 70 | // config.mapSetValue("Class", getClassId()); 71 | // config.mapSetValue("Name", getName()); 72 | 73 | // rviz_common::properties::Property::save(config); 74 | } 75 | 76 | } // end namespace topological_rviz_tools 77 | -------------------------------------------------------------------------------- /topological_rviz_tools/src/tag_property.cpp: -------------------------------------------------------------------------------- 1 | #include "topological_rviz_tools/tag_property.hpp" 2 | 3 | namespace topological_rviz_tools 4 | { 5 | 6 | TagProperty::TagProperty(const QString& name, 7 | const QString& default_value, 8 | const QString& description, 9 | const QString& node_name, 10 | Property* parent, 11 | const char *changed_slot, 12 | QObject* receiver) 13 | : rviz_common::properties::StringProperty(name, default_value, description, parent, changed_slot, receiver) 14 | , tag_value_(default_value.toStdString()) 15 | , reset_value_(false) 16 | , node_name_(node_name.toStdString()) 17 | { 18 | connect(this, SIGNAL(changed()), this, SLOT(updateTag())); 19 | ros::NodeHandle nh; 20 | tagUpdate_ = nh.serviceClient("/topological_map_manager/modify_node_tags", true); 21 | } 22 | 23 | void TagProperty::updateTag(){ 24 | if (reset_value_){ // this function gets called when we reset a value when the service call fails, so ignore that. 25 | reset_value_ = false; 26 | return; 27 | } 28 | 29 | topological_navigation_msgs::ModifyTag srv; 30 | srv.request.tag = tag_value_.c_str(); 31 | srv.request.new_tag = getString().toStdString().c_str(); 32 | srv.request.node.push_back(node_name_); 33 | 34 | if (tagUpdate_.call(srv)) { 35 | if (srv.response.success) { 36 | RCLCPP_INFO(logger_, "Successfully updated tag %s to %s", srv.request.tag.c_str(), srv.request.new_tag.c_str()); 37 | Q_EMIT tagModified(); 38 | tag_value_ = getString().toStdString(); 39 | } else { 40 | RCLCPP_INFO(logger_, "Failed to update tag %s: %s", srv.request.tag.c_str(), srv.response.meta.c_str()); 41 | reset_value_ = true; 42 | setValue(QString::fromStdString(tag_value_)); 43 | } 44 | } else { 45 | RCLCPP_WARN(logger_, "Failed to get response from service to update tag %s", srv.request.tag.c_str()); 46 | reset_value_ = true; 47 | setValue(QString::fromStdString(tag_value_)); 48 | } 49 | } 50 | 51 | TagProperty::~TagProperty() 52 | { 53 | } 54 | 55 | } // end namespace topological_rviz_tools 56 | -------------------------------------------------------------------------------- /topological_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(topological_utils) 3 | 4 | # Export information to downstream packages 5 | ament_export_dependencies(rosidl_default_runtime) 6 | ament_package() 7 | 8 | -------------------------------------------------------------------------------- /topological_utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_utils/__init__.py -------------------------------------------------------------------------------- /topological_utils/launch/create_topological_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /topological_utils/launch/dummy_topological_navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /topological_utils/launch/empty_topological_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /topological_utils/launch/mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /topological_utils/launch/topological_map_edition.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /topological_utils/launch/topological_prediction_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /topological_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | topological_utils 5 | 3.0.5 6 | The ros2 topological_utils package 7 | 8 | 9 | Marc Hanheide 10 | James R Heselden 11 | 12 | James R Heselden 13 | Jaime Pulido Fentanes 14 | Gautham P Das 15 | 16 | Apache License 2.0 17 | 18 | ament_copyright 19 | ament_flake8 20 | ament_pep257 21 | python3-pytest 22 | 23 | topological_navigation_msgs 24 | 25 | 26 | 27 | ament_python 28 | 29 | 30 | -------------------------------------------------------------------------------- /topological_utils/resource/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_utils/resource/__init__.py -------------------------------------------------------------------------------- /topological_utils/resource/topological_utils: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_utils/resource/topological_utils -------------------------------------------------------------------------------- /topological_utils/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/topological_utils 3 | [install] 4 | install_scripts=$base/lib/topological_utils 5 | -------------------------------------------------------------------------------- /topological_utils/support/empty.tmap: -------------------------------------------------------------------------------- 1 | node: 2 | ChargingPoint 3 | waypoint: 4 | 0,0,0,0,0,0,0 5 | edges: 6 | Station, undocking 7 | vertices: 8 | 0.250000, 0.690000 9 | -0.075000, 0.690000 10 | -0.400000, 0.287000 11 | -0.400000,-0.287000 12 | -0.075000,-0.690000 13 | 0.250000,-0.690000 14 | node: 15 | Station 16 | waypoint: 17 | -1.2,0.0,0.0,0,0,0.0,1 18 | edges: 19 | ChargingPoint, docking 20 | vertices: 21 | 0.69, 0.287 22 | 0.287, 0.69 23 | -0.287, 0.69 24 | -0.69, 0.287 25 | -0.69, -0.287 26 | -0.287, -0.69 27 | 0.287, -0.69 28 | 0.69, -0.287 29 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_utils/topological_utils/__init__.py -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/__init__,py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_utils/topological_utils/scripts/__init__,py -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LCAS/topological_navigation/826d3b5300a43b7c0e4f578f23b1ae2ee19b942a/topological_utils/topological_utils/scripts/__init__.py -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/add_content.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | import topological_navigation_msgs.srv 6 | 7 | 8 | def content_client(node, content): 9 | rospy.wait_for_service('/topological_map_manager/add_content_to_node') 10 | try: 11 | cont = rospy.ServiceProxy('/topological_map_manager/add_content_to_node', topological_navigation_msgs.srv.AddContent) 12 | resp1 = cont(node, content) 13 | return resp1 14 | except rospy.ServiceException as e: 15 | return "Service call failed: %s"%e 16 | 17 | 18 | if __name__ == "__main__": 19 | print(sys.argv) 20 | 21 | filename=str(sys.argv[1]) 22 | waypoint=str(sys.argv[2]) 23 | #map_name=str(sys.argv[3]) 24 | 25 | json_data=open(filename, 'rb').read() 26 | #text0 = "Hello ROS World" 27 | print("Sending %s"%(json_data)) 28 | resp=content_client(waypoint, json_data) 29 | print(resp) 30 | 31 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/add_edge.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | #import pymongo 6 | 7 | 8 | #from strands_navigation_msgs.msg import TopologicalNode 9 | #from ros_datacentre.message_store import MessageStoreProxy 10 | #from topological_navigation.topological_node import * 11 | from topological_navigation.topological_map import * 12 | 13 | #import topological_navigation.msg 14 | 15 | 16 | 17 | class topologicalEdgeAdd(object): 18 | 19 | def __init__(self, pointset, or_waypoint, de_waypoint, action) : 20 | 21 | self.topo_map = topological_map(pointset) 22 | self.topo_map.add_edge(or_waypoint, de_waypoint, action) 23 | rospy.loginfo("All Done ...") 24 | 25 | 26 | if __name__ == '__main__': 27 | pointset=str(sys.argv[1]) 28 | or_waypoint=str(sys.argv[2]) 29 | de_waypoint=str(sys.argv[3]) 30 | action = str(sys.argv[4]) 31 | rospy.init_node('add_edge') 32 | server = topologicalEdgeAdd(pointset,or_waypoint, de_waypoint, action) -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/add_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | #import pymongo 6 | 7 | import std_msgs.msg 8 | from geometry_msgs.msg import Pose 9 | 10 | #from strands_navigation_msgs.msg import TopologicalNode 11 | #from ros_datacentre.message_store import MessageStoreProxy 12 | #from topological_navigation.topological_node import * 13 | from topological_navigation.topological_map import * 14 | 15 | 16 | #import topological_navigation.msg 17 | 18 | 19 | 20 | class topologicalNodeAdd(object): 21 | 22 | def __init__(self, pointset, name, dist) : 23 | 24 | try: 25 | pos = rospy.wait_for_message('robot_pose', Pose, timeout=10.0) 26 | except rospy.ROSException : 27 | rospy.logwarn("Failed to get robot pose") 28 | return 29 | 30 | self.topo_map = topological_map(pointset) 31 | self.topo_map.add_node(name,dist, pos, 'move_base') 32 | 33 | map_update = rospy.Publisher('/update_map', std_msgs.msg.Time) 34 | map_update.publish(rospy.Time.now()) 35 | rospy.loginfo("All Done ...") 36 | 37 | 38 | if __name__ == '__main__': 39 | pointset=str(sys.argv[1]) 40 | name=str(sys.argv[2]) 41 | dist=float(sys.argv[3]) 42 | rospy.init_node('node_add') 43 | server = topologicalNodeAdd(pointset,name,dist) -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/add_node_tags.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Thu Sep 12 20:21:38 2019 5 | 6 | @author: gpdas 7 | 8 | """ 9 | 10 | import yaml 11 | import sys 12 | import os 13 | 14 | 15 | def load_data_from_yaml(file_path): 16 | assert os.path.exists(file_path) 17 | 18 | f_handle = open(file_path, "r") 19 | f_data = yaml.load(f_handle) 20 | f_handle.close() 21 | return f_data 22 | 23 | 24 | if __name__ == "__main__": 25 | if len(sys.argv) < 4: 26 | print("Usage: add_node_tags.py ") 27 | else: 28 | in_file = sys.argv[1] 29 | out_file = sys.argv[2] 30 | cfg_file = sys.argv[3] 31 | 32 | topo_map = load_data_from_yaml(in_file) 33 | 34 | tags = load_data_from_yaml(cfg_file) 35 | node_tag = {} 36 | for tag in tags: 37 | for node in tags[tag]: 38 | node_tag[node] = tag 39 | 40 | for node in topo_map: 41 | if node["meta"]["node"] in node_tag: 42 | if "tag" in node["meta"]: 43 | if node_tag[node["meta"]["node"]] not in node["meta"]["tag"]: 44 | node["meta"]["tag"].append(node_tag[node["meta"]["node"]]) 45 | else: 46 | node["meta"]["tag"] = [node_tag[node["meta"]["node"]]] 47 | 48 | with open(out_file, 'w') as f_out: 49 | yaml.dump(topo_map, f_out, default_flow_style=False) 50 | print("Successfully created the new file with node tags") 51 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/check_map: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | """ 4 | Outputs a list of available topological maps 5 | """ 6 | 7 | from topological_utils.queries import get_maps 8 | from strands_navigation_msgs.msg import TopologicalNode 9 | from mongodb_store.message_store import MessageStoreProxy 10 | import sys 11 | 12 | if __name__ == "__main__": 13 | if len(sys.argv)!=2: 14 | print "Usage: check_map map_name" 15 | sys.exit(1) 16 | map_name = sys.argv[1] 17 | print "Checking map '%s'" % map_name 18 | maps = get_maps() 19 | if map_name not in maps: 20 | print "!!! MAP DOES NOT EXIST !!!" 21 | sys.exit(1) 22 | map_details = maps[map_name] 23 | print "-" * 50 24 | print "Name: ", map_name 25 | print "Number of nodes: ", map_details["number_nodes"] 26 | print "Edge actions: ", str(list(map_details["edge_actions"])) 27 | print "Last modified: ", map_details["last_modified"] 28 | print "-" * 50 29 | 30 | msg_store = MessageStoreProxy(collection='topological_maps') 31 | 32 | nodes = zip(*msg_store.query(TopologicalNode._type, {}, {'pointset':map_name}))[0] 33 | node_names = [node.name for node in nodes] 34 | 35 | 36 | 37 | print "Checking consistency..." 38 | valid = True 39 | 40 | count = map(lambda x: node_names.count(x), node_names) 41 | duplicates = [node_names[i] for i, cnt in enumerate(count) if cnt > 1] 42 | if len(duplicates) !=0: 43 | print " !!! Duplicated nodes: %s" % ", ".join(set(duplicates)) 44 | 45 | edge_ids = [] 46 | for node in nodes: 47 | for edge in node.edges: 48 | if edge.node not in node_names: 49 | print " !!! Node '%s' has an edge to non-existent node '%s'" % (node.name, edge.node) 50 | valid = False 51 | if len(edge.edge_id) < 1: 52 | print " !!! Node '%s' has an edge with a invalid blank id field" % node.name 53 | valid = False 54 | elif edge.edge_id in edge_ids: 55 | print " !!! Node '%s' has an edge with a duplicate edge id '%s'." % (node.name, edge.edge_id) 56 | valid = False 57 | else: 58 | edge_ids.append(edge.edge_id) 59 | if not valid: 60 | print "FAILED." 61 | else: 62 | print "PASSED." 63 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/crop_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import yaml 5 | from PIL import Image 6 | import math 7 | 8 | def find_bounds(map_image): 9 | x_min = map_image.size[0] 10 | x_end = 0 11 | y_min = map_image.size[1] 12 | y_end = 0 13 | pix = map_image.load() 14 | for x in range(map_image.size[0]): 15 | for y in range(map_image.size[1]): 16 | val = pix[x, y] 17 | if val != 205: # not unknown 18 | x_min = min(x, x_min) 19 | x_end = max(x, x_end) 20 | y_min = min(y, y_min) 21 | y_end = max(y, y_end) 22 | return x_min, x_end, y_min, y_end 23 | 24 | def computed_cropped_origin(map_image, bounds, resolution, origin): 25 | """ Compute the image for the cropped map when map_image is cropped by bounds and had origin before. """ 26 | ox = origin[0] 27 | oy = origin[1] 28 | oth = origin[2] 29 | 30 | # First figure out the delta we have to translate from the lower left corner (which is the origin) 31 | # in the image system 32 | dx = bounds[0] * resolution 33 | dy = (map_image.size[1] - bounds[3]) * resolution 34 | 35 | # Next rotate this by the theta and add to the old origin 36 | 37 | new_ox = ox + dx * math.cos(oth) - dy * math.sin(oth) 38 | new_oy = oy + dx * math.sin(oth) + dy * math.cos(oth) 39 | 40 | return [new_ox, new_oy, oth] 41 | 42 | if __name__ == "__main__": 43 | if len(sys.argv) < 2: 44 | print >> sys.stderr, "Usage: %s map.yaml [cropped.yaml]" % sys.argv[0] 45 | sys.exit(1) 46 | 47 | with open(sys.argv[1]) as f: 48 | map_data = yaml.safe_load(f) 49 | 50 | if len(sys.argv) > 2: 51 | crop_name = sys.argv[2] 52 | if crop_name.endswith(".yaml"): 53 | crop_name = crop_name[:-5] 54 | crop_yaml = crop_name + ".yaml" 55 | crop_image = crop_name + ".pgm" 56 | else: 57 | crop_yaml = "cropped.yaml" 58 | crop_image = "cropped.pgm" 59 | 60 | map_image_file = map_data["image"] 61 | resolution = map_data["resolution"] 62 | origin = map_data["origin"] 63 | 64 | map_image = Image.open(map_image_file) 65 | 66 | bounds = find_bounds(map_image) 67 | 68 | # left, upper, right, lower 69 | cropped_image = map_image.crop((bounds[0], bounds[2], bounds[1] + 1, bounds[3] + 1)) 70 | 71 | cropped_image.save(crop_image) 72 | map_data["image"] = crop_image 73 | map_data["origin"] = computed_cropped_origin(map_image, bounds, resolution, origin) 74 | with open(crop_yaml, "w") as f: 75 | yaml.dump(map_data, f) 76 | 77 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/edge_reconf_groups_to_tmap2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Created on Tue Jul 6 13:07:18 2021 4 | 5 | @author: Adam Binch (abinch@sagarobotics.com) 6 | """ 7 | ######################################################################################################### 8 | import sys, rospy, yaml 9 | from topological_navigation.manager2 import map_manager_2 10 | 11 | 12 | def load_yaml(filename): 13 | with open(filename,'r') as f: 14 | return yaml.load(f) 15 | 16 | 17 | def edge_groups_to_tmap2(f_tmap2, groups, group_names): 18 | 19 | mm2 = map_manager_2(advertise_srvs=False) 20 | mm2.init_map(filename=f_tmap2) 21 | 22 | for name in group_names: 23 | rospy.loginfo("Adding parameters for edge group {}".format(name)) 24 | group = groups["edge_nav_reconfig_groups"][name] 25 | 26 | for edge_id in group["edges"]: 27 | for param in group["parameters"]: 28 | if not isinstance(param["value"], str): 29 | value = str(param["value"]) 30 | value_is_string = False 31 | else: 32 | value = param["value"] 33 | value_is_string = True 34 | 35 | mm2.add_param_to_edge_config(edge_id, param["ns"], param["name"], value, value_is_string, update=False, write_map=False) 36 | 37 | mm2.write_topological_map(f_tmap2) 38 | ######################################################################################################### 39 | 40 | 41 | ######################################################################################################### 42 | if __name__ == '__main__' : 43 | 44 | if "-h" in sys.argv or "--help" in sys.argv or len(sys.argv) < 4: 45 | print "usage is edge_reconf_groups_to_tmap2.py tmap.tmap2 edge_reconf_groups.yaml ['group_1', 'group_2',...,'group_n']" 46 | sys.exit(1) 47 | else: 48 | print sys.argv 49 | f_tmap2 = sys.argv[1] 50 | f_groups = sys.argv[2] 51 | group_names = sys.argv[3].strip('[]').split(',') 52 | 53 | print("Populating topological map 2 '{}' with parameters from edge reconfigure groups '{}'".format(f_tmap2, f_groups)) 54 | 55 | rospy.init_node("edge_reconf_groups_to_tmap2") 56 | 57 | groups = load_yaml(f_groups) 58 | edge_groups_to_tmap2(f_tmap2, groups, group_names) 59 | ######################################################################################################### -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/goal_converter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Created on Thu Apr 29 10:04:30 2021 4 | 5 | @author: Adam Binch (abinch@sagarobotics.com) 6 | """ 7 | ######################################################################################################### 8 | import sys, os, traceback, rospy 9 | 10 | from topological_navigation.manager2 import map_manager_2 11 | 12 | 13 | def convert_goal(root_dir, action, action_type): 14 | 15 | tmap_files = [] 16 | for root, dirs, files in os.walk(root_dir): 17 | for _file in files: 18 | if _file.endswith(".tmap2"): 19 | tmap_files.append(os.path.join(root, _file)) 20 | 21 | mm = map_manager_2(advertise_srvs=False) 22 | for f in tmap_files: 23 | rospy.loginfo("CHANGING THE GOAL IN " + f) 24 | 25 | try: 26 | mm.init_map(filename=f) 27 | mm.update_action(action, action_type, "", update=False, write_map=False) 28 | mm.write_topological_map(f) 29 | except Exception: 30 | rospy.logerr("Unable to convert goal in {}\n{}".format(f, traceback.format_exc())) 31 | sys.exit(1) 32 | 33 | rospy.sleep(1.0) 34 | ######################################################################################################### 35 | 36 | 37 | ######################################################################################################### 38 | if __name__ == '__main__' : 39 | 40 | if "-h" in sys.argv or "--help" in sys.argv or len(sys.argv) < 4: 41 | print("usage is goal_converter.py root_directory action action_type") 42 | sys.exit(1) 43 | else: 44 | root_dir = sys.argv[1] 45 | action = sys.argv[2] 46 | action_type = sys.argv[3] 47 | 48 | print("Converting the goal type of {} action to {} for topological maps found in {}".format(action, action_type, root_dir)) 49 | 50 | rospy.init_node("topological_goal_converter") 51 | convert_goal(root_dir, action, action_type) 52 | ######################################################################################################### 53 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/insert_empty_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import rospy 4 | import yaml 5 | from topological_navigation.load_maps_from_yaml import YamlMapLoader 6 | from mongodb_store.message_store import MessageStoreProxy 7 | from topological_navigation_msgs.msg import TopologicalNode 8 | from topological_navigation_msgs.srv import AddNode 9 | from geometry_msgs.msg import Pose 10 | 11 | if __name__ == '__main__': 12 | rospy.init_node('insert_empty_map') 13 | 14 | if len(sys.argv) != 2: 15 | rospy.logwarn("usage: insert_empty_map.py pointset_name") 16 | sys.exit(1) 17 | 18 | pointset = sys.argv[1] 19 | map_loader = YamlMapLoader() 20 | if pointset in map_loader.get_maps(): 21 | rospy.logwarn("Map with name {0} already exists. Try another one.".format(sys.argv[1])) 22 | sys.exit(1) 23 | 24 | msg_store = MessageStoreProxy(collection='topological_maps') 25 | 26 | empty_node = TopologicalNode() 27 | empty_node.name = "temp_node" 28 | empty_node.map = "unused" 29 | empty_node.pointset = pointset 30 | 31 | meta = {} 32 | meta['pointset'] = pointset 33 | meta['map'] = "unused" 34 | meta['node'] = "temp_node" 35 | 36 | msg_store.insert(empty_node, meta) 37 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/joy_add_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | 6 | import std_msgs.msg 7 | import sensor_msgs.msg 8 | from geometry_msgs.msg import Pose 9 | 10 | #from strands_navigation_msgs.msg import TopologicalNode 11 | #from ros_datacentre.message_store import MessageStoreProxy 12 | #from topological_navigation.topological_node import * 13 | from topological_navigation_msgs.msg import TopologicalMap 14 | from topological_navigation.topological_map import * 15 | 16 | 17 | #import topological_navigation.msg 18 | 19 | 20 | 21 | class topologicalNodeAdd(object): 22 | 23 | def __init__(self, base_name, dist) : 24 | self.map_received = False 25 | rospy.Subscriber('joy', sensor_msgs.msg.Joy, self.JoyCallback) 26 | rospy.Subscriber('topological_map', TopologicalMap, self.MapCallback) 27 | 28 | self.button = rospy.get_param('~button', 1) 29 | 30 | rospy.loginfo("Waiting for Topological map ...") 31 | while not self.map_received : 32 | pass 33 | 34 | rospy.loginfo("All Done ...") 35 | rospy.spin() 36 | 37 | 38 | def JoyCallback(self, msg) : 39 | if msg.buttons[self.button]: 40 | #print "JOY" 41 | self.add_node() 42 | 43 | def MapCallback(self, msg) : 44 | self.topo_map = topological_map(msg.name, msg=msg) 45 | self.map_received =True 46 | 47 | 48 | def add_node(self) : 49 | 50 | try: 51 | current = rospy.wait_for_message('current_node', std_msgs.msg.String, timeout=10.0) 52 | except rospy.ROSException : 53 | rospy.logwarn("Failed to get current node") 54 | return 55 | 56 | print current.data 57 | if current.data == 'none': 58 | try: 59 | pos = rospy.wait_for_message('robot_pose', Pose, timeout=10.0) 60 | except rospy.ROSException : 61 | rospy.logwarn("Failed to get robot pose") 62 | return 63 | 64 | lnames=[] 65 | for i in self.topo_map.nodes : 66 | if i.name.startswith('WayPoint') : 67 | nam = i.name.strip('WayPoint') 68 | lnames.append(int(nam)) 69 | 70 | lnames.sort() 71 | print "used names:" 72 | for j in lnames : 73 | print j 74 | 75 | print "Chosen name!!!!!!" 76 | if lnames: 77 | nodname = 'WayPoint%d'%(int(lnames[-1])+1) 78 | else : 79 | nodname = 'WayPoint1' 80 | print nodname 81 | #self.topo_map = topological_map(pointset) 82 | self.topo_map.add_node(nodname,8.0, pos, 'move_base') 83 | 84 | map_update = rospy.Publisher('/update_map', std_msgs.msg.Time) 85 | map_update.publish(rospy.Time.now()) 86 | 87 | else: 88 | rospy.loginfo("I will NOT add a node within the influence area of another! Solve this and try again!") 89 | 90 | 91 | 92 | if __name__ == '__main__': 93 | # pointset=str(sys.argv[1]) 94 | # name=str(sys.argv[2]) 95 | # dist=float(sys.argv[3]) 96 | name = 'WayPoint' 97 | dist = 10.0 98 | rospy.init_node('node_add') 99 | server = topologicalNodeAdd(name,dist) -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/joy_add_waypoint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | 6 | import std_msgs.msg 7 | import sensor_msgs.msg 8 | from geometry_msgs.msg import Pose 9 | 10 | #from strands_navigation_msgs.msg import TopologicalNode 11 | #from ros_datacentre.message_store import MessageStoreProxy 12 | #from topological_navigation.topological_node import * 13 | from topological_navigation_msgs.msg import TopologicalMap 14 | from topological_navigation.topological_map import * 15 | 16 | 17 | #import topological_navigation.msg 18 | 19 | 20 | 21 | class addWaypoint(object): 22 | 23 | def __init__(self, filename) : 24 | rospy.Subscriber('joy', sensor_msgs.msg.Joy, self.JoyCallback) 25 | self.filename = filename 26 | #fh = open(filename, "a") 27 | #fh.close 28 | rospy.loginfo("All Done ...") 29 | rospy.spin() 30 | 31 | 32 | def JoyCallback(self, msg) : 33 | if msg.buttons[1]: 34 | print "JOY" 35 | self.add_waypoint() 36 | 37 | 38 | def add_waypoint(self) : 39 | 40 | try: 41 | p = rospy.wait_for_message('robot_pose', Pose, timeout=10.0) 42 | except rospy.ROSException : 43 | rospy.logwarn("Failed to get robot pose") 44 | return 45 | 46 | fh = open(filename, "a") 47 | 48 | s_output = "%s,%s,%s,%s,%s,%s,%s\n" %(str(p.position.x),str(p.position.y),str(p.position.z), 49 | str(p.orientation.x),str(p.orientation.y),str(p.orientation.z),str(p.orientation.w)) 50 | fh.write(s_output) 51 | fh.close 52 | 53 | 54 | 55 | 56 | if __name__ == '__main__': 57 | filename=str(sys.argv[1]) 58 | # name=str(sys.argv[2]) 59 | # dist=float(sys.argv[3]) 60 | name = 'WayPoint' 61 | dist = 10.0 62 | rospy.init_node('node_add') 63 | server = addWaypoint(filename) -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/list_maps: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | """ 4 | Outputs a list of available topological maps 5 | """ 6 | 7 | from topological_utils.queries import get_maps 8 | 9 | if __name__ == "__main__": 10 | for map_name,details in get_maps().items(): 11 | print "-" * 50 12 | print "Name: ", map_name 13 | print "Number of nodes: ", details["number_nodes"] 14 | print "Edge actions: ", str(list(details["edge_actions"])) 15 | print "Last modified: ", details["last_modified"] 16 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/load_json_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import json 4 | import yaml 5 | import sys 6 | 7 | from topological_navigation_msgs.msg import TopologicalNode 8 | import mongodb_store.util as dc_util 9 | from mongodb_store.message_store import MessageStoreProxy 10 | 11 | 12 | if __name__ == '__main__': 13 | if len(sys.argv) < 2 : 14 | print "usage: insert_map input_file.txt" 15 | sys.exit(2) 16 | 17 | filename=str(sys.argv[1]) 18 | #dataset_name=str(sys.argv[2]) 19 | #map_name=str(sys.argv[3]) 20 | 21 | msg_store = MessageStoreProxy(collection='topological_maps') 22 | 23 | json_data=open(filename, 'rb').read() 24 | 25 | data = json.loads(json_data) 26 | 27 | for i in data: 28 | meta = i['meta'] 29 | msgv = dc_util.dictionary_to_message(i['node'], TopologicalNode) 30 | msg_store.insert(msgv,meta) 31 | #mongodb_store.util.store_message(points_db,p,val) 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/load_yaml_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import yaml 5 | import argparse 6 | 7 | from topological_navigation.load_maps_from_yaml import YamlMapLoader 8 | from std_srvs.srv import Empty 9 | 10 | 11 | def load_yaml(filename): 12 | rospy.loginfo("loading %s"%filename) 13 | with open(filename, 'r') as f: 14 | return yaml.load(f) 15 | 16 | if __name__ == '__main__': 17 | parser = argparse.ArgumentParser() 18 | parser.add_argument("yaml_map", help="The yaml file or directory containing yaml files to insert into the datacentre", type=str) 19 | 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) 20 | parser.add_argument("-f", "--force", help="Override maps with the same name in the datacentre", action="store_true") 21 | 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") 22 | args, unknown = parser.parse_known_args() # Necessary due to roslaunch injecting rubbish arguments 23 | 24 | rospy.init_node('load_yaml_map') 25 | y = YamlMapLoader() 26 | 27 | data = y.read_maps(args.yaml_map) 28 | y.insert_maps(data=data, new_pointset=args.pointset, force=args.force) 29 | 30 | if args.keep_alive: 31 | rospy.Service("~loaded", Empty, None) 32 | rospy.spin() 33 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/map_collection_change.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import json 4 | import sys 5 | import rospy 6 | from geometry_msgs.msg import Pose 7 | from topological_navigation_msgs.msg import TopologicalNode, Vertex, Edge 8 | 9 | import pymongo 10 | #import mongodb_store.util 11 | from mongodb_store.message_store import MessageStoreProxy 12 | 13 | 14 | 15 | 16 | 17 | if __name__ == '__main__': 18 | #if len(sys.argv) < 4 : 19 | # print "usage: insert_map input_file.txt dataset_name map_name" 20 | # sys.exit(2) 21 | 22 | #filename=str(sys.argv[1]) 23 | #dataset_name=str(sys.argv[2]) 24 | #map_name=str(sys.argv[3]) 25 | 26 | msg_store = MessageStoreProxy() 27 | msg_store_maps = MessageStoreProxy(collection='topological_maps') 28 | 29 | 30 | query_meta = {} 31 | query_meta["stored_type"] = "topological_navigation_msgs/TopologicalNode" 32 | 33 | available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 34 | 35 | #print available 36 | 37 | if available <= 0 : 38 | #rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") 39 | #rospy.logerr("Available pointsets: "+str(available)) 40 | raise Exception("Can't find waypoints.") 41 | 42 | else : 43 | message_list = msg_store.query(TopologicalNode._type, {}, query_meta) 44 | for i in message_list: 45 | #print i 46 | meta = {} 47 | meta["node"] = i[0].name 48 | meta["map"] = i[0].map 49 | meta["pointset"] = i[0].pointset 50 | available = len(msg_store_maps.query(TopologicalNode._type, {}, meta)) 51 | if available == 0 : 52 | msg_store_maps.insert(i[0],meta) 53 | else : 54 | rospy.logerr("this point is already in datacentre:") 55 | print meta -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/map_converter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Created on Thu Apr 29 10:04:30 2021 4 | 5 | @author: Adam Binch (abinch@sagarobotics.com) 6 | """ 7 | ######################################################################################################### 8 | import sys, os, traceback 9 | import rospy 10 | 11 | from topological_navigation.manager import map_manager 12 | 13 | 14 | def convert_tmaps(root_dir, ext_old, ext_new): 15 | 16 | tmap_files = [] 17 | for root, dirs, files in os.walk(root_dir): 18 | for _file in files: 19 | if _file.endswith(ext_old): 20 | tmap_files.append(os.path.join(root, _file)) 21 | 22 | mm = map_manager() 23 | for f in tmap_files: 24 | rospy.loginfo("CONVERTING " + f) 25 | 26 | try: 27 | mm.init_map(f, load_from_file=True) 28 | mm.manager2.write_topological_map(os.path.splitext(f)[0] + ext_new) 29 | except Exception: 30 | rospy.logerr("Unable to convert {}\n{}".format(f, traceback.format_exc())) 31 | 32 | rospy.sleep(1.0) 33 | ######################################################################################################### 34 | 35 | 36 | ######################################################################################################### 37 | if __name__ == '__main__' : 38 | 39 | if "-h" in sys.argv or "--help" in sys.argv or len(sys.argv) < 4: 40 | print("usage is map_converter.py root_directory old_topomap_ext new_topomap_ext") 41 | sys.exit(1) 42 | else: 43 | root_dir = sys.argv[1] 44 | ext_old = sys.argv[2] 45 | ext_new = sys.argv[3] 46 | 47 | print("Converting topological maps with ext {} found in {} to new format with ext {}".format(ext_old, root_dir, ext_new)) 48 | 49 | rospy.init_node("topological_map_converter") 50 | convert_tmaps(root_dir, ext_old, ext_new) 51 | ######################################################################################################### -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/map_to_json.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | import actionlib 6 | import pymongo 7 | import json 8 | import bson 9 | import sys 10 | import math 11 | import yaml 12 | import pickle 13 | 14 | from topological_navigation.topological_node import * 15 | from actionlib_msgs.msg import * 16 | from move_base_msgs.msg import * 17 | from geometry_msgs.msg import Pose 18 | from std_msgs.msg import String 19 | 20 | from topological_navigation_msgs.msg import TopologicalNode 21 | 22 | from mongodb_store.message_store import MessageStoreProxy 23 | import topological_navigation.msg 24 | 25 | 26 | 27 | 28 | class MapExport(object): 29 | 30 | def __init__(self, dataset_name, filename) : 31 | #print "loading file from map %s" %filename 32 | self.lnodes = self.loadMap(dataset_name, filename) 33 | 34 | 35 | def loadMap(self, point_set, filename): 36 | 37 | point_set=str(sys.argv[1]) 38 | #map_name=str(sys.argv[3]) 39 | 40 | msg_store = MessageStoreProxy(collection='topological_maps') 41 | 42 | query_meta = {} 43 | query_meta["pointset"] = point_set 44 | 45 | available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 46 | 47 | print available 48 | 49 | if available <= 0 : 50 | rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") 51 | rospy.logerr("Available pointsets: "+str(available)) 52 | raise Exception("Can't find waypoints.") 53 | 54 | else : 55 | query_meta = {} 56 | query_meta["pointset"] = point_set 57 | 58 | message_list = msg_store.query(TopologicalNode._type, {}, query_meta) 59 | 60 | #node=TopologicalNode() 61 | 62 | top_map=[] 63 | for i in message_list: 64 | nodeinf = {} 65 | nodeinf["node"] = yaml.load(str(i[0])) 66 | nodeinf["node"]["localise_by_topic"] = json.dumps(nodeinf["node"]["localise_by_topic"]) 67 | nodeinf["meta"] = i[1] #str(bson.json_util.dumps(i[1], indent=1)) 68 | 69 | nodeinf["meta"].pop("last_updated_by", None) 70 | nodeinf["meta"].pop('inserted_at', None) 71 | nodeinf["meta"].pop('last_updated_at', None) 72 | nodeinf["meta"].pop('stored_type', None) 73 | nodeinf["meta"].pop('stored_class', None) 74 | nodeinf["meta"].pop('inserted_by', None) 75 | nodeinf["meta"].pop('_id', None) 76 | top_map.append(nodeinf) 77 | #val = bson.json_util.dumps(nodeinf["meta"], indent=1) 78 | 79 | fh = open(filename, "w") 80 | #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) 81 | s_output = str(bson.json_util.dumps(top_map, indent=1, sort_keys=True) ) 82 | #print s_output 83 | fh.write(s_output) 84 | fh.close 85 | 86 | 87 | if __name__ == '__main__': 88 | if len(sys.argv) < 3 : 89 | print "usage: map_to_json dataset_name output_file.json" 90 | sys.exit(2) 91 | 92 | dataset_name=str(sys.argv[1]) 93 | filename=str(sys.argv[2]) 94 | rospy.init_node('topological_map_exporter') 95 | server = MapExport(dataset_name, filename) 96 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/map_to_yaml.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | import json 6 | import sys 7 | import yaml 8 | #import pickle 9 | 10 | from topological_navigation.topological_node import * 11 | from actionlib_msgs.msg import * 12 | 13 | from geometry_msgs.msg import Pose 14 | from std_msgs.msg import String 15 | 16 | from topological_navigation_msgs.msg import TopologicalNode 17 | 18 | from mongodb_store.message_store import MessageStoreProxy 19 | import topological_navigation.msg 20 | 21 | 22 | 23 | 24 | class MapExport(object): 25 | 26 | def __init__(self, dataset_name, filename) : 27 | #print "loading file from map %s" %filename 28 | self.lnodes = self.loadMap(dataset_name, filename) 29 | 30 | 31 | def loadMap(self, point_set, filename): 32 | 33 | point_set=str(sys.argv[1]) 34 | #map_name=str(sys.argv[3]) 35 | 36 | msg_store = MessageStoreProxy(collection='topological_maps') 37 | 38 | query_meta = {} 39 | query_meta["pointset"] = point_set 40 | 41 | available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 42 | 43 | print available 44 | 45 | if available <= 0 : 46 | rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") 47 | rospy.logerr("Available pointsets: "+str(available)) 48 | raise Exception("Can't find waypoints.") 49 | 50 | else : 51 | query_meta = {} 52 | query_meta["pointset"] = point_set 53 | 54 | message_list = msg_store.query(TopologicalNode._type, {}, query_meta) 55 | 56 | #node=TopologicalNode() 57 | 58 | top_map=[] 59 | for i in message_list: 60 | nodeinf = {} 61 | nodeinf["node"] = yaml.load(str(i[0])) 62 | if nodeinf["node"]["localise_by_topic"]: 63 | nodeinf["node"]["localise_by_topic"] = json.dumps(nodeinf["node"]["localise_by_topic"]) 64 | nodeinf["meta"] = i[1] #str(bson.json_util.dumps(i[1], indent=1)) 65 | nodeinf["meta"].pop("last_updated_by", None) 66 | nodeinf["meta"].pop('inserted_at', None) 67 | nodeinf["meta"].pop('last_updated_at', None) 68 | nodeinf["meta"].pop('stored_type', None) 69 | nodeinf["meta"].pop('stored_class', None) 70 | nodeinf["meta"].pop('inserted_by', None) 71 | nodeinf["meta"].pop('_id', None) 72 | top_map.append(nodeinf) 73 | #val = bson.json_util.dumps(nodeinf["meta"], indent=1) 74 | 75 | 76 | 77 | top_map.sort(key=lambda x: x['node']['name']) 78 | yml = yaml.safe_dump(top_map, default_flow_style=False) 79 | #print yml 80 | #print s_output 81 | 82 | fh = open(filename, "w") 83 | #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) 84 | s_output = str(yml) 85 | #print s_output 86 | fh.write(s_output) 87 | fh.close 88 | 89 | # fh = open(filename, "w") 90 | # #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) 91 | # s_output = str(bson.json_util.dumps(top_map, indent=1, sort_keys=True) ) 92 | # print s_output 93 | # fh.write(s_output) 94 | # fh.close 95 | 96 | 97 | if __name__ == '__main__': 98 | if len(sys.argv) < 3 : 99 | print "usage: map_to_yaml dataset_name output_file.yaml" 100 | sys.exit(2) 101 | 102 | dataset_name=str(sys.argv[1]) 103 | filename=str(sys.argv[2]) 104 | rospy.init_node('topological_map_exporter') 105 | server = MapExport(dataset_name, filename) 106 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/node_metadata.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import json 4 | import sys 5 | import rospy 6 | 7 | 8 | from topological_navigation.topological_node import * 9 | from topological_navigation_msgs.msg import TopologicalNode 10 | from mongodb_store.message_store import MessageStoreProxy 11 | 12 | from topological_utils.queries import get_nodes 13 | from topological_utils.srv import * 14 | 15 | def handle_query_nodes(req): 16 | print "Queried for %s %s"%(req.pointset, req.meta_category) 17 | 18 | resp = NodeMetadataResponse() 19 | resp.name=[] 20 | resp.description = [] 21 | resp.goto_node=[] 22 | resp.node_type = [] 23 | 24 | nodes = get_nodes(req.pointset, req.meta_category) 25 | 26 | for node in nodes: 27 | for gui_node in node[1]["contains"]: 28 | if (gui_node["category"] == req.meta_category) : 29 | resp.name.append(gui_node["name"]) 30 | resp.node_type.append(gui_node["category"]) 31 | resp.goto_node.append(node[1]["node"]) 32 | if gui_node.has_key("description"): 33 | resp.description.append(gui_node["description"]) 34 | else: 35 | resp.description.append("") 36 | 37 | if gui_node.has_key("at_node"): 38 | resp.at_node.append(gui_node["at_node"]) 39 | else: 40 | resp.at_node.append(False) 41 | 42 | print "Returning ",len(resp.name)," nodes for the meta_query ",req.meta_category 43 | return resp; 44 | 45 | def query_nodes_server(): 46 | rospy.init_node('query_nodes_server') 47 | s = rospy.Service('query_node_metadata', NodeMetadata, handle_query_nodes) 48 | print "Ready to query the database for topological nodes." 49 | rospy.spin() 50 | 51 | if __name__ == '__main__': 52 | query_nodes_server() 53 | 54 | 55 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/node_rm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | import pymongo 6 | 7 | import std_msgs.msg 8 | #from strands_navigation_msgs.msg import TopologicalNode 9 | #from ros_datacentre.message_store import MessageStoreProxy 10 | #from topological_navigation.topological_node import * 11 | from topological_navigation.topological_map import * 12 | 13 | #import topological_navigation.msg 14 | 15 | 16 | 17 | class topologicalNodeRM(object): 18 | 19 | def __init__(self, pointset, waypoint) : 20 | 21 | self.topo_map = topological_map(pointset) 22 | self.topo_map.remove_node(waypoint) 23 | 24 | 25 | map_update = rospy.Publisher('/update_map', std_msgs.msg.Time) 26 | map_update.publish(rospy.Time.now()) 27 | 28 | rospy.loginfo("All Done ...") 29 | 30 | 31 | if __name__ == '__main__': 32 | pointset=str(sys.argv[1]) 33 | waypoint=str(sys.argv[2]) 34 | rospy.init_node('node_rm') 35 | server = topologicalNodeRM(pointset,waypoint) -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/print_nav_stats.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from topological_navigation_msgs.msg import TopologicalMap, NavStatistics, NavRoute 5 | from mongodb_store.message_store import MessageStoreProxy 6 | #from strands_executive_msgs.msg import Task 7 | #from strands_executive_msgs.srv import AddTasks 8 | #from strands_executive_msgs import task_utils 9 | 10 | class NavRelaxant(object): 11 | def __init__(self, count_threshold): 12 | super(NavRelaxant, self).__init__() 13 | self.node_pairs = [] 14 | rospy.Subscriber('topological_map', TopologicalMap, self.map_callback) 15 | self.msg_store = MessageStoreProxy(collection='nav_stats') 16 | 17 | 18 | def map_callback(self, msg): 19 | node_pairs = [] 20 | for node in msg.nodes: 21 | for edge in node.edges: 22 | node_pairs.append((node.name, edge.node, edge.edge_id)) 23 | self.node_pairs = node_pairs 24 | 25 | def print_pair(self, start, end): 26 | count = len(self.msg_store.query(NavStatistics._type, {"origin": start, "target": end, "final_node": end})) 27 | rospy.loginfo('Nav stats from %s to %s: %s' % (start, end, count)) 28 | 29 | def print_nav_stats(self): 30 | 31 | # only really needed for testing 32 | while len(self.node_pairs) == 0 and not rospy.is_shutdown(): 33 | rospy.sleep(1) 34 | rospy.loginfo('Waiting for nodes') 35 | 36 | 37 | for (start, end, edge_id) in self.node_pairs: 38 | self.print_pair(start, end) 39 | 40 | if __name__ == '__main__': 41 | rospy.init_node('nav_relaxant') 42 | relaxant = NavRelaxant(count_threshold=rospy.get_param('~count_threshold', 5)) 43 | relaxant.print_nav_stats() 44 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/remove_node_tags.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Thu Sep 12 20:21:38 2019 5 | 6 | @author: gpdas 7 | 8 | """ 9 | 10 | 11 | import yaml 12 | import sys 13 | import os 14 | 15 | 16 | def load_data_from_yaml(file_path): 17 | assert os.path.exists(file_path) 18 | 19 | f_handle = open(file_path, "r") 20 | f_data = yaml.load(f_handle) 21 | f_handle.close() 22 | return f_data 23 | 24 | if __name__ == "__main__": 25 | if len(sys.argv) < 4: 26 | print ("Usage: remove_node_tags.py ") 27 | else: 28 | in_file = sys.argv[1] 29 | out_file = sys.argv[2] 30 | cfg_file = sys.argv[3] 31 | 32 | topo_map = load_data_from_yaml(in_file) 33 | 34 | tags = load_data_from_yaml(cfg_file) 35 | node_tag = {} 36 | for tag in tags: 37 | for node in tags[tag]: 38 | node_tag[node] = tag 39 | 40 | for node in topo_map: 41 | if node["meta"]["node"] in node_tag: 42 | if "tag" in node["meta"]: 43 | if node_tag[node["meta"]["node"]] in node["meta"]["tag"]: 44 | node["meta"]["tag"].remove(node_tag[node["meta"]["node"]]) 45 | if not node["meta"]["tag"]: 46 | node["meta"].pop("tag") 47 | 48 | with open(out_file, 'w') as f_out: 49 | yaml.dump(topo_map, f_out, default_flow_style=False) 50 | print("Successfully created the new file removing configured node tags") 51 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/rename_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | """ 4 | Outputs a list of available topological maps 5 | """ 6 | 7 | from topological_utils.nodes import rename_node 8 | import sys 9 | 10 | if __name__ == "__main__": 11 | if len(sys.argv) != 4: 12 | print "Usage: rename_node current_node_name new_node_name tological_map_name)" 13 | sys.exit(1) 14 | old_name, new_name, map_name = sys.argv[1:] 15 | print "Changing %s[%s] to %s"%(map_name, old_name, new_name) 16 | try: 17 | n, e = rename_node(old_name, new_name, map_name) 18 | except Exception, e: 19 | print "Failed: ", e.message 20 | sys.exit(1) 21 | print "Changed ", n, " nodes." 22 | print "Changed ", e, " edges." 23 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/rm_map_from_db.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | import pymongo 6 | 7 | 8 | from topological_navigation.topological_map import * 9 | 10 | 11 | 12 | class topologicalNodeRM(object): 13 | 14 | def __init__(self, pointset) : 15 | 16 | self.topo_map = topological_map(pointset) 17 | self.topo_map.delete_map() 18 | rospy.loginfo("All Done ...") 19 | 20 | 21 | if __name__ == '__main__': 22 | pointset=str(sys.argv[1]) 23 | rospy.init_node('map_rm') 24 | server = topologicalNodeRM(pointset) -------------------------------------------------------------------------------- /topological_utils/topological_utils/scripts/topological_map_update.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | import rospy 4 | import sys 5 | 6 | from time import sleep 7 | import std_msgs.msg 8 | 9 | 10 | if __name__ == '__main__' : 11 | rospy.init_node("topological_map_update") 12 | rospy.loginfo("Publishing update request for topological map ...") 13 | map_update = rospy.Publisher('/update_map', std_msgs.msg.Time) 14 | 15 | rospy.sleep(rospy.Duration.from_sec(1)) 16 | tmstp = std_msgs.msg.Time() 17 | tmstp.data = rospy.Time.now() 18 | map_update.publish(tmstp) 19 | rospy.loginfo("All Done ...") 20 | rospy.loginfo("Bye") 21 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/topological_utils/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/topological_utils/nodes.py: -------------------------------------------------------------------------------- 1 | """ 2 | Provides routines for working with topological map nodes 3 | """ 4 | 5 | from topological_navigation_msgs.msg import TopologicalNode 6 | from mongodb_store.message_store import MessageStoreProxy 7 | import queries 8 | import rospy 9 | import copy 10 | 11 | def rename_node(name, new_name, top_map_name): 12 | """ 13 | Renames a topological map node. All edges will be updated to reflect the change. 14 | 15 | @param name: the current name of the node to be changec 16 | @param new_name: the new name to give the node 17 | @param top_map_name: the name of the topological map to work with (pointset) 18 | 19 | @return (number of nodes changed, number of edges changed) 20 | """ 21 | 22 | maps = queries.get_maps() 23 | if top_map_name not in maps: 24 | raise Exception("Unknown topological map.") 25 | 26 | msg_store = MessageStoreProxy(collection='topological_maps') 27 | 28 | nodes = msg_store.query(TopologicalNode._type, {}, {'pointset':top_map_name}) 29 | node_names = [node.name for node,meta in nodes] 30 | node_renames = 0 31 | edge_changes = 0 32 | node_changes = 0 33 | 34 | if name not in node_names: 35 | raise Exception("No such node.") 36 | if new_name in node_names: 37 | raise Exception("New node name already in use.") 38 | 39 | old_metas = [] 40 | for node, meta in nodes: 41 | old_metas.append(copy.deepcopy(meta)) 42 | if meta["node"] == name: 43 | meta["node"] = new_name 44 | if node.name == name: 45 | node.name = new_name 46 | node_renames += 1 47 | if node_renames > 1: 48 | raise Exception("More than one node has the same name!") 49 | for edge in node.edges: 50 | if edge.node == name: 51 | edge.node = new_name 52 | if edge.edge_id.find(name) > -1: 53 | edge.edge_id = edge.edge_id.replace(name, new_name) 54 | edge_changes += 1 55 | 56 | # Save the changed nodes 57 | for ((node, meta), old_meta) in zip(nodes, old_metas): 58 | changed = msg_store.update(node, meta, {'name':old_meta['node'], 59 | 'pointset':meta['pointset']}) 60 | if changed.success: 61 | node_changes += 1 62 | 63 | return (node_changes, edge_changes) 64 | -------------------------------------------------------------------------------- /topological_utils/topological_utils/topological_utils/queries.py: -------------------------------------------------------------------------------- 1 | """ 2 | Provides routines for querying the database for map information. 3 | """ 4 | 5 | from topological_navigation_msgs.msg import TopologicalNode 6 | from mongodb_store.message_store import MessageStoreProxy 7 | import rospy 8 | 9 | def get_maps(): 10 | """ 11 | Queries the database and returns details of the available topological maps. 12 | :return: A dictionary where each key is the name of a topological map and each 13 | item is a dictionary of details. Details are: 14 | "number_nodes" ; integer 15 | "edge_actions" : list of action server names used for traversal 16 | "last_modified" : datetime.datetime object for the last time a node was inserted 17 | """ 18 | maps = dict() 19 | msg_store = MessageStoreProxy(collection='topological_maps') 20 | 21 | nodes = msg_store.query(TopologicalNode._type) 22 | 23 | for node in nodes: 24 | pointset = node[1]["pointset"] 25 | if not maps.has_key(pointset): 26 | maps[pointset] = {"number_nodes": 0, "edge_actions": set(), "last_modified": ""} 27 | maps[pointset]["number_nodes"] += 1 28 | if (maps[pointset]["last_modified"] == "" or 29 | node[1]["inserted_at"] > maps[pointset]["last_modified"]): 30 | maps[pointset]["last_modified"] = node[1]["inserted_at"] 31 | for edge in node[0].edges: 32 | maps[pointset]["edge_actions"].add(edge.action) 33 | 34 | return maps 35 | 36 | 37 | def get_nodes(point_set, meta_query): 38 | 39 | msg_store = MessageStoreProxy(collection="topological_maps") 40 | 41 | query_meta = {} 42 | query_meta["pointset"] = {} 43 | #query_meta["map"] = {} 44 | query_meta["contains.category"] = {} 45 | query_meta["pointset"]['$regex'] = point_set 46 | #query_meta["map"]['$regex'] = map_name 47 | query_meta["contains.category"]['$regex'] = meta_query 48 | 49 | 50 | nodes = msg_store.query(TopologicalNode._type, {}, query_meta); 51 | available = len(nodes) > 0 52 | 53 | if available <= 0 : 54 | print "Desired pointset '"+point_set+"' not in datacentre" 55 | print "Available pointsets: "+str(available) 56 | 57 | return nodes 58 | 59 | if __name__ == "__main__": 60 | get_maps() 61 | --------------------------------------------------------------------------------