├── entrypoint.sh ├── README.md ├── Dockerfile ├── package.xml ├── config └── navigation.yaml ├── CHANGELOG.rst ├── LICENSE ├── .github └── workflows │ └── tii-navigation.yaml ├── CMakeLists.txt ├── launch ├── navigation.py └── octomap_and_navigation.py ├── include └── navigation │ └── astar_planner.hpp ├── scripts └── waypoint_publisher.py ├── src ├── astar_planner.cpp └── navigation.cpp └── rviz └── uav_monitor.rviz /entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | ROS_FLAGS="" 4 | if [[ ${SIMULATION+x} != "" ]]; then 5 | ROS_FLAGS="use_sim_time:=true ${ROS_FLAGS}" 6 | fi 7 | 8 | exec ros-with-env ros2 launch navigation navigation.py ${ROS_FLAGS} 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Deprecated 2 | ## This package is no longer maintained or supported. 3 | 4 | # navigation 5 | 6 | Path planning for collision-free navigation 7 | 8 | Development, debug 9 | ------------------ 10 | 11 | [See documentation in container base image](https://github.com/tiiuae/fog-ros-baseimage/tree/main#development--debug-for-concrete-projects) 12 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ghcr.io/tiiuae/fog-ros-baseimage:builder-latest AS builder 2 | 3 | COPY . /main_ws/src/ 4 | 5 | # this: 6 | # 1) builds the application 7 | # 2) packages the application as .deb & writes it to /main_ws/ 8 | RUN /packaging/build.sh 9 | 10 | # ▲ runtime ──┐ 11 | # └── build ▼ 12 | 13 | FROM ghcr.io/tiiuae/fog-ros-baseimage:stable 14 | 15 | ENTRYPOINT /entrypoint.sh 16 | 17 | COPY entrypoint.sh /entrypoint.sh 18 | 19 | COPY --from=builder /main_ws/ros-*-navigation_*_amd64.deb /navigation.deb 20 | 21 | RUN apt update && apt install -y --no-install-recommends ./navigation.deb \ 22 | && rm /navigation.deb 23 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | navigation 6 | 0.0.5 7 | Provides collision free path-planning 8 | stibipet 9 | 10 | BSD 3-Clause 11 | 12 | ament_cmake 13 | 14 | rclcpp 15 | rclcpp_components 16 | rclcpp_lifecycle 17 | launch 18 | launch_ros 19 | builtin_interfaces 20 | 21 | nav_msgs 22 | std_msgs 23 | std_srvs 24 | fog_msgs 25 | geometry_msgs 26 | visualization_msgs 27 | octomap 28 | octomap_msgs 29 | dynamicEDT3D 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /config/navigation.yaml: -------------------------------------------------------------------------------- 1 | planning: 2 | euclidean_distance_cutoff: 3.0 # [m] 3 | safe_obstacle_distance: 1.0 # [m] 4 | bumper_distance_factor: 0.9 # virtual bumper will intervene at (safe_obstacle_distance * bumper_distance_factor) 5 | unknown_is_occupied: false 6 | navigation_tolerance: 0.4 # [m] 7 | min_altitude: 1.0 # [m] 8 | max_altitude: 10.0 # [m] 9 | max_goal_distance: 50.0 # [m] 10 | distance_penalty: 1.0 11 | greedy_penalty: 1.0 12 | planning_tree_resolution: 0.4 13 | max_waypoint_distance: 0.4 # [m] or set negative to allow maximal segment length 14 | max_yaw_step: 0.1 # [rad] 15 | planning_timeout: 4.5 # [s] 16 | replanning_limit: 3 # number of repeated attempts 17 | replanning_distance: 15.0 # [m] Euclidean distance from first waypoint 18 | override_previous_commands: true # new GOTO will override all previous actions 19 | main_update_rate: 10.0 # [Hz] 20 | 21 | visualization: 22 | visualize_planner: true 23 | show_unoccupied: false 24 | tree_points_scale: 0.22 25 | field_points_scale: 0.22 26 | expansions_points_scale: 0.15 27 | path_points_scale: 0.18 28 | goal_points_scale : 0.28 29 | 30 | bumper: 31 | enabled: false 32 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package navigation 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.3 (2021-06-21) 6 | ----------- 7 | * Global coordinates navigation update (`#5 `_) 8 | * add global coordinate input interface 9 | * add include dirs to cmakelists 10 | * fix github action build_id 11 | * Merge pull request `#4 `_ from tiiuae/repository_dispatch 12 | * CI: add rebuild repository dispatch event 13 | * add rebuild repository dispatch event 14 | * Merge pull request `#3 `_ from tiiuae/fix-pkg-name 15 | * pkg_name to navigation 16 | * Merge pull request `#2 `_ from tiiuae/integration_fixes 17 | * tty fix + moved launch file from fog_core 18 | * moved launch file from fog_core 19 | * Merge pull request `#1 `_ from tiiuae/DP-853_f4f_navigation_build 20 | * add CI workflow 21 | * add the option to override previous commands or interrupt navigation and hover 22 | * Contributors: Esa Kulmala, Jari Nippula, Sergey Smirnov, sergey-unikie, stibipet 23 | 24 | 0.0.2 (2021-06-02) 25 | ----------- 26 | * subscribe to control diagnostics for mission status 27 | * update path generation and postprocessing 28 | * Contributors: stibipet 29 | 30 | 0.0.1 (2021-05-19) 31 | ------------------ 32 | * Add service to set path with fog_msgs::srv::Path 33 | * Add changelog 34 | * Update input to use nav_msgs::Path + std_srvs::Trigger 35 | * Contributors: Vojtech Spurny, stibipet 36 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Multi-robot Systems (MRS) group at Czech Technical University in Prague 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /.github/workflows/tii-navigation.yaml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | repository_dispatch: 5 | types: [fog-ros-baseimage-update] 6 | push: 7 | 8 | jobs: 9 | build: 10 | runs-on: ubuntu-latest 11 | steps: 12 | - uses: actions/checkout@v2 13 | 14 | - uses: docker/setup-buildx-action@v1 15 | 16 | - name: Set image tag format without suffix 17 | run: | 18 | echo "IMAGE_TAG_FORMAT=type=sha" >> $GITHUB_ENV 19 | if: github.event_name == 'push' 20 | 21 | - name: Set image tag format with suffix 22 | # it is possible that run_number should be used instead run_attempt 23 | # run_attempt is unique number on every run and run_attempt resets to 1 if re-build is not used 24 | run: | 25 | echo "IMAGE_TAG_FORMAT=type=sha,suffix=-${{ github.event.client_payload.run_attempt }}" >> $GITHUB_ENV 26 | if: github.event_name == 'repository_dispatch' 27 | 28 | - name: Docker meta 29 | id: meta 30 | uses: docker/metadata-action@v3 31 | with: 32 | images: ghcr.io/tiiuae/tii-navigation 33 | tags: | 34 | type=ref,event=branch 35 | type=semver,pattern={{version}} 36 | type=raw,value=latest 37 | ${{ env.IMAGE_TAG_FORMAT }} 38 | 39 | - name: Login to GitHub Container Registry 40 | uses: docker/login-action@v1 41 | with: 42 | registry: ghcr.io 43 | username: ${{ github.actor }} 44 | password: ${{ secrets.GITHUB_TOKEN }} 45 | 46 | - name: Build container image and push 47 | uses: docker/build-push-action@v2 48 | with: 49 | context: . 50 | push: true 51 | tags: ${{ steps.meta.outputs.tags }} 52 | labels: ${{ steps.meta.outputs.labels }} 53 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(navigation) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 7 | 8 | add_definitions("-Wall") 9 | add_definitions("-Wextra") 10 | add_definitions("-Wpedantic") 11 | add_definitions("-g") 12 | add_definitions("-O3") 13 | 14 | find_package(ament_cmake REQUIRED) 15 | find_package(rclcpp REQUIRED) 16 | find_package(rclcpp_components REQUIRED) 17 | find_package(std_msgs REQUIRED) 18 | find_package(std_srvs REQUIRED) 19 | find_package(fog_msgs 0.0.8 REQUIRED) 20 | find_package(nav_msgs REQUIRED) 21 | find_package(geometry_msgs REQUIRED) 22 | find_package(visualization_msgs REQUIRED) 23 | find_package(octomap REQUIRED) 24 | find_package(octomap_msgs REQUIRED) 25 | find_package(dynamicEDT3D REQUIRED) 26 | 27 | include_directories( 28 | include 29 | ${rclcpp_INCLUDE_DIRS} 30 | ${OCTOMAP_INCLUDE_DIRS} 31 | ${DYNAMICEDT3D_INCLUDE_DIRS} 32 | ) 33 | 34 | ## -------------------------------------------------------------- 35 | ## | compile | 36 | ## -------------------------------------------------------------- 37 | 38 | #{ navigation 39 | add_library(navigation SHARED 40 | src/navigation.cpp 41 | src/astar_planner.cpp 42 | ) 43 | 44 | target_compile_definitions(navigation 45 | PRIVATE "${PROJECT_NAME}_BUILDING_DLL") 46 | 47 | ament_target_dependencies(navigation 48 | rclcpp 49 | rclcpp_components 50 | std_msgs 51 | std_srvs 52 | fog_msgs 53 | nav_msgs 54 | geometry_msgs 55 | visualization_msgs 56 | octomap 57 | octomap_msgs 58 | dynamicEDT3D 59 | ) 60 | 61 | target_link_libraries(navigation ${OCTOMAP_LIBRARIES} ${DYNAMICEDT3D_LIBRARIES}) 62 | rclcpp_components_register_nodes(navigation PLUGIN "${PROJECT_NAME}::Navigation" EXECUTABLE navigation) 63 | 64 | #} 65 | 66 | ## -------------------------------------------------------------- 67 | ## | install | 68 | ## -------------------------------------------------------------- 69 | 70 | install(TARGETS 71 | navigation 72 | ARCHIVE DESTINATION lib 73 | LIBRARY DESTINATION lib 74 | RUNTIME DESTINATION bin 75 | # DESTINATION lib/${PROJECT_NAME} 76 | ) 77 | 78 | ament_export_include_directories(include) 79 | 80 | # install(DIRECTORY include 81 | # DESTINATION include 82 | # ) 83 | 84 | install(DIRECTORY launch 85 | DESTINATION share/${PROJECT_NAME} 86 | ) 87 | 88 | install(DIRECTORY config 89 | DESTINATION share/${PROJECT_NAME} 90 | ) 91 | 92 | install(DIRECTORY scripts 93 | DESTINATION lib/${PROJECT_NAME} 94 | ) 95 | 96 | ament_package() 97 | -------------------------------------------------------------------------------- /launch/navigation.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from ament_index_python.packages import get_package_share_directory 3 | from launch_ros.actions import Node 4 | from launch_ros.actions import ComposableNodeContainer 5 | from launch_ros.descriptions import ComposableNode 6 | import os 7 | import sys 8 | 9 | def generate_launch_description(): 10 | 11 | ld = launch.LaunchDescription() 12 | 13 | pkg_name = "navigation" 14 | pkg_share_path = get_package_share_directory(pkg_name) 15 | 16 | ld.add_action(launch.actions.DeclareLaunchArgument("use_sim_time", default_value="false")) 17 | ld.add_action(launch.actions.DeclareLaunchArgument("debug", default_value="false")) 18 | dbg_sub = None 19 | if sys.stdout.isatty(): 20 | dbg_sub = [launch.substitutions.PythonExpression(['"" if "false" == "', launch.substitutions.LaunchConfiguration("debug"), '" else "debug_ros2launch ' + os.ttyname(sys.stdout.fileno()) + '"']), 'stdbuf -o L'] 21 | 22 | 23 | DRONE_DEVICE_ID=os.getenv('DRONE_DEVICE_ID') 24 | 25 | namespace=DRONE_DEVICE_ID 26 | ld.add_action(ComposableNodeContainer( 27 | namespace='', 28 | name=namespace+'_navigation', 29 | package='rclcpp_components', 30 | executable='component_container_mt', 31 | composable_node_descriptions=[ 32 | ComposableNode( 33 | package=pkg_name, 34 | plugin='navigation::Navigation', 35 | namespace=namespace, 36 | name='navigation', 37 | parameters=[ 38 | pkg_share_path + '/config/navigation.yaml', 39 | {"use_sim_time": launch.substitutions.LaunchConfiguration("use_sim_time")}, 40 | ], 41 | remappings=[ 42 | ("~/octomap_in", "/" + DRONE_DEVICE_ID + "/octomap_server/octomap_full"), 43 | ("~/odometry_in", "/" + DRONE_DEVICE_ID + "/control_interface/local_odom"), 44 | ("~/desired_pose_in", "/" + DRONE_DEVICE_ID + "/control_interface/desired_pose"), 45 | ("~/hover_in", "~/hover"), 46 | ("~/goto_in", "~/goto_waypoints"), 47 | ("~/goto_trigger_in", "~/goto_start"), 48 | ("~/control_diagnostics_in", "/" + DRONE_DEVICE_ID + "/control_interface/diagnostics"), 49 | ("~/bumper_in", "/" + DRONE_DEVICE_ID + "/bumper/obstacle_sectors"), 50 | 51 | ("~/local_waypoint_in", "~/local_waypoint"), 52 | ("~/local_path_in", "~/local_path"), 53 | ("~/gps_waypoint_in", "~/gps_waypoint"), 54 | ("~/gps_path_in", "~/gps_path"), 55 | 56 | ("~/diagnostics_out", "~/diagnostics"), 57 | ("~/local_path_out", "/" + DRONE_DEVICE_ID + "/control_interface/local_path"), 58 | ("~/gps_path_out", "/" + DRONE_DEVICE_ID + "/control_interface/gps_path"), 59 | 60 | ("~/waypoint_to_local_out", "/" + DRONE_DEVICE_ID + "/control_interface/waypoint_to_local"), 61 | ("~/path_to_local_out", "/" + DRONE_DEVICE_ID + "/control_interface/path_to_local"), 62 | ], 63 | ), 64 | ], 65 | output='screen', 66 | prefix=dbg_sub, 67 | parameters=[{"use_sim_time": launch.substitutions.LaunchConfiguration("use_sim_time")},], 68 | )) 69 | 70 | return ld 71 | -------------------------------------------------------------------------------- /launch/octomap_and_navigation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import launch 4 | from launch_ros.actions import Node 5 | from launch_ros.actions import ComposableNodeContainer 6 | from launch_ros.descriptions import ComposableNode 7 | from ament_index_python.packages import get_package_share_directory 8 | import os 9 | import sys 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | ld = launch.LaunchDescription() 15 | 16 | pkg_name = "navigation" 17 | pkg_share_path = get_package_share_directory(package_name=pkg_name) 18 | 19 | navigation_path = get_package_share_directory(package_name="navigation") 20 | octomap_server_path = get_package_share_directory(package_name="octomap_server2") 21 | 22 | DRONE_DEVICE_ID=os.getenv('DRONE_DEVICE_ID') 23 | 24 | ld.add_action(launch.actions.DeclareLaunchArgument("debug", default_value="false")) 25 | ld.add_action(launch.actions.DeclareLaunchArgument("use_sim_time", default_value="false")) 26 | dbg_sub = None 27 | if sys.stdout.isatty(): 28 | dbg_sub = launch.substitutions.PythonExpression(['"" if "false" == "', launch.substitutions.LaunchConfiguration("debug"), '" else "debug_ros2launch ' + os.ttyname(sys.stdout.fileno()) + '"']) 29 | 30 | namespace=DRONE_DEVICE_ID 31 | 32 | ld.add_action(ComposableNodeContainer( 33 | namespace='', 34 | name=namespace+'_octomap_and_navigation', 35 | package='rclcpp_components', 36 | executable='component_container_mt', 37 | # executable='component_container', 38 | composable_node_descriptions=[ 39 | # navigation 40 | ComposableNode( 41 | namespace=namespace, 42 | name='navigation', 43 | package='navigation', 44 | plugin='navigation::Navigation', 45 | 46 | parameters=[ 47 | navigation_path + '/config/navigation.yaml', 48 | {"use_sim_time": launch.substitutions.LaunchConfiguration("use_sim_time")}, 49 | ], 50 | remappings=[ 51 | ("~/octomap_in", "/" + DRONE_DEVICE_ID + "/octomap_server/octomap_full"), 52 | ("~/odometry_in", "/" + DRONE_DEVICE_ID + "/control_interface/local_odom"), 53 | ("~/waypoints_out", "/" + DRONE_DEVICE_ID + "/control_interface/waypoints"), 54 | ("~/goto_in", "~/goto_waypoints"), 55 | ("~/goto_trigger_in", "~/goto_start"), 56 | ("~/set_path_in", "~/set_path"), 57 | ], 58 | 59 | ), 60 | # octomap_server 61 | ComposableNode( 62 | namespace=namespace, 63 | name='octomap_server', 64 | package='octomap_server2', 65 | plugin='octomap_server::OctomapServer', 66 | remappings=[ 67 | # subscribers 68 | ('laser_scan_in', 'rplidar/scan'), 69 | # publishers 70 | ('octomap_binary_out', '~/octomap_binary'), 71 | ('octomap_full_out', '~/octomap_full'), 72 | ('occupied_cells_vis_array_out', '~/occupied_cells_vis_array'), 73 | ('free_cells_vis_array_out', '~/free_cells_vis_array_out'), 74 | ('octomap_point_cloud_centers_out', '~/octomap_point_cloud_centers'), 75 | ('octomap_free_centers_out', '~/octomap_free_centers_out'), 76 | ('projected_map_out', '~/projected_map'), 77 | # service servers 78 | ('octomap_binary', '~/octomap_binary'), 79 | ('octomap_full', '~/octomap_full'), 80 | ('clear_bbx', '~/clear_bbx'), 81 | ('reset', '~/reset'), 82 | ], 83 | parameters=[ 84 | octomap_server_path + '/config/params.yaml', 85 | {"frame_id": "world"}, 86 | {"use_sim_time": launch.substitutions.LaunchConfiguration("use_sim_time")}, 87 | ], 88 | ), 89 | ], 90 | output='screen', 91 | prefix=dbg_sub, 92 | parameters=[{"use_sim_time": launch.substitutions.LaunchConfiguration("use_sim_time")},], 93 | )) 94 | return ld 95 | -------------------------------------------------------------------------------- /include/navigation/astar_planner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef NAVIGATION_ASTAR_PLANNER_HPP 2 | #define NAVIGATION_ASTAR_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace navigation 14 | { 15 | 16 | enum TreeValue 17 | { 18 | OCCUPIED = 0, 19 | FREE = 1 20 | }; 21 | 22 | enum PlanningResult 23 | { 24 | COMPLETE = 0, 25 | GOAL_REACHED, 26 | INCOMPLETE, 27 | GOAL_IN_OBSTACLE, 28 | FAILURE 29 | }; 30 | 31 | 32 | struct Node 33 | { 34 | octomap::OcTreeKey key; 35 | double total_cost; 36 | double cum_dist; 37 | double goal_dist; 38 | 39 | bool operator==(const Node &other) const; 40 | bool operator!=(const Node &other) const; 41 | bool operator<(const Node &other) const; 42 | bool operator<=(const Node &other) const; 43 | }; 44 | 45 | struct CostComparator 46 | { 47 | bool operator()(const Node &n1, const Node &n2) const; 48 | }; 49 | 50 | struct LeafComparator 51 | { 52 | bool operator()(const std::pair &l1, const std::pair &l2) const; 53 | }; 54 | 55 | struct HashFunction 56 | { 57 | bool operator()(const Node &n) const; 58 | }; 59 | 60 | class AstarPlanner { 61 | 62 | public: 63 | AstarPlanner(double safe_obstacle_distance, double euclidean_distance_cutoff, double planning_tree_resolution, double distance_penalty, double greedy_penalty, 64 | double min_altitude, double max_altitude, double timeout_threshold, double max_waypoint_distance, bool unknown_is_occupied); 65 | 66 | private: 67 | double safe_obstacle_distance; 68 | double euclidean_distance_cutoff; 69 | double planning_tree_resolution; 70 | double distance_penalty; 71 | double greedy_penalty; 72 | double timeout_threshold; 73 | double max_waypoint_distance; 74 | double min_altitude; 75 | double max_altitude; 76 | bool unknown_is_occupied; 77 | 78 | public: 79 | std::pair, PlanningResult> findPath( 80 | const octomap::point3d &start_coord, const octomap::point3d &goal_coord, const octomap::point3d &pos_cmd, std::shared_ptr mapping_tree, 81 | double timeout, std::function visualizeTree, 82 | std::function &, const std::unordered_set &, const octomap::OcTree &)> 83 | visualizeExpansions); 84 | 85 | private: 86 | const std::vector> EXPANSION_DIRECTIONS = {{-1, -1, -1}, {-1, -1, 0}, {-1, -1, 1}, {-1, 0, -1}, {-1, 0, 0}, {-1, 0, 1}, {-1, 1, -1}, 87 | {-1, 1, 0}, {-1, 1, 1}, {0, -1, -1}, {0, -1, 0}, {0, -1, 1}, {0, 0, -1}, {0, 0, 1}, 88 | {0, 1, -1}, {0, 1, 0}, {0, 1, 1}, {1, -1, -1}, {1, -1, 0}, {1, -1, 1}, {1, 0, -1}, 89 | {1, 0, 0}, {1, 0, 1}, {1, 1, -1}, {1, 1, 0}, {1, 1, 1}}; 90 | double getNodeDepth(const octomap::OcTreeKey &key, octomap::OcTree &tree); 91 | 92 | std::vector getNeighborhood(const octomap::OcTreeKey &key, octomap::OcTree &tree); 93 | 94 | octomap::OcTreeKey expand(const octomap::OcTreeKey &key, const std::vector &direction); 95 | 96 | double distEuclidean(const octomap::point3d &p1, const octomap::point3d &p2); 97 | 98 | double distEuclidean(const octomap::OcTreeKey &k1, const octomap::OcTreeKey &k2, octomap::OcTree &tree); 99 | 100 | bool freeStraightPath(const octomap::point3d p1, const octomap::point3d p2, octomap::OcTree &tree); 101 | 102 | std::vector backtrackPathKeys(const Node &start, const Node &end, std::unordered_map &parent_map); 103 | 104 | std::vector keysToCoords(std::vector keys, octomap::OcTree &tree); 105 | 106 | DynamicEDTOctomap euclideanDistanceTransform(std::shared_ptr tree); 107 | 108 | std::optional>> createPlanningTree(std::shared_ptr tree, 109 | const octomap::point3d &start, double resolution); 110 | 111 | std::pair generateTemporaryGoal(const octomap::point3d &start, const octomap::point3d &goal, const octomap::point3d &pos_cmd, octomap::OcTree &tree); 112 | 113 | std::vector filterPath(const std::vector &waypoints, octomap::OcTree &tree); 114 | 115 | std::vector prepareOutputPath(const std::vector &keys, octomap::OcTree &tree); 116 | 117 | /* geometry_msgs::msg::Quaternion yawToQuaternionMsg(const double &yaw); */ 118 | }; 119 | 120 | } // namespace navigation 121 | #endif 122 | -------------------------------------------------------------------------------- /scripts/waypoint_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | ## Debug script for easier nav_msgs.msg.Path publishing 4 | 5 | import os 6 | import numpy as np 7 | import math 8 | 9 | import rclpy 10 | from rclpy.node import Node 11 | from rclpy.time import Time 12 | 13 | import time as pythontime 14 | 15 | from nav_msgs.msg import Path as NavPath 16 | from fog_msgs.srv import Path as SetPath 17 | from geometry_msgs.msg import Point 18 | from geometry_msgs.msg import PoseStamped 19 | from geometry_msgs.msg import Quaternion 20 | 21 | WAYPOINTS_LOCAL=[ 22 | [0,0,3,0], 23 | [-5,4,2,0], 24 | [7,-5,2,0], 25 | [0,0,2,0] 26 | ] 27 | 28 | WAYPOINTS_GPS=[ 29 | [47.397708, 8.5456038, 4, 0], 30 | [47.3977, 8.5456038, 2, 1.5708], 31 | [47.39775, 8.5456, 2, -1.5708], 32 | [47.39758, 8.545706, 1.5, 3.14], 33 | [47.397708, 8.5456038, 2, -3.14] 34 | ] 35 | 36 | def quaternion_from_euler(roll, pitch, yaw): 37 | """ 38 | Converts euler roll, pitch, yaw to quaternion (w in last place) 39 | quat = [x, y, z, w] 40 | Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. 41 | """ 42 | cy = math.cos(yaw * 0.5) 43 | sy = math.sin(yaw * 0.5) 44 | cp = math.cos(pitch * 0.5) 45 | sp = math.sin(pitch * 0.5) 46 | cr = math.cos(roll * 0.5) 47 | sr = math.sin(roll * 0.5) 48 | 49 | q = Quaternion() 50 | q.w = cy * cp * cr + sy * sp * sr 51 | q.x = cy * cp * sr - sy * sp * cr 52 | q.y = sy * cp * sr + cy * sp * cr 53 | q.z = sy * cp * cr - cy * sp * sr 54 | 55 | return q 56 | 57 | class PathPublisherNode(Node): 58 | 59 | def __init__(self): 60 | 61 | DRONE_DEVICE_ID = os.getenv('DRONE_DEVICE_ID') 62 | 63 | super().__init__("waypoint_publisher") 64 | self.client_local = self.create_client(SetPath, "/" + DRONE_DEVICE_ID + "/navigation/local_path") 65 | self.client_gps = self.create_client(SetPath, "/" + DRONE_DEVICE_ID + "/navigation/gps_path") 66 | 67 | def publish(self): 68 | path = Path() 69 | path.header.stamp = rclpy.clock.Clock().now().to_msg() 70 | path.header.frame_id = "world" 71 | 72 | for wp in WAYPOINTS: 73 | pose = PoseStamped() 74 | pose.header.stamp = path.header.stamp 75 | pose.header.frame_id = "world" 76 | point = Point() 77 | point.x = float(wp[0]) 78 | point.y = float(wp[1]) 79 | point.z = float(wp[2]) 80 | pose.pose.position = point 81 | q = quaternion_from_euler(0,0,wp[3]) 82 | pose.pose.orientation = q 83 | path.poses.append(pose) 84 | print('Publishing: "%s"' % path.poses) 85 | self.publisher.publish(path) 86 | 87 | # #{ call_service_local 88 | def call_service_local(self): 89 | path = NavPath() 90 | path.header.stamp = rclpy.clock.Clock().now().to_msg() 91 | path.header.frame_id = "world" 92 | 93 | for wp in WAYPOINTS_LOCAL: 94 | pose = PoseStamped() 95 | pose.header.stamp = path.header.stamp 96 | pose.header.frame_id = "world" 97 | point = Point() 98 | point.x = float(wp[0]) 99 | point.y = float(wp[1]) 100 | point.z = float(wp[2]) 101 | pose.pose.position = point 102 | q = quaternion_from_euler(0,0,wp[3]) 103 | pose.pose.orientation = q 104 | path.poses.append(pose) 105 | print('Calling service : "%s"' % self.client_local.srv_name) 106 | path_req = SetPath.Request() 107 | path_req.path = path 108 | self.future = self.client_local.call_async(path_req) 109 | # #} 110 | 111 | # #{ call_service_gps 112 | def call_service_gps(self): 113 | path = NavPath() 114 | path.header.stamp = rclpy.clock.Clock().now().to_msg() 115 | path.header.frame_id = "world" 116 | 117 | for wp in WAYPOINTS_GPS: 118 | pose = PoseStamped() 119 | pose.header.stamp = path.header.stamp 120 | pose.header.frame_id = "world" 121 | point = Point() 122 | point.x = float(wp[0]) 123 | point.y = float(wp[1]) 124 | point.z = float(wp[2]) 125 | pose.pose.position = point 126 | q = quaternion_from_euler(0,0,wp[3]) 127 | pose.pose.orientation = q 128 | path.poses.append(pose) 129 | print('Calling service : "%s"' % self.client_gps.srv_name) 130 | path_req = SetPath.Request() 131 | path_req.path = path 132 | self.future = self.client_gps.call_async(path_req) 133 | # #} 134 | 135 | def main(args=None): 136 | rclpy.init(args=args) 137 | node = PathPublisherNode() 138 | 139 | rate = node.create_rate(0.1) 140 | node.call_service_gps() 141 | # node.call_service_local() 142 | 143 | while rclpy.ok(): 144 | rclpy.spin_once(node) 145 | if node.future.done(): 146 | try: 147 | response = node.future.result() 148 | except Exception as e: 149 | node.get_logger().info( 150 | 'Service call failed %r' % (e,)) 151 | else: 152 | if response.success: 153 | node.get_logger().info( 154 | 'Service SUCCEEDED with response "%s"' % 155 | (response.message)) 156 | else: 157 | node.get_logger().info( 158 | 'Service FAILED with response "%s"' % 159 | (response.message)) 160 | break 161 | 162 | node.call_service_gps() 163 | # node.call_service_local() 164 | rate.sleep() 165 | 166 | node.destroy_node() 167 | rclpy.shutdown() 168 | 169 | if __name__ == "__main__": 170 | main() 171 | -------------------------------------------------------------------------------- /src/astar_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace navigation 6 | { 7 | 8 | bool Node::operator==(const Node &other) const { 9 | return key == other.key; 10 | } 11 | 12 | bool Node::operator!=(const Node &other) const { 13 | return key != other.key; 14 | } 15 | 16 | bool Node::operator<(const Node &other) const { 17 | 18 | if (total_cost == other.total_cost) { 19 | return goal_dist < other.goal_dist; 20 | } 21 | 22 | return total_cost < other.total_cost; 23 | } 24 | 25 | bool Node::operator<=(const Node &other) const { 26 | 27 | if (total_cost == other.total_cost) { 28 | return goal_dist <= other.goal_dist; 29 | } 30 | 31 | return total_cost <= other.total_cost; 32 | } 33 | 34 | bool CostComparator::operator()(const Node &n1, const Node &n2) const { 35 | 36 | if (n1.total_cost == n2.total_cost) { 37 | return n1.goal_dist > n2.goal_dist; 38 | } 39 | 40 | return n1.total_cost > n2.total_cost; 41 | } 42 | 43 | bool HashFunction::operator()(const Node &n) const { 44 | using std::hash; 45 | return ((hash()(n.key.k[0]) ^ (hash()(n.key.k[1]) << 1)) >> 1) ^ (hash()(n.key.k[2]) << 1); 46 | } 47 | 48 | bool LeafComparator::operator()(const std::pair &l1, const std::pair &l2) const { 49 | return l1.second < l2.second; 50 | } 51 | 52 | /* AstarPlanner constructor //{ */ 53 | AstarPlanner::AstarPlanner(double safe_obstacle_distance, double euclidean_distance_cutoff, double planning_tree_resolution, double distance_penalty, 54 | double greedy_penalty, double min_altitude, double max_altitude, double timeout_threshold, double max_waypoint_distance, 55 | bool unknown_is_occupied) { 56 | 57 | this->safe_obstacle_distance = safe_obstacle_distance; 58 | this->euclidean_distance_cutoff = euclidean_distance_cutoff; 59 | this->planning_tree_resolution = planning_tree_resolution; 60 | this->distance_penalty = distance_penalty; 61 | this->greedy_penalty = greedy_penalty; 62 | this->min_altitude = min_altitude; 63 | this->max_altitude = max_altitude; 64 | this->timeout_threshold = timeout_threshold; 65 | this->max_waypoint_distance = max_waypoint_distance; 66 | this->unknown_is_occupied = unknown_is_occupied; 67 | } 68 | //} 69 | 70 | /* findPath //{ */ 71 | 72 | std::pair, PlanningResult> AstarPlanner::findPath( 73 | const octomap::point3d &start_coord, const octomap::point3d &goal_coord, const octomap::point3d &pos_cmd, std::shared_ptr mapping_tree, 74 | double timeout, std::function visualizeTree, 75 | std::function &, const std::unordered_set &, const octomap::OcTree &)> 76 | visualizeExpansions) { 77 | 78 | printf("[Astar]: Astar: start [%.2f, %.2f, %.2f]\n", start_coord.x(), start_coord.y(), start_coord.z()); 79 | printf("[Astar]: Astar: goal [%.2f, %.2f, %.2f]\n", goal_coord.x(), goal_coord.y(), goal_coord.z()); 80 | 81 | auto time_start = std::chrono::high_resolution_clock::now(); 82 | 83 | this->timeout_threshold = timeout; 84 | 85 | auto time_start_planning_tree = std::chrono::high_resolution_clock::now(); 86 | auto tree_with_tunnel = createPlanningTree(mapping_tree, start_coord, planning_tree_resolution); 87 | printf("[Astar]: the planning tree took %.2f s to create\n", 88 | std::chrono::duration(std::chrono::high_resolution_clock::now() - time_start_planning_tree).count()); 89 | 90 | if (!tree_with_tunnel) { 91 | printf("[Astar]: could not create a planning tree\n"); 92 | return {std::vector(), FAILURE}; 93 | } 94 | 95 | visualizeTree((*tree_with_tunnel).first); 96 | 97 | auto tree = tree_with_tunnel.value().first; 98 | auto tunnel = tree_with_tunnel.value().second; 99 | 100 | auto map_goal = goal_coord; 101 | auto map_query = tree_with_tunnel->first.search(goal_coord); 102 | bool original_goal = true; 103 | 104 | if (map_query == NULL) { 105 | printf("[Astar]: Goal is outside of map\n"); 106 | auto temp_goal = generateTemporaryGoal(start_coord, goal_coord, pos_cmd, tree); 107 | printf("[Astar]: Generated a temporary goal: [%.2f, %.2f, %.2f]\n", temp_goal.first.x(), temp_goal.first.y(), temp_goal.first.z()); 108 | if (temp_goal.second) { 109 | std::vector vertical_path; 110 | vertical_path.push_back(start_coord); 111 | vertical_path.push_back(temp_goal.first); 112 | return {vertical_path, INCOMPLETE}; 113 | } else { 114 | map_goal = temp_goal.first; 115 | original_goal = false; 116 | } 117 | } else if (map_query->getValue() == TreeValue::OCCUPIED) { 118 | printf("[Astar]: Goal is inside an inflated obstacle\n"); 119 | if (distEuclidean(map_goal, start_coord) <= 1 * safe_obstacle_distance) { 120 | printf("[Astar]: Path special case, we cannot get closer\n"); 121 | return {std::vector(), GOAL_REACHED}; 122 | } 123 | } 124 | 125 | std::priority_queue, CostComparator> open_heap; 126 | std::unordered_set open; 127 | std::unordered_set closed; 128 | std::unordered_map parent_map; // first = child, second = parent 129 | 130 | octomap::OcTreeKey start; 131 | if (tunnel.empty()) { 132 | start = tree.coordToKey(start_coord); 133 | } else { 134 | start = tree.coordToKey(tunnel.back()); 135 | } 136 | 137 | auto planning_start = tree.keyToCoord(start); 138 | auto goal = tree.coordToKey(map_goal); 139 | 140 | if (distEuclidean(planning_start, map_goal) <= 2 * planning_tree_resolution) { 141 | 142 | printf("[Astar]: Path special case, we are there\n"); 143 | 144 | visualizeExpansions(open, closed, tree); 145 | 146 | return {std::vector(), GOAL_REACHED}; 147 | } 148 | 149 | std::cout << "[Astar]: Planning from: " << planning_start.x() << ", " << planning_start.y() << ", " << planning_start.z() << "\n"; 150 | std::cout << "[Astar]: Planning to: " << map_goal.x() << ", " << map_goal.y() << ", " << map_goal.z() << "\n"; 151 | 152 | Node first; 153 | first.key = start; 154 | first.cum_dist = 0; 155 | first.goal_dist = distEuclidean(start, goal, tree); 156 | first.total_cost = first.cum_dist + first.goal_dist; 157 | open_heap.push(first); 158 | open.insert(first); 159 | 160 | Node best_node = first; 161 | Node best_node_greedy = first; 162 | 163 | Node last_closed; 164 | 165 | while (!open.empty() && rclcpp::ok()) { 166 | 167 | Node current = open_heap.top(); 168 | open_heap.pop(); 169 | open.erase(current); 170 | closed.insert(current); 171 | 172 | last_closed = current; 173 | 174 | auto time_now = std::chrono::high_resolution_clock::now(); 175 | 176 | if (std::chrono::duration(time_now - time_start).count() > timeout_threshold) { 177 | 178 | printf("[Astar]: Planning timeout! Using current best node as goal.\n"); 179 | auto path_keys = backtrackPathKeys(best_node == first ? best_node_greedy : best_node, first, parent_map); 180 | printf("[Astar]: Path found. Length: %ld\n", path_keys.size()); 181 | 182 | visualizeExpansions(open, closed, tree); 183 | 184 | return {prepareOutputPath(path_keys, tree), INCOMPLETE}; 185 | } 186 | 187 | auto current_coord = tree.keyToCoord(current.key); 188 | 189 | if (distEuclidean(current_coord, map_goal) <= 2 * planning_tree_resolution) { 190 | 191 | auto path_keys = backtrackPathKeys(current, first, parent_map); 192 | path_keys.push_back(tree.coordToKey(map_goal)); 193 | printf("[Astar]: Path found. Length: %ld\n", path_keys.size()); 194 | 195 | visualizeExpansions(open, closed, tree); 196 | 197 | if (original_goal) { 198 | return {prepareOutputPath(path_keys, tree), COMPLETE}; 199 | } 200 | return {prepareOutputPath(path_keys, tree), INCOMPLETE}; 201 | } 202 | 203 | // expand 204 | auto neighbors = getNeighborhood(current.key, tree); 205 | 206 | for (auto &nkey : neighbors) { 207 | 208 | Node n; 209 | n.key = nkey; 210 | 211 | auto closed_query = closed.find(n); 212 | auto open_query = open.find(n); 213 | 214 | // in open map 215 | n.goal_dist = distEuclidean(nkey, goal, tree); 216 | n.cum_dist = current.cum_dist + distEuclidean(current.key, nkey, tree); 217 | n.total_cost = greedy_penalty * n.goal_dist + distance_penalty * n.cum_dist; 218 | 219 | if (closed_query == closed.end() && open_query == open.end()) { 220 | 221 | if (n <= best_node) { 222 | best_node = n; 223 | } 224 | 225 | if (n.goal_dist <= best_node_greedy.goal_dist) { 226 | best_node_greedy = n; 227 | } 228 | 229 | open_heap.push(n); 230 | open.insert(n); 231 | parent_map[n] = current; 232 | } 233 | } 234 | } 235 | 236 | visualizeExpansions(open, closed, tree); 237 | 238 | if (best_node != first) { 239 | 240 | auto path_keys = backtrackPathKeys(best_node, first, parent_map); 241 | 242 | printf("[Astar]: direct path does not exist, going to the 'best_node'\n"); 243 | 244 | return {prepareOutputPath(path_keys, tree), INCOMPLETE}; 245 | } 246 | 247 | if (best_node_greedy != first) { 248 | 249 | auto path_keys = backtrackPathKeys(best_node_greedy, first, parent_map); 250 | 251 | printf("[Astar]: direct path does not exist, going to the best_node_greedy'\n"); 252 | 253 | return {prepareOutputPath(path_keys, tree), INCOMPLETE}; 254 | } 255 | 256 | if (!tunnel.empty()) { 257 | std::vector path_to_safety; 258 | path_to_safety.push_back(start_coord); 259 | path_to_safety.push_back(tunnel.back()); 260 | printf("[Astar]: path does not exist, escaping no-go zone'\n"); 261 | return {path_to_safety, INCOMPLETE}; 262 | } 263 | 264 | printf("[Astar]: PATH DOES NOT EXIST!\n"); 265 | 266 | return {std::vector(), FAILURE}; 267 | } 268 | //} 269 | 270 | /* getNeighborhood() //{ */ 271 | 272 | std::vector AstarPlanner::getNeighborhood(const octomap::OcTreeKey &key, octomap::OcTree &tree) { 273 | 274 | std::vector neighbors; 275 | 276 | for (auto &d : EXPANSION_DIRECTIONS) { 277 | 278 | auto newkey = expand(key, d); 279 | auto tree_node = tree.search(newkey); 280 | 281 | if (tree_node != NULL) { 282 | // free cell? 283 | if (tree_node->getValue() == TreeValue::FREE && tree.keyToCoord(newkey).z() >= min_altitude && tree.keyToCoord(newkey).z() <= max_altitude) { 284 | neighbors.push_back(newkey); 285 | } 286 | } 287 | } 288 | 289 | return neighbors; 290 | } 291 | 292 | //} 293 | 294 | /* expand() //{ */ 295 | 296 | octomap::OcTreeKey AstarPlanner::expand(const octomap::OcTreeKey &key, const std::vector &direction) { 297 | 298 | octomap::OcTreeKey k; 299 | 300 | k.k[0] = key.k[0] + direction[0]; 301 | k.k[1] = key.k[1] + direction[1]; 302 | k.k[2] = key.k[2] + direction[2]; 303 | 304 | return k; 305 | } 306 | 307 | //} 308 | 309 | /* distEuclidean() //{ */ 310 | 311 | double AstarPlanner::distEuclidean(const octomap::point3d &p1, const octomap::point3d &p2) { 312 | 313 | return (p1 - p2).norm(); 314 | } 315 | 316 | double AstarPlanner::distEuclidean(const octomap::OcTreeKey &k1, const octomap::OcTreeKey &k2, octomap::OcTree &tree) { 317 | 318 | double voxel_dist = sqrt(pow(k1.k[0] - k2.k[0], 2) + pow(k1.k[1] - k2.k[1], 2) + pow(k1.k[2] - k2.k[2], 2)); 319 | 320 | return voxel_dist * tree.getResolution(); 321 | } 322 | 323 | //} 324 | 325 | /* freeStraightPath() //{ */ 326 | 327 | bool AstarPlanner::freeStraightPath(const octomap::point3d p1, const octomap::point3d p2, octomap::OcTree &tree) { 328 | 329 | octomap::KeyRay ray; 330 | tree.computeRayKeys(p1, p2, ray); 331 | 332 | for (auto &k : ray) { 333 | 334 | auto tree_node = tree.search(k); 335 | 336 | if (tree_node == NULL) { 337 | // Path may exist, but goes through unknown cells 338 | return false; 339 | } 340 | 341 | if (tree_node->getValue() == TreeValue::OCCUPIED) { 342 | // Path goes through occupied cells 343 | return false; 344 | } 345 | } 346 | 347 | return true; 348 | } 349 | 350 | //} 351 | 352 | /* backtrackPathKeys() //{ */ 353 | 354 | std::vector AstarPlanner::backtrackPathKeys(const Node &from, const Node &to, std::unordered_map &parent_map) { 355 | 356 | std::vector keys; 357 | 358 | Node current = from; 359 | keys.push_back(current.key); 360 | 361 | while (current.key != to.key) { 362 | current = parent_map.find(current)->second; 363 | keys.push_back(current.key); 364 | }; 365 | 366 | keys.push_back(to.key); 367 | 368 | // reverse order 369 | std::reverse(keys.begin(), keys.end()); 370 | return keys; 371 | } 372 | 373 | //} 374 | 375 | /* keysToCoords() //{ */ 376 | 377 | std::vector AstarPlanner::keysToCoords(std::vector keys, octomap::OcTree &tree) { 378 | 379 | std::vector coords; 380 | 381 | for (auto &k : keys) { 382 | coords.push_back(tree.keyToCoord(k)); 383 | } 384 | 385 | return coords; 386 | } 387 | 388 | //} 389 | 390 | /* euclideanDistanceTransform() //{ */ 391 | 392 | DynamicEDTOctomap AstarPlanner::euclideanDistanceTransform(std::shared_ptr tree) { 393 | 394 | double x, y, z; 395 | 396 | tree->getMetricMin(x, y, z); 397 | octomap::point3d metric_min(x, y, z); 398 | 399 | tree->getMetricMax(x, y, z); 400 | octomap::point3d metric_max(x, y, z); 401 | 402 | DynamicEDTOctomap edf(euclidean_distance_cutoff, tree.get(), metric_min, metric_max, unknown_is_occupied); 403 | edf.update(); 404 | 405 | return edf; 406 | } 407 | //} 408 | 409 | /* createPlanningTree() //{ */ 410 | 411 | std::optional>> AstarPlanner::createPlanningTree(std::shared_ptr tree, 412 | const octomap::point3d &start, double resolution) { 413 | 414 | auto edf = euclideanDistanceTransform(tree); 415 | octomap::OcTree binary_tree = octomap::OcTree(resolution); 416 | 417 | tree->expand(); 418 | 419 | for (auto it = tree->begin(); it != tree->end(); it++) { 420 | if (edf.getDistance(it.getCoordinate()) <= safe_obstacle_distance) { 421 | binary_tree.setNodeValue(it.getCoordinate(), TreeValue::OCCUPIED); // obstacle or close to obstacle 422 | } else { 423 | binary_tree.setNodeValue(it.getCoordinate(), TreeValue::FREE); // free and safe 424 | } 425 | } 426 | 427 | std::vector tunnel; 428 | 429 | octomap::point3d current_coords = start; 430 | auto binary_tree_query = binary_tree.search(current_coords); 431 | 432 | if (binary_tree_query != NULL && binary_tree_query->getValue() != TreeValue::FREE) { 433 | 434 | printf("[Astar]: start is inside of an inflated obstacle, tunneling out\n"); 435 | 436 | // tunnel out of expanded walls 437 | 438 | int iter1 = 0; 439 | 440 | while (rclcpp::ok() && binary_tree_query != NULL && iter1++ <= 100) { 441 | 442 | if (iter1++ > 100) { 443 | return {}; 444 | } 445 | 446 | tunnel.push_back(current_coords); 447 | binary_tree.setNodeValue(current_coords, TreeValue::FREE); 448 | 449 | float obstacle_dist; 450 | octomap::point3d closest_obstacle; 451 | 452 | edf.getDistanceAndClosestObstacle(current_coords, obstacle_dist, closest_obstacle); 453 | octomap::point3d dir_away_from_obstacle = current_coords - closest_obstacle; 454 | 455 | if (obstacle_dist >= safe_obstacle_distance) { 456 | printf("[Astar]: tunnel created with %d\n", int(tunnel.size())); 457 | break; 458 | } 459 | 460 | current_coords += dir_away_from_obstacle.normalized() * float(binary_tree.getResolution()); 461 | 462 | int iter2 = 0; 463 | 464 | while (binary_tree.search(current_coords) == binary_tree_query) { 465 | 466 | if (iter2++ > 100) { 467 | return {}; 468 | } 469 | 470 | current_coords += dir_away_from_obstacle.normalized() * float(binary_tree.getResolution()); 471 | } 472 | 473 | binary_tree_query = binary_tree.search(current_coords); 474 | } 475 | } 476 | 477 | std::pair> result = {binary_tree, tunnel}; 478 | 479 | return result; 480 | } 481 | 482 | //} 483 | 484 | /* filterPath() //{ */ 485 | 486 | std::vector AstarPlanner::filterPath(const std::vector &waypoints, octomap::OcTree &tree) { 487 | 488 | if (waypoints.size() < 3) { 489 | printf("[Astar]: Not enough points for filtering!\n"); 490 | return waypoints; 491 | } 492 | 493 | /* removing obsolete points //{ */ 494 | 495 | std::vector filtered; 496 | 497 | filtered.push_back(waypoints.front()); 498 | 499 | size_t k = 2; 500 | 501 | while (k < waypoints.size()) { 502 | 503 | if (!freeStraightPath(filtered.back(), waypoints[k], tree)) { 504 | filtered.push_back(waypoints[k - 1]); 505 | } 506 | 507 | k++; 508 | } 509 | 510 | filtered.push_back(waypoints.back()); 511 | //} 512 | 513 | return filtered; 514 | } 515 | //} 516 | 517 | /* prepareOutputPath() //{ */ 518 | 519 | std::vector AstarPlanner::prepareOutputPath(const std::vector &keys, octomap::OcTree &tree) { 520 | auto waypoints = keysToCoords(keys, tree); 521 | auto processed = filterPath(waypoints, tree); 522 | 523 | return processed; 524 | } // namespace navigation 525 | //} 526 | 527 | /* generateTemporaryGoal() //{ */ 528 | 529 | std::pair AstarPlanner::generateTemporaryGoal(const octomap::point3d &start, const octomap::point3d &goal, 530 | const octomap::point3d &pos_cmd, octomap::OcTree &tree) { 531 | 532 | bool vertical_priority = false; 533 | octomap::point3d temp_goal; 534 | 535 | // check if it is necessary to change altitude and acquire more of map 536 | if (std::abs(goal.z() - start.z()) > planning_tree_resolution) { 537 | vertical_priority = true; 538 | printf("[Astar]: give priority to vertical motion\n"); 539 | temp_goal.x() = start.x(); 540 | temp_goal.y() = start.y(); 541 | // temp_goal.z() = goal.z(); // scan new layers of octomap if needed 542 | 543 | double extra_motion = goal.z() - start.z(); 544 | extra_motion = (extra_motion / std::abs(extra_motion)) * planning_tree_resolution; 545 | 546 | // double alt_diff = pos_cmd.z() - start.z(); // odometry and desired altitude may differ 547 | 548 | temp_goal.z() = goal.z() + extra_motion; // scan new layers of octomap in that case 549 | 550 | if (temp_goal.z() > max_altitude) { 551 | printf("[Astar]: capping at max altitude\n"); 552 | temp_goal.z() = max_altitude; 553 | } 554 | if (temp_goal.z() < min_altitude) { 555 | printf("[Astar]: capping at min altitude\n"); 556 | temp_goal.z() = min_altitude; 557 | } 558 | return {temp_goal, vertical_priority}; 559 | } 560 | 561 | // try to explore unknown cells 562 | std::set, LeafComparator> leafs; 563 | 564 | for (auto it = tree.begin_leafs(); it != tree.end_leafs(); it++) { 565 | 566 | if (it->getValue() == TreeValue::OCCUPIED) { 567 | continue; 568 | } 569 | 570 | auto k = it.getKey(); 571 | k.k[2] += 1; 572 | 573 | if (tree.search(k) == NULL) { 574 | continue; 575 | } 576 | 577 | k.k[2] -= 2; 578 | 579 | if (tree.search(k) == NULL) { 580 | continue; 581 | } 582 | 583 | leafs.insert({it, distEuclidean(it.getCoordinate(), goal)}); 584 | } 585 | 586 | // sort free nodes on the map edge by their distance from goal 587 | if (!leafs.empty()) { 588 | // select the closest point 589 | return {leafs.begin()->first.getCoordinate(), vertical_priority}; 590 | } 591 | 592 | // solution that is only good for smaller obstacles 593 | octomap::KeyRay ray; 594 | tree.computeRayKeys(start, goal, ray); 595 | 596 | for (auto &k : ray) { 597 | auto coords = tree.keyToCoord(k); 598 | if (tree.search(coords) != NULL && tree.search(coords)->getValue() == TreeValue::FREE) { 599 | temp_goal = coords; 600 | } 601 | } 602 | 603 | return {temp_goal, vertical_priority}; 604 | } 605 | //} 606 | 607 | } // namespace navigation 608 | -------------------------------------------------------------------------------- /rviz/uav_monitor.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | - /TF1/Tree1 10 | - /ControlManager1/SafetyhArea1 11 | - /MpcTracker1/MpcPredictionTrajectory1 12 | - /Navigation1 13 | Splitter Ratio: 0.6941176652908325 14 | Tree Height: 835 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: LaserScan 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.20000000298023224 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 100 57 | Reference Frame: tii/stable_origin 58 | Value: true 59 | - Alpha: 0.5 60 | Cell Size: 5 61 | Class: rviz/Grid 62 | Color: 160; 160; 164 63 | Enabled: true 64 | Line Style: 65 | Line Width: 0.029999999329447746 66 | Value: Lines 67 | Name: Grid 68 | Normal Cell Count: 0 69 | Offset: 70 | X: 0 71 | Y: 0 72 | Z: 0 73 | Plane: XY 74 | Plane Cell Count: 100 75 | Reference Frame: tii/stable_origin 76 | Value: true 77 | - Class: rviz/TF 78 | Enabled: false 79 | Frame Timeout: 15 80 | Frames: 81 | All Enabled: false 82 | Marker Alpha: 1 83 | Marker Scale: 1 84 | Name: TF 85 | Show Arrows: true 86 | Show Axes: true 87 | Show Names: true 88 | Tree: 89 | {} 90 | Update Interval: 0 91 | Value: false 92 | - Alpha: 1 93 | Autocompute Intensity Bounds: true 94 | Autocompute Value Bounds: 95 | Max Value: 10 96 | Min Value: -10 97 | Value: true 98 | Axis: Z 99 | Channel Name: intensity 100 | Class: rviz/LaserScan 101 | Color: 0; 0; 0 102 | Color Transformer: FlatColor 103 | Decay Time: 0 104 | Enabled: true 105 | Invert Rainbow: false 106 | Max Color: 255; 255; 255 107 | Min Color: 0; 0; 0 108 | Name: LaserScan 109 | Position Transformer: XYZ 110 | Queue Size: 10 111 | Selectable: true 112 | Size (Pixels): 3 113 | Size (m): 0.10000000149011612 114 | Style: Flat Squares 115 | Topic: /tii/rplidar/scan 116 | Unreliable: false 117 | Use Fixed Frame: true 118 | Use rainbow: true 119 | Value: true 120 | - Alpha: 0.10000000149011612 121 | Class: mrs_rviz_plugins/Bumper 122 | Collision alpha: 0.5 123 | Collision color: 255; 0; 0 124 | Color: 204; 51; 204 125 | Colorize collisions: true 126 | Display mode: sensor types 127 | Enabled: true 128 | History Length: 1 129 | Horizontal collision threshold: 1 130 | Name: Bumper 131 | Queue Size: 10 132 | Show sectors with no data: false 133 | Show undetected obstacles: true 134 | Topic: /tii/bumper/obstacle_sectors 135 | Unreliable: false 136 | Value: true 137 | Vertical collision threshold: 1 138 | - Class: rviz/Group 139 | Displays: 140 | - Class: rviz/MarkerArray 141 | Enabled: true 142 | Marker Topic: /tii/control_manager/safety_area_markers 143 | Name: SafetyhArea 144 | Namespaces: 145 | "": true 146 | Queue Size: 100 147 | Value: true 148 | - Class: rviz/MarkerArray 149 | Enabled: false 150 | Marker Topic: /tii/control_manager/disturbances_markers 151 | Name: Disturbances 152 | Namespaces: 153 | {} 154 | Queue Size: 100 155 | Value: false 156 | - Class: rviz/MarkerArray 157 | Enabled: false 158 | Marker Topic: /tii/control_manager/safety_area_coordinates_markers 159 | Name: SafetyAreaCoordinates 160 | Namespaces: 161 | {} 162 | Queue Size: 100 163 | Value: false 164 | - Angle Tolerance: 0.009999999776482582 165 | Class: rviz/Odometry 166 | Covariance: 167 | Orientation: 168 | Alpha: 0.5 169 | Color: 255; 255; 127 170 | Color Style: Unique 171 | Frame: Local 172 | Offset: 1 173 | Scale: 1 174 | Value: true 175 | Position: 176 | Alpha: 0.30000001192092896 177 | Color: 204; 51; 204 178 | Scale: 1 179 | Value: true 180 | Value: true 181 | Enabled: true 182 | Keep: 1 183 | Name: cmd_odom 184 | Position Tolerance: 0.009999999776482582 185 | Queue Size: 10 186 | Shape: 187 | Alpha: 1 188 | Axes Length: 1 189 | Axes Radius: 0.10000000149011612 190 | Color: 255; 25; 0 191 | Head Length: 0.30000001192092896 192 | Head Radius: 0.10000000149011612 193 | Shaft Length: 1 194 | Shaft Radius: 0.05000000074505806 195 | Value: Axes 196 | Topic: /tii/control_manager/cmd_odom 197 | Unreliable: false 198 | Value: true 199 | - Class: rviz/Group 200 | Displays: 201 | - Alpha: 1 202 | Arrow Length: 0.30000001192092896 203 | Axes Length: 0.30000001192092896 204 | Axes Radius: 0.009999999776482582 205 | Class: rviz/PoseArray 206 | Color: 0; 170; 0 207 | Enabled: true 208 | Head Length: 0.07000000029802322 209 | Head Radius: 0.029999999329447746 210 | Name: Poses 211 | Queue Size: 10 212 | Shaft Length: 0.23000000417232513 213 | Shaft Radius: 0.009999999776482582 214 | Shape: Arrow (Flat) 215 | Topic: /uav1/control_manager/trajectory_original/poses 216 | Unreliable: false 217 | Value: true 218 | - Class: rviz/MarkerArray 219 | Enabled: true 220 | Marker Topic: /uav1/control_manager/trajectory_original/markers 221 | Name: Markers 222 | Namespaces: 223 | {} 224 | Queue Size: 100 225 | Value: true 226 | Enabled: true 227 | Name: OriginalTrajectory 228 | Enabled: true 229 | Name: ControlManager 230 | - Class: rviz/Group 231 | Displays: 232 | - Angle Tolerance: 0.009999999776482582 233 | Class: rviz/Odometry 234 | Covariance: 235 | Orientation: 236 | Alpha: 0.5 237 | Color: 255; 255; 127 238 | Color Style: Unique 239 | Frame: Local 240 | Offset: 1 241 | Scale: 1 242 | Value: true 243 | Position: 244 | Alpha: 0.30000001192092896 245 | Color: 204; 51; 204 246 | Scale: 1 247 | Value: true 248 | Value: true 249 | Enabled: false 250 | Keep: 1 251 | Name: position_cmd 252 | Position Tolerance: 0.009999999776482582 253 | Queue Size: 10 254 | Shape: 255 | Alpha: 1 256 | Axes Length: 1 257 | Axes Radius: 0.10000000149011612 258 | Color: 255; 0; 0 259 | Head Length: 0.30000001192092896 260 | Head Radius: 0.10000000149011612 261 | Shaft Length: 1 262 | Shaft Radius: 0.05000000074505806 263 | Value: Arrow 264 | Topic: /uav1/control_manager/cmd_odom 265 | Unreliable: true 266 | Value: false 267 | - Angle Tolerance: 0.009999999776482582 268 | Class: rviz/Odometry 269 | Covariance: 270 | Orientation: 271 | Alpha: 0.5 272 | Color: 255; 255; 127 273 | Color Style: Unique 274 | Frame: Local 275 | Offset: 1 276 | Scale: 1 277 | Value: true 278 | Position: 279 | Alpha: 0.30000001192092896 280 | Color: 204; 51; 204 281 | Scale: 1 282 | Value: true 283 | Value: false 284 | Enabled: true 285 | Keep: 1 286 | Name: odom_main 287 | Position Tolerance: 0.009999999776482582 288 | Queue Size: 10 289 | Shape: 290 | Alpha: 1 291 | Axes Length: 1 292 | Axes Radius: 0.10000000149011612 293 | Color: 0; 25; 255 294 | Head Length: 1 295 | Head Radius: 0.30000001192092896 296 | Shaft Length: 1 297 | Shaft Radius: 0.05000000074505806 298 | Value: Axes 299 | Topic: /tii/odometry/odom_main 300 | Unreliable: true 301 | Value: true 302 | - Angle Tolerance: 0.009999999776482582 303 | Class: rviz/Odometry 304 | Covariance: 305 | Orientation: 306 | Alpha: 0.5 307 | Color: 255; 255; 127 308 | Color Style: Unique 309 | Frame: Local 310 | Offset: 1 311 | Scale: 1 312 | Value: true 313 | Position: 314 | Alpha: 0.30000001192092896 315 | Color: 204; 51; 204 316 | Scale: 1 317 | Value: true 318 | Value: false 319 | Enabled: true 320 | Keep: 1 321 | Name: odom_main 322 | Position Tolerance: 0.009999999776482582 323 | Queue Size: 10 324 | Shape: 325 | Alpha: 1 326 | Axes Length: 1 327 | Axes Radius: 0.10000000149011612 328 | Color: 0; 25; 255 329 | Head Length: 1 330 | Head Radius: 0.30000001192092896 331 | Shaft Length: 1 332 | Shaft Radius: 0.05000000074505806 333 | Value: Arrow 334 | Topic: /tii/odometry/odom_main 335 | Unreliable: true 336 | Value: true 337 | Enabled: true 338 | Name: Odometry 339 | - Class: rviz/Group 340 | Displays: 341 | - Alpha: 1 342 | Arrow Length: 0.30000001192092896 343 | Axes Length: 0.30000001192092896 344 | Axes Radius: 0.009999999776482582 345 | Class: rviz/PoseArray 346 | Color: 0; 0; 255 347 | Enabled: true 348 | Head Length: 0.07000000029802322 349 | Head Radius: 0.029999999329447746 350 | Name: MpcPredictionTrajectory 351 | Queue Size: 10 352 | Shaft Length: 0.23000000417232513 353 | Shaft Radius: 0.009999999776482582 354 | Shape: Arrow (Flat) 355 | Topic: /tii/control_manager/mpc_tracker/predicted_trajectory_debugging 356 | Unreliable: true 357 | Value: true 358 | - Angle Tolerance: 0.0010000000474974513 359 | Class: rviz/Odometry 360 | Covariance: 361 | Orientation: 362 | Alpha: 0.5 363 | Color: 255; 255; 127 364 | Color Style: Unique 365 | Frame: Local 366 | Offset: 1 367 | Scale: 1 368 | Value: true 369 | Position: 370 | Alpha: 0.30000001192092896 371 | Color: 204; 51; 204 372 | Scale: 1 373 | Value: true 374 | Value: true 375 | Enabled: false 376 | Keep: 1 377 | Name: MpcTrackerReference 378 | Position Tolerance: 0.0010000000474974513 379 | Queue Size: 10 380 | Shape: 381 | Alpha: 1 382 | Axes Length: 1 383 | Axes Radius: 0.10000000149011612 384 | Color: 255; 25; 0 385 | Head Length: 0.30000001192092896 386 | Head Radius: 0.10000000149011612 387 | Shaft Length: 1 388 | Shaft Radius: 0.05000000074505806 389 | Value: Axes 390 | Topic: /tii/control_manager/mpc_tracker/setpoint_odom 391 | Unreliable: false 392 | Value: false 393 | - Alpha: 1 394 | Arrow Length: 0.30000001192092896 395 | Axes Length: 0.10000000149011612 396 | Axes Radius: 0.05000000074505806 397 | Class: rviz/PoseArray 398 | Color: 255; 25; 0 399 | Enabled: true 400 | Head Length: 0.07000000029802322 401 | Head Radius: 0.029999999329447746 402 | Name: MpcInnerReference 403 | Queue Size: 10 404 | Shaft Length: 0.23000000417232513 405 | Shaft Radius: 0.009999999776482582 406 | Shape: Axes 407 | Topic: /tii/control_manager/mpc_tracker/mpc_reference_debugging 408 | Unreliable: false 409 | Value: true 410 | - Class: rviz/Group 411 | Displays: 412 | - Alpha: 1 413 | Arrow Length: 0.30000001192092896 414 | Axes Length: 0.30000001192092896 415 | Axes Radius: 0.009999999776482582 416 | Class: rviz/PoseArray 417 | Color: 255; 25; 0 418 | Enabled: true 419 | Head Length: 0.07000000029802322 420 | Head Radius: 0.029999999329447746 421 | Name: Poses 422 | Queue Size: 10 423 | Shaft Length: 0.23000000417232513 424 | Shaft Radius: 0.009999999776482582 425 | Shape: Arrow (Flat) 426 | Topic: /uav1/control_manager/mpc_tracker/trajectory_processed/poses 427 | Unreliable: false 428 | Value: true 429 | - Class: rviz/MarkerArray 430 | Enabled: true 431 | Marker Topic: /uav1/control_manager/mpc_tracker/trajectory_processed/markers 432 | Name: Markers 433 | Namespaces: 434 | {} 435 | Queue Size: 100 436 | Value: true 437 | Enabled: true 438 | Name: PostprocessedTrajectory 439 | Enabled: true 440 | Name: MpcTracker 441 | - Alpha: 0.699999988079071 442 | Class: rviz/Map 443 | Color Scheme: map 444 | Draw Behind: false 445 | Enabled: false 446 | Name: HectorMap 447 | Topic: /tii/hector_mapping/map 448 | Unreliable: false 449 | Use Timestamp: false 450 | Value: false 451 | - Alpha: 1 452 | Autocompute Intensity Bounds: true 453 | Autocompute Value Bounds: 454 | Max Value: 3.299999952316284 455 | Min Value: 0.699999988079071 456 | Value: true 457 | Axis: Z 458 | Channel Name: intensity 459 | Class: rviz/PointCloud2 460 | Color: 255; 255; 255 461 | Color Transformer: AxisColor 462 | Decay Time: 0 463 | Enabled: true 464 | Invert Rainbow: false 465 | Max Color: 255; 255; 255 466 | Min Color: 0; 0; 0 467 | Name: OccupiedCells 468 | Position Transformer: XYZ 469 | Queue Size: 10 470 | Selectable: true 471 | Size (Pixels): 3 472 | Size (m): 0.20000000298023224 473 | Style: Boxes 474 | Topic: /tii/octomap_server/octomap_point_cloud_centers 475 | Unreliable: false 476 | Use Fixed Frame: true 477 | Use rainbow: true 478 | Value: true 479 | - Class: rviz/Group 480 | Displays: 481 | - Alpha: 1 482 | Buffer Length: 1 483 | Class: rviz/Path 484 | Color: 25; 255; 0 485 | Enabled: true 486 | Head Diameter: 0.30000001192092896 487 | Head Length: 0.20000000298023224 488 | Length: 0.30000001192092896 489 | Line Style: Lines 490 | Line Width: 0.029999999329447746 491 | Name: Path 492 | Offset: 493 | X: 0 494 | Y: 0 495 | Z: 0 496 | Pose Color: 255; 85; 255 497 | Pose Style: None 498 | Queue Size: 10 499 | Radius: 0.029999999329447746 500 | Shaft Diameter: 0.10000000149011612 501 | Shaft Length: 0.10000000149011612 502 | Topic: /waypoints 503 | Unreliable: false 504 | Value: true 505 | - Class: rviz/Marker 506 | Enabled: true 507 | Marker Topic: /tii/navigation/goal_markers_out 508 | Name: GoalMarker 509 | Namespaces: 510 | goals: true 511 | Queue Size: 100 512 | Value: true 513 | - Class: rviz/Marker 514 | Enabled: true 515 | Marker Topic: /tii/navigation/binary_tree_markers_out 516 | Name: PlanningTree 517 | Namespaces: 518 | tree: true 519 | Queue Size: 100 520 | Value: true 521 | - Class: rviz/Marker 522 | Enabled: true 523 | Marker Topic: /tii/navigation/expansion_markers_out 524 | Name: Expansions 525 | Namespaces: 526 | expansions: true 527 | Queue Size: 100 528 | Value: true 529 | - Class: rviz/Marker 530 | Enabled: true 531 | Marker Topic: /tii/navigation/path_markers_out 532 | Name: PathMarkers 533 | Namespaces: 534 | {} 535 | Queue Size: 100 536 | Value: true 537 | Enabled: true 538 | Name: Navigation 539 | Enabled: true 540 | Global Options: 541 | Background Color: 255; 255; 255 542 | Default Light: true 543 | Fixed Frame: tii/stable_origin 544 | Frame Rate: 60 545 | Name: root 546 | Tools: 547 | - Class: rviz/Interact 548 | Hide Inactive Objects: true 549 | - Class: rviz/MoveCamera 550 | - Class: rviz/Select 551 | - Class: rviz/FocusCamera 552 | - Class: rviz/Measure 553 | - Class: rviz/SetInitialPose 554 | Theta std deviation: 0.2617993950843811 555 | Topic: /initialpose 556 | X std deviation: 0.5 557 | Y std deviation: 0.5 558 | - Class: rviz/SetGoal 559 | Topic: /move_base_simple/goal 560 | - Class: rviz/PublishPoint 561 | Single click: true 562 | Topic: /clicked_point 563 | Value: true 564 | Views: 565 | Current: 566 | Class: rviz/ThirdPersonFollower 567 | Distance: 21.227123260498047 568 | Enable Stereo Rendering: 569 | Stereo Eye Separation: 0.05999999865889549 570 | Stereo Focal Distance: 1 571 | Swap Stereo Eyes: false 572 | Value: false 573 | Field of View: 0.7853981852531433 574 | Focal Point: 575 | X: 32.12359619140625 576 | Y: 18.160879135131836 577 | Z: -2.0057761503267102e-05 578 | Focal Shape Fixed Size: false 579 | Focal Shape Size: 0.05000000074505806 580 | Invert Z Axis: false 581 | Name: Current View 582 | Near Clip Distance: 0.009999999776482582 583 | Pitch: 0.8997982740402222 584 | Target Frame: uav1/fcu 585 | Yaw: 4.070415019989014 586 | Saved: ~ 587 | Window Geometry: 588 | Displays: 589 | collapsed: false 590 | Height: 1050 591 | Hide Left Dock: false 592 | Hide Right Dock: true 593 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003c4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000004c6000001a1000001a900000153fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003c4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006502000003c9000003450000025b000000c90000000100000116000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000038f0000003efc0100000002fb0000000800540069006d006500000000000000038f0000023f00fffffffb0000000800540069006d006501000000000000045000000000000000000000025b000003c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 594 | Selection: 595 | collapsed: false 596 | Time: 597 | collapsed: false 598 | Tool Properties: 599 | collapsed: false 600 | Views: 601 | collapsed: true 602 | Width: 951 603 | X: 961 604 | Y: 22 605 | -------------------------------------------------------------------------------- /src/navigation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | using namespace std::placeholders; 39 | 40 | namespace navigation 41 | { 42 | 43 | enum status_t 44 | { 45 | IDLE = 0, 46 | PLANNING, 47 | COMMANDING, 48 | MOVING, 49 | AVOIDING 50 | }; 51 | 52 | enum waypoint_status_t 53 | { 54 | EMPTY = 0, 55 | ONGOING, 56 | REACHED, 57 | UNREACHABLE 58 | }; 59 | 60 | const std::string STATUS_STRING[] = {"IDLE", "PLANNING", "COMMANDING", "MOVING", "AVOIDING"}; 61 | const std::string WAYPOINT_STATUS_STRING[] = {"EMPTY", "ONGOING", "REACHED", "UNREACHABLE"}; 62 | 63 | double getYaw(const geometry_msgs::msg::Quaternion &q) { 64 | return atan2(2.0 * (q.z * q.w + q.x * q.y), -1.0 + 2.0 * (q.w * q.w + q.x * q.x)); 65 | } 66 | 67 | geometry_msgs::msg::Quaternion yawToQuaternionMsg(const double &yaw) { 68 | geometry_msgs::msg::Quaternion msg; 69 | Eigen::Quaterniond q = 70 | Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); 71 | msg.w = q.w(); 72 | msg.x = q.x(); 73 | msg.y = q.y(); 74 | msg.z = q.z(); 75 | return msg; 76 | } 77 | 78 | octomap::point3d toPoint3d(const Eigen::Vector4d &vec) { 79 | octomap::point3d p; 80 | p.x() = vec.x(); 81 | p.y() = vec.y(); 82 | p.z() = vec.z(); 83 | return p; 84 | } 85 | 86 | double nanosecondsToSecs(const int64_t nanoseconds) { 87 | return nanoseconds / 1e9; 88 | } 89 | 90 | /* class Navigation //{ */ 91 | class Navigation : public rclcpp::Node { 92 | public: 93 | Navigation(rclcpp::NodeOptions options); 94 | 95 | private: 96 | // internal variables 97 | bool is_initialized_ = false; 98 | bool getting_octomap_ = false; 99 | bool getting_odometry_ = false; 100 | bool getting_desired_pose_ = false; 101 | bool getting_control_diagnostics_ = false; 102 | bool visualize_planner_ = true; 103 | bool show_unoccupied_ = false; 104 | bool override_previous_commands_ = false; 105 | bool bumper_active_ = false; 106 | 107 | std::string parent_frame_; 108 | int replanning_counter_ = 0; 109 | 110 | bool control_moving_ = false; 111 | bool goal_reached_ = false; 112 | bool hover_requested_ = false; 113 | 114 | Eigen::Vector4d uav_pos_; 115 | Eigen::Vector4d desired_pose_; 116 | Eigen::Vector4d current_goal_; 117 | Eigen::Vector4d last_goal_; 118 | std::mutex octree_mutex_; 119 | std::mutex bumper_mutex_; 120 | std::mutex control_diagnostics_mutex_; 121 | std::shared_ptr octree_; 122 | status_t status_ = IDLE; 123 | waypoint_status_t waypoint_status_ = EMPTY; 124 | 125 | std::vector waypoint_out_buffer_; 126 | std::deque waypoint_in_buffer_; 127 | size_t current_waypoint_id_; 128 | 129 | rclcpp::CallbackGroup::SharedPtr callback_group_; 130 | rclcpp::TimerBase::SharedPtr execution_timer_; 131 | void navigationRoutine(void); 132 | 133 | std::unique_ptr bumper_msg_; 134 | 135 | // params 136 | double euclidean_distance_cutoff_; 137 | double safe_obstacle_distance_; 138 | double bumper_distance_factor_; 139 | double navigation_tolerance_; 140 | bool unknown_is_occupied_; 141 | double min_altitude_; 142 | double max_altitude_; 143 | double max_goal_distance_; 144 | double distance_penalty_; 145 | double greedy_penalty_; 146 | double planning_tree_resolution_; 147 | double max_waypoint_distance_; 148 | double max_yaw_step_; 149 | double planning_timeout_; 150 | int replanning_limit_; 151 | double replanning_distance_; 152 | double main_update_rate_; 153 | bool bumper_enabled_; 154 | bool diagnostics_received_ = false; 155 | 156 | // visualization params 157 | double tree_points_scale_; 158 | double field_points_scale_; 159 | double expansions_points_scale_; 160 | double path_points_scale_; 161 | double goal_points_scale_; 162 | 163 | // publishers 164 | rclcpp::Publisher::SharedPtr field_publisher_; 165 | rclcpp::Publisher::SharedPtr binary_tree_publisher_; 166 | rclcpp::Publisher::SharedPtr expansion_publisher_; 167 | rclcpp::Publisher::SharedPtr path_publisher_; 168 | rclcpp::Publisher::SharedPtr goal_publisher_; 169 | 170 | rclcpp::Publisher::SharedPtr status_publisher_; 171 | rclcpp::Publisher::SharedPtr future_trajectory_publisher_; 172 | rclcpp::Publisher::SharedPtr diagnostics_publisher_; 173 | 174 | // subscribers 175 | rclcpp::Subscription::SharedPtr octomap_subscriber_; 176 | rclcpp::Subscription::SharedPtr odometry_subscriber_; 177 | rclcpp::Subscription::SharedPtr desired_pose_subscriber_; 178 | rclcpp::Subscription::SharedPtr goto_subscriber_; 179 | rclcpp::Subscription::SharedPtr control_diagnostics_subscriber_; 180 | rclcpp::Subscription::SharedPtr bumper_subscriber_; 181 | 182 | // subscriber callbacks 183 | void octomapCallback(const octomap_msgs::msg::Octomap::UniquePtr msg); 184 | void odometryCallback(const nav_msgs::msg::Odometry::UniquePtr msg); 185 | void desiredPoseCallback(const geometry_msgs::msg::PoseStamped::UniquePtr msg); 186 | void gotoCallback(const nav_msgs::msg::Path::UniquePtr msg); 187 | void controlDiagnosticsCallback(const fog_msgs::msg::ControlInterfaceDiagnostics::UniquePtr msg); 188 | void bumperCallback(const fog_msgs::msg::ObstacleSectors::UniquePtr msg); 189 | 190 | // services provided 191 | rclcpp::Service::SharedPtr goto_trigger_service_; 192 | rclcpp::Service::SharedPtr hover_service_; 193 | 194 | rclcpp::Service::SharedPtr local_waypoint_service_; 195 | rclcpp::Service::SharedPtr local_path_service_; 196 | rclcpp::Service::SharedPtr gps_waypoint_service_; 197 | rclcpp::Service::SharedPtr gps_path_service_; 198 | 199 | 200 | rclcpp::Client::SharedPtr local_path_client_; 201 | // rclcpp::Client::SharedPtr gps_path_client_; 202 | rclcpp::Client::SharedPtr waypoint_to_local_client_; 203 | rclcpp::Client::SharedPtr path_to_local_client_; 204 | 205 | // service callbacks 206 | bool gotoTriggerCallback(const std::shared_ptr request, std::shared_ptr response); 207 | bool hoverCallback(const std::shared_ptr request, std::shared_ptr response); 208 | 209 | bool localWaypointCallback(const std::shared_ptr request, std::shared_ptr response); 210 | bool localPathCallback(const std::shared_ptr request, std::shared_ptr response); 211 | bool gpsWaypointCallback(const std::shared_ptr request, std::shared_ptr response); 212 | bool gpsPathCallback(const std::shared_ptr request, std::shared_ptr response); 213 | 214 | // future callback 215 | bool waypointFutureCallback(rclcpp::Client::SharedFuture future); 216 | bool pathFutureCallback(rclcpp::Client::SharedFuture future); 217 | 218 | // visualization 219 | void visualizeTree(const octomap::OcTree &tree); 220 | void visualizeExpansions(const std::unordered_set open, const std::unordered_set &closed, 221 | const octomap::OcTree &tree); 222 | void visualizePath(const std::vector &waypoints); 223 | void visualizeGoals(const std::deque &waypoints); 224 | 225 | std_msgs::msg::ColorRGBA generateColor(const double r, const double g, const double b, const double a); 226 | 227 | std::vector resamplePath(const std::vector &waypoints, const double start_yaw, const double end_yaw); 228 | 229 | std::shared_ptr waypointsToPathSrv(const std::vector &waypoints, bool use_first = true); 230 | void hover(); 231 | 232 | void publishDiagnostics(); 233 | void publishFutureTrajectory(std::vector waypoints); 234 | 235 | bool bumperCheckObstacles(const fog_msgs::msg::ObstacleSectors &bumper_msg); 236 | Eigen::Vector3d bumperGetAvoidanceVector(const fog_msgs::msg::ObstacleSectors &bumper_msg); 237 | 238 | template 239 | bool parse_param(const std::string ¶m_name, T ¶m_dest); 240 | }; 241 | //} 242 | 243 | /* constructor //{ */ 244 | Navigation::Navigation(rclcpp::NodeOptions options) : Node("navigation", options) { 245 | 246 | RCLCPP_INFO(this->get_logger(), "[%s]: Initializing...", this->get_name()); 247 | 248 | /* parse params from config file //{ */ 249 | RCLCPP_INFO(this->get_logger(), "-------------- Loading parameters --------------"); 250 | bool loaded_successfully = true; 251 | loaded_successfully &= parse_param("planning.euclidean_distance_cutoff", euclidean_distance_cutoff_); 252 | loaded_successfully &= parse_param("planning.safe_obstacle_distance", safe_obstacle_distance_); 253 | loaded_successfully &= parse_param("planning.bumper_distance_factor", bumper_distance_factor_); 254 | loaded_successfully &= parse_param("planning.unknown_is_occupied", unknown_is_occupied_); 255 | loaded_successfully &= parse_param("planning.navigation_tolerance", navigation_tolerance_); 256 | loaded_successfully &= parse_param("planning.min_altitude", min_altitude_); 257 | loaded_successfully &= parse_param("planning.max_altitude", max_altitude_); 258 | loaded_successfully &= parse_param("planning.max_goal_distance", max_goal_distance_); 259 | loaded_successfully &= parse_param("planning.distance_penalty", distance_penalty_); 260 | loaded_successfully &= parse_param("planning.greedy_penalty", greedy_penalty_); 261 | loaded_successfully &= parse_param("planning.planning_tree_resolution", planning_tree_resolution_); 262 | loaded_successfully &= parse_param("planning.max_waypoint_distance", max_waypoint_distance_); 263 | loaded_successfully &= parse_param("planning.max_yaw_step", max_yaw_step_); 264 | loaded_successfully &= parse_param("planning.planning_timeout", planning_timeout_); 265 | loaded_successfully &= parse_param("planning.replanning_limit", replanning_limit_); 266 | loaded_successfully &= parse_param("planning.replanning_distance", replanning_distance_); 267 | loaded_successfully &= parse_param("planning.override_previous_commands", override_previous_commands_); 268 | loaded_successfully &= parse_param("planning.main_update_rate", main_update_rate_); 269 | 270 | loaded_successfully &= parse_param("visualization.visualize_planner", visualize_planner_); 271 | loaded_successfully &= parse_param("visualization.show_unoccupied", show_unoccupied_); 272 | loaded_successfully &= parse_param("visualization.tree_points_scale", tree_points_scale_); 273 | loaded_successfully &= parse_param("visualization.field_points_scale", field_points_scale_); 274 | loaded_successfully &= parse_param("visualization.expansions_points_scale", expansions_points_scale_); 275 | loaded_successfully &= parse_param("visualization.path_points_scale", path_points_scale_); 276 | loaded_successfully &= parse_param("visualization.goal_points_scale", goal_points_scale_); 277 | 278 | loaded_successfully &= parse_param("bumper.enabled", bumper_enabled_); 279 | 280 | if (!loaded_successfully) { 281 | const std::string str = "Could not load all non-optional parameters. Shutting down."; 282 | RCLCPP_ERROR(this->get_logger(), str.c_str()); 283 | rclcpp::shutdown(); 284 | return; 285 | } 286 | //} 287 | 288 | current_waypoint_id_ = 0; 289 | bumper_msg_.reset(); 290 | 291 | callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); 292 | auto sub_opt = rclcpp::SubscriptionOptions(); 293 | sub_opt.callback_group = callback_group_; 294 | 295 | // publishers 296 | field_publisher_ = this->create_publisher("~/field_markers_out", 1); 297 | binary_tree_publisher_ = this->create_publisher("~/binary_tree_markers_out", 1); 298 | expansion_publisher_ = this->create_publisher("~/expansion_markers_out", 1); 299 | path_publisher_ = this->create_publisher("~/path_markers_out", 1); 300 | goal_publisher_ = this->create_publisher("~/goal_markers_out", 1); 301 | status_publisher_ = this->create_publisher("~/status_out", 1); 302 | future_trajectory_publisher_ = this->create_publisher("~/future_trajectory_out", 1); 303 | diagnostics_publisher_ = this->create_publisher("~/diagnostics_out", 5); 304 | 305 | // subscribers 306 | odometry_subscriber_ = this->create_subscription("~/odometry_in", 1, std::bind(&Navigation::odometryCallback, this, _1)); 307 | desired_pose_subscriber_ = 308 | this->create_subscription("~/desired_pose_in", 1, std::bind(&Navigation::desiredPoseCallback, this, _1)); 309 | goto_subscriber_ = this->create_subscription("~/goto_in", 1, std::bind(&Navigation::gotoCallback, this, _1)); 310 | control_diagnostics_subscriber_ = this->create_subscription( 311 | "~/control_diagnostics_in", 1, std::bind(&Navigation::controlDiagnosticsCallback, this, _1)); 312 | octomap_subscriber_ = this->create_subscription("~/octomap_in", 1, std::bind(&Navigation::octomapCallback, this, _1), sub_opt); 313 | bumper_subscriber_ = this->create_subscription("~/bumper_in", 1, std::bind(&Navigation::bumperCallback, this, _1), sub_opt); 314 | 315 | // clients 316 | local_path_client_ = this->create_client("~/local_path_out"); 317 | // gps_path_client_ = this->create_client("~/gps_path_out"); 318 | waypoint_to_local_client_ = this->create_client("~/waypoint_to_local_out"); 319 | path_to_local_client_ = this->create_client("~/path_to_local_out"); 320 | 321 | // service handlers 322 | goto_trigger_service_ = this->create_service("~/goto_trigger_in", std::bind(&Navigation::gotoTriggerCallback, this, _1, _2)); 323 | hover_service_ = this->create_service("~/hover_in", std::bind(&Navigation::hoverCallback, this, _1, _2)); 324 | local_waypoint_service_ = this->create_service("~/local_waypoint_in", std::bind(&Navigation::localWaypointCallback, this, _1, _2)); 325 | local_path_service_ = this->create_service("~/local_path_in", std::bind(&Navigation::localPathCallback, this, _1, _2)); 326 | gps_waypoint_service_ = this->create_service("~/gps_waypoint_in", std::bind(&Navigation::gpsWaypointCallback, this, _1, _2)); 327 | gps_path_service_ = this->create_service("~/gps_path_in", std::bind(&Navigation::gpsPathCallback, this, _1, _2)); 328 | 329 | // timers 330 | execution_timer_ = 331 | this->create_wall_timer(std::chrono::duration(1.0 / main_update_rate_), std::bind(&Navigation::navigationRoutine, this), callback_group_); 332 | 333 | if (max_waypoint_distance_ <= 0) { 334 | max_waypoint_distance_ = replanning_distance_; 335 | } 336 | 337 | is_initialized_ = true; 338 | RCLCPP_INFO(this->get_logger(), "[%s]: Initialized", this->get_name()); 339 | } 340 | //} 341 | 342 | /* octomapCallback //{ */ 343 | void Navigation::octomapCallback(const octomap_msgs::msg::Octomap::UniquePtr msg) { 344 | getting_octomap_ = true; 345 | parent_frame_ = msg->header.frame_id; 346 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Getting octomap", this->get_name()); 347 | octomap_msgs::msg::Octomap map_msg = *msg; 348 | 349 | auto treePtr = octomap_msgs::fullMsgToMap(*msg); 350 | 351 | if (!treePtr) { 352 | RCLCPP_WARN(this->get_logger(), "[%s]: Octomap message is empty!", this->get_name()); 353 | } else { 354 | std::scoped_lock lock(octree_mutex_); 355 | octree_ = std::shared_ptr(dynamic_cast(treePtr)); 356 | } 357 | } 358 | //} 359 | 360 | /* odometryCallback //{ */ 361 | void Navigation::odometryCallback(const nav_msgs::msg::Odometry::UniquePtr msg) { 362 | if (!is_initialized_) { 363 | return; 364 | } 365 | 366 | getting_odometry_ = true; 367 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Getting odometry", this->get_name()); 368 | uav_pos_[0] = msg->pose.pose.position.x; 369 | uav_pos_[1] = msg->pose.pose.position.y; 370 | uav_pos_[2] = msg->pose.pose.position.z; 371 | uav_pos_[3] = getYaw(msg->pose.pose.orientation); 372 | } 373 | //} 374 | 375 | /* desiredPoseCallback //{ */ 376 | void Navigation::desiredPoseCallback(const geometry_msgs::msg::PoseStamped::UniquePtr msg) { 377 | if (!is_initialized_) { 378 | return; 379 | } 380 | 381 | getting_desired_pose_ = true; 382 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Getting desired pose", this->get_name()); 383 | desired_pose_[0] = msg->pose.position.x; 384 | desired_pose_[1] = msg->pose.position.y; 385 | desired_pose_[2] = msg->pose.position.z; 386 | desired_pose_[3] = getYaw(msg->pose.orientation); 387 | } 388 | //} 389 | 390 | /* controlDiagnosticsCallback //{ */ 391 | void Navigation::controlDiagnosticsCallback(const fog_msgs::msg::ControlInterfaceDiagnostics::UniquePtr msg) { 392 | std::scoped_lock lock(control_diagnostics_mutex_); 393 | if (!is_initialized_) { 394 | return; 395 | } 396 | getting_control_diagnostics_ = true; 397 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Getting control_interface diagnostics", this->get_name()); 398 | control_moving_ = msg->moving; 399 | goal_reached_ = msg->mission_finished; 400 | diagnostics_received_ = true; 401 | } 402 | //} 403 | 404 | /* bumperCallback //{ */ 405 | void Navigation::bumperCallback(const fog_msgs::msg::ObstacleSectors::UniquePtr msg) { 406 | if (!is_initialized_) { 407 | return; 408 | } 409 | 410 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Getting bumper msgs", this->get_name()); 411 | std::scoped_lock lock(bumper_mutex_); 412 | bumper_msg_ = std::unique_ptr{new fog_msgs::msg::ObstacleSectors{*msg}}; 413 | } 414 | //} 415 | 416 | /* gotoCallback //{ */ 417 | void Navigation::gotoCallback(const nav_msgs::msg::Path::UniquePtr msg) { 418 | 419 | if (!is_initialized_) { 420 | RCLCPP_ERROR(this->get_logger(), "[%s]: Goto rejected, node not initialized", this->get_name()); 421 | return; 422 | } 423 | 424 | if (!getting_octomap_) { 425 | RCLCPP_ERROR(this->get_logger(), "[%s]: Goto rejected, octomap not received", this->get_name()); 426 | return; 427 | } 428 | 429 | if (!getting_control_diagnostics_) { 430 | RCLCPP_ERROR(this->get_logger(), "[%s]: Goto rejected, control_interface diagnostics not received", this->get_name()); 431 | return; 432 | } 433 | 434 | if (msg->poses.empty()) { 435 | RCLCPP_ERROR(this->get_logger(), "[%s]: Goto rejected, path input does not contain any waypoints", this->get_name()); 436 | return; 437 | } 438 | 439 | if (status_ != IDLE) { 440 | if (!override_previous_commands_) { 441 | RCLCPP_ERROR(this->get_logger(), "[%s]: Goto rejected, vehicle not IDLE", this->get_name()); 442 | return; 443 | } else { 444 | RCLCPP_INFO(this->get_logger(), "[%s]: Override previous navigation commands", this->get_name()); 445 | hover(); 446 | } 447 | } 448 | 449 | RCLCPP_INFO(this->get_logger(), "[%s]: Recieved %ld waypoints", this->get_name(), msg->poses.size()); 450 | 451 | waypoint_in_buffer_.clear(); 452 | current_waypoint_id_ = 0; 453 | for (const auto &p : msg->poses) { 454 | Eigen::Vector4d point; 455 | point[0] = p.pose.position.x; 456 | point[1] = p.pose.position.y; 457 | point[2] = p.pose.position.z; 458 | point[3] = getYaw(p.pose.orientation); 459 | waypoint_in_buffer_.push_back(point); 460 | } 461 | RCLCPP_INFO(this->get_logger(), "[%s]: Waiting for planning trigger", this->get_name()); 462 | } 463 | //} 464 | 465 | /* gotoTriggerCallback //{ */ 466 | bool Navigation::gotoTriggerCallback([[maybe_unused]] const std::shared_ptr request, 467 | std::shared_ptr response) { 468 | if (!is_initialized_) { 469 | response->message = "Goto rejected, node not initialized"; 470 | response->success = false; 471 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 472 | return true; 473 | } 474 | 475 | if (!getting_octomap_) { 476 | response->message = "Goto rejected, octomap not received"; 477 | response->success = false; 478 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 479 | return true; 480 | } 481 | 482 | if (!getting_control_diagnostics_) { 483 | response->message = "Goto rejected, control_interface diagnostics not received"; 484 | response->success = false; 485 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 486 | return true; 487 | } 488 | 489 | if (current_waypoint_id_ >= waypoint_in_buffer_.size() || waypoint_in_buffer_.empty()) { 490 | response->message = "Goto rejected, no waypoint provided"; 491 | response->success = false; 492 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 493 | return true; 494 | } 495 | 496 | if (status_ != IDLE) { 497 | if (!override_previous_commands_) { 498 | RCLCPP_ERROR(this->get_logger(), "[%s]: Goto rejected, vehicle not IDLE", this->get_name()); 499 | return true; 500 | } else { 501 | RCLCPP_INFO(this->get_logger(), "[%s]: Override previous navigation commands", this->get_name()); 502 | hover(); 503 | } 504 | } 505 | 506 | response->message = "Planning started"; 507 | response->success = true; 508 | RCLCPP_INFO(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 509 | status_ = PLANNING; 510 | return true; 511 | } 512 | //} 513 | 514 | /* localPathCallback //{ */ 515 | bool Navigation::localPathCallback([[maybe_unused]] const std::shared_ptr request, 516 | std::shared_ptr response) { 517 | if (!is_initialized_) { 518 | response->message = "Path rejected, node not initialized"; 519 | response->success = false; 520 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 521 | return true; 522 | } 523 | 524 | if (!getting_octomap_) { 525 | response->message = "Path rejected, octomap not received"; 526 | response->success = false; 527 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 528 | return true; 529 | } 530 | 531 | if (!getting_control_diagnostics_) { 532 | response->message = "Path rejected, control_interface diagnostics not received"; 533 | response->success = false; 534 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 535 | return true; 536 | } 537 | 538 | if (request->path.poses.empty()) { 539 | response->message = "Path rejected, path input does not contain any waypoints"; 540 | response->success = false; 541 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 542 | return true; 543 | } 544 | 545 | if (status_ != IDLE) { 546 | if (!override_previous_commands_) { 547 | response->message = "Path rejected, vehicle not IDLE"; 548 | response->success = false; 549 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 550 | return true; 551 | } else { 552 | RCLCPP_INFO(this->get_logger(), "[%s]: Override previous navigation commands", this->get_name()); 553 | hover(); 554 | } 555 | } 556 | 557 | waypoint_in_buffer_.clear(); 558 | current_waypoint_id_ = 0; 559 | for (const auto &p : request->path.poses) { 560 | Eigen::Vector4d point; 561 | point[0] = p.pose.position.x; 562 | point[1] = p.pose.position.y; 563 | point[2] = p.pose.position.z; 564 | point[3] = getYaw(p.pose.orientation); 565 | waypoint_in_buffer_.push_back(point); 566 | } 567 | 568 | RCLCPP_INFO(this->get_logger(), "[%s]: Recieved %ld waypoints. Planning started", this->get_name(), request->path.poses.size()); 569 | response->message = "Planning started"; 570 | response->success = true; 571 | status_ = PLANNING; 572 | return true; 573 | } 574 | //} 575 | 576 | /* gpsPathCallback //{ */ 577 | bool Navigation::gpsPathCallback([[maybe_unused]] const std::shared_ptr request, 578 | std::shared_ptr response) { 579 | if (!is_initialized_) { 580 | response->message = "Path rejected, node not initialized"; 581 | response->success = false; 582 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 583 | return true; 584 | } 585 | 586 | if (!getting_octomap_) { 587 | response->message = "Path rejected, octomap not received"; 588 | response->success = false; 589 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 590 | return true; 591 | } 592 | 593 | if (!getting_control_diagnostics_) { 594 | response->message = "Path rejected, control_interface diagnostics not received"; 595 | response->success = false; 596 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 597 | return true; 598 | } 599 | 600 | if (request->path.poses.empty()) { 601 | response->message = "Path rejected, path input does not contain any waypoints"; 602 | response->success = false; 603 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 604 | return true; 605 | } 606 | 607 | if (status_ != IDLE) { 608 | if (!override_previous_commands_) { 609 | response->message = "Path rejected, vehicle not IDLE"; 610 | response->success = false; 611 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 612 | return true; 613 | } else { 614 | RCLCPP_INFO(this->get_logger(), "[%s]: Override previous navigation commands", this->get_name()); 615 | hover(); 616 | } 617 | } 618 | 619 | auto convert_path_srv = std::make_shared(); 620 | convert_path_srv->path = request->path; 621 | RCLCPP_INFO(this->get_logger(), "[%s]: Calling Path transform", this->get_name()); 622 | auto call_result = path_to_local_client_->async_send_request(convert_path_srv, std::bind(&Navigation::pathFutureCallback, this, std::placeholders::_1)); 623 | 624 | response->message = "Processing path"; 625 | response->success = true; 626 | return true; 627 | } 628 | //} 629 | 630 | /* localWaypointCallback //{ */ 631 | bool Navigation::localWaypointCallback([[maybe_unused]] const std::shared_ptr request, 632 | std::shared_ptr response) { 633 | if (!is_initialized_) { 634 | response->message = "Goto rejected, node not initialized"; 635 | response->success = false; 636 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 637 | return true; 638 | } 639 | 640 | if (!getting_octomap_) { 641 | response->message = "Goto rejected, octomap not received"; 642 | response->success = false; 643 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 644 | return true; 645 | } 646 | 647 | if (!getting_control_diagnostics_) { 648 | response->message = "Goto rejected, control_interface diagnostics not received"; 649 | response->success = false; 650 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 651 | return true; 652 | } 653 | 654 | if (status_ != IDLE) { 655 | if (!override_previous_commands_) { 656 | response->message = "Goto rejected, vehicle not IDLE"; 657 | response->success = false; 658 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 659 | return true; 660 | } else { 661 | RCLCPP_INFO(this->get_logger(), "[%s]: Override previous navigation commands", this->get_name()); 662 | hover(); 663 | } 664 | } 665 | 666 | waypoint_in_buffer_.clear(); 667 | current_waypoint_id_ = 0; 668 | Eigen::Vector4d point; 669 | point[0] = request->goal[0]; 670 | point[1] = request->goal[1]; 671 | point[2] = request->goal[2]; 672 | point[3] = request->goal[3]; 673 | waypoint_in_buffer_.push_back(point); 674 | 675 | RCLCPP_INFO(this->get_logger(), "[%s]: Waypoint set: %.2f, %.2f, %.2f, %.2f", this->get_name(), point.x(), point.y(), point.z(), point.w()); 676 | 677 | RCLCPP_INFO(this->get_logger(), "[%s]: Planning started", this->get_name()); 678 | status_ = PLANNING; 679 | 680 | response->message = "Planning started"; 681 | response->success = true; 682 | return true; 683 | } 684 | //} 685 | 686 | /* gpsWaypointCallback //{ */ 687 | bool Navigation::gpsWaypointCallback([[maybe_unused]] const std::shared_ptr request, 688 | std::shared_ptr response) { 689 | if (!is_initialized_) { 690 | response->message = "Goto rejected, node not initialized"; 691 | response->success = false; 692 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 693 | return true; 694 | } 695 | 696 | if (!getting_octomap_) { 697 | response->message = "Goto rejected, octomap not received"; 698 | response->success = false; 699 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 700 | return true; 701 | } 702 | 703 | if (!getting_control_diagnostics_) { 704 | response->message = "Goto rejected, control_interface diagnostics not received"; 705 | response->success = false; 706 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 707 | return true; 708 | } 709 | 710 | if (status_ != IDLE) { 711 | if (!override_previous_commands_) { 712 | response->message = "Goto rejected, vehicle not IDLE"; 713 | response->success = false; 714 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 715 | return true; 716 | } else { 717 | RCLCPP_INFO(this->get_logger(), "[%s]: Override previous navigation commands", this->get_name()); 718 | hover(); 719 | } 720 | } 721 | 722 | RCLCPP_INFO(this->get_logger(), "[%s]: Creating coord transform request", this->get_name()); 723 | auto waypoint_convert_srv = std::make_shared(); 724 | waypoint_convert_srv->latitude_deg = request->goal[0]; 725 | waypoint_convert_srv->longitude_deg = request->goal[1]; 726 | waypoint_convert_srv->relative_altitude_m = request->goal[2]; 727 | waypoint_convert_srv->yaw = request->goal[3]; 728 | RCLCPP_INFO(this->get_logger(), "[%s]: Calling coord transform", this->get_name()); 729 | auto call_result = 730 | waypoint_to_local_client_->async_send_request(waypoint_convert_srv, std::bind(&Navigation::waypointFutureCallback, this, std::placeholders::_1)); 731 | 732 | 733 | response->message = "Processing goto"; 734 | response->success = true; 735 | return true; 736 | } 737 | //} 738 | 739 | /* hoverCallback //{ */ 740 | bool Navigation::hoverCallback([[maybe_unused]] const std::shared_ptr request, 741 | std::shared_ptr response) { 742 | if (!is_initialized_) { 743 | response->message = "Hover rejected, node not initialized"; 744 | response->success = false; 745 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 746 | return true; 747 | } 748 | 749 | if (!getting_control_diagnostics_) { 750 | response->message = "Hover rejected, control_interface diagnostics not received"; 751 | response->success = false; 752 | RCLCPP_ERROR(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 753 | return true; 754 | } 755 | 756 | if (status_ == IDLE) { 757 | response->message = "Hover not necessary, vehicle is IDLE"; 758 | response->success = false; 759 | RCLCPP_WARN(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 760 | return true; 761 | } 762 | 763 | hover_requested_ = true; 764 | response->message = "Navigation stopped. Hovering"; 765 | response->success = true; 766 | RCLCPP_INFO(this->get_logger(), "[%s]: %s", this->get_name(), response->message.c_str()); 767 | return true; 768 | } 769 | //} 770 | 771 | /* navigationRoutine //{ */ 772 | void Navigation::navigationRoutine(void) { 773 | if (is_initialized_ && getting_octomap_ && getting_control_diagnostics_ && getting_odometry_ && getting_desired_pose_) { 774 | 775 | if (bumper_enabled_) { 776 | std::scoped_lock lock(bumper_mutex_); 777 | if (bumper_msg_ == nullptr || nanosecondsToSecs((this->get_clock()->now() - bumper_msg_->header.stamp).nanoseconds()) > 1.0) { 778 | RCLCPP_WARN(this->get_logger(), "[%s]: Missing bumper data calling hover.", this->get_name()); 779 | hover(); 780 | status_ = IDLE; 781 | waypoint_status_ = EMPTY; 782 | } 783 | } 784 | 785 | switch (status_) { 786 | 787 | /* IDLE //{ */ 788 | case IDLE: { 789 | hover_requested_ = false; 790 | replanning_counter_ = 0; 791 | 792 | if (bumper_enabled_) { 793 | std::scoped_lock lock(bumper_mutex_); 794 | 795 | bool obstacle_detected = bumperCheckObstacles(*bumper_msg_); 796 | if (!bumper_active_) { 797 | if (obstacle_detected) { 798 | RCLCPP_WARN(this->get_logger(), "[%s]: [Bumper] - obstacle in proximity! Starting avoidance", this->get_name()); 799 | status_ = AVOIDING; 800 | break; 801 | } 802 | } else if (!obstacle_detected) { 803 | RCLCPP_WARN(this->get_logger(), "[%s]: Deactivating bumper", this->get_name()); 804 | bumper_active_ = false; 805 | status_ = PLANNING; 806 | break; 807 | } 808 | } 809 | 810 | std::vector waypoints; 811 | waypoints.push_back(uav_pos_); 812 | publishFutureTrajectory(waypoints); 813 | break; 814 | } 815 | //} 816 | 817 | /* PLANNING //{ */ 818 | case PLANNING: { 819 | 820 | waypoint_out_buffer_.clear(); 821 | 822 | if (hover_requested_) { 823 | hover(); 824 | status_ = IDLE; 825 | waypoint_status_ = EMPTY; 826 | break; 827 | } 828 | 829 | std::scoped_lock lock(octree_mutex_); 830 | 831 | if (octree_ == NULL || octree_->size() < 1) { 832 | RCLCPP_WARN(this->get_logger(), "[%s]: Octomap is NULL or empty! Abort planning and swiching to IDLE", this->get_name()); 833 | status_ = IDLE; 834 | waypoint_status_ = EMPTY; 835 | break; 836 | } 837 | 838 | if (waypoint_in_buffer_.empty() || (current_waypoint_id_) >= waypoint_in_buffer_.size()) { 839 | RCLCPP_WARN(this->get_logger(), "[%s]: No navigation goals available. Switching to IDLE", this->get_name()); 840 | waypoint_in_buffer_.clear(); 841 | current_waypoint_id_ = 0; 842 | status_ = IDLE; 843 | break; 844 | } 845 | 846 | last_goal_ = current_goal_; 847 | current_goal_ = waypoint_in_buffer_[current_waypoint_id_]; 848 | 849 | RCLCPP_INFO(this->get_logger(), "[%s]: Waypoint [%.2f, %.2f, %.2f, %.2f] set as a next goal", this->get_name(), 850 | current_goal_[0], current_goal_[1], current_goal_[2], current_goal_[3]); 851 | RCLCPP_INFO(this->get_logger(), "[%s]: Last received desired pose [%.2f, %.2f, %.2f, %.2f]", this->get_name(), 852 | desired_pose_[0], desired_pose_[1], desired_pose_[2], desired_pose_[3]); 853 | 854 | visualizeGoals(waypoint_in_buffer_); 855 | 856 | if (replanning_counter_ >= replanning_limit_) { 857 | RCLCPP_ERROR(this->get_logger(), 858 | "[%s]: No waypoint produced after %d repeated attempts. " 859 | "Please provide a new waypoint", 860 | this->get_name(), replanning_counter_); 861 | waypoint_in_buffer_.clear(); 862 | current_waypoint_id_ = 0; 863 | replanning_counter_ = 0; 864 | status_ = IDLE; 865 | waypoint_status_ = UNREACHABLE; 866 | } 867 | 868 | navigation::AstarPlanner planner = 869 | navigation::AstarPlanner(safe_obstacle_distance_, euclidean_distance_cutoff_, planning_tree_resolution_, distance_penalty_, greedy_penalty_, 870 | min_altitude_, max_altitude_, planning_timeout_, max_waypoint_distance_, unknown_is_occupied_); 871 | 872 | octomap::point3d planning_start = toPoint3d(uav_pos_); 873 | octomap::point3d pos_cmd = toPoint3d(desired_pose_); 874 | /*if ((desired_pose_.head<3>() - uav_pos_.head<3>()).norm() <= navigation_tolerance_) { 875 | planning_start = toPoint3d(desired_pose_); 876 | } else { 877 | planning_start = toPoint3d(uav_pos_); 878 | }*/ 879 | octomap::point3d planning_goal = toPoint3d(current_goal_); 880 | 881 | std::pair, PlanningResult> waypoints = 882 | planner.findPath(planning_start, planning_goal, pos_cmd, octree_, planning_timeout_, std::bind(&Navigation::visualizeTree, this, _1), 883 | std::bind(&Navigation::visualizeExpansions, this, _1, _2, _3)); 884 | 885 | RCLCPP_INFO(this->get_logger(), "[%s]: Planner returned %ld waypoints, before resampling:", this->get_name(), waypoints.first.size()); 886 | 887 | for (auto &w :waypoints.first) { 888 | RCLCPP_INFO(this->get_logger(), "[%s]: %.2f, %.2f, %.2f", this->get_name(), w.x(), w.y(), w.z()); 889 | } 890 | /* GOAL_REACHED //{ */ 891 | if (waypoints.second == GOAL_REACHED) { 892 | RCLCPP_INFO(this->get_logger(), "[%s]: Current goal reached", this->get_name()); 893 | waypoint_status_ = REACHED; 894 | if (current_waypoint_id_ >= waypoint_in_buffer_.size()) { 895 | RCLCPP_INFO(this->get_logger(), "[%s]: The last provided navigation goal has been visited. Switching to IDLE", this->get_name()); 896 | waypoint_in_buffer_.clear(); 897 | current_waypoint_id_ = 0; 898 | status_ = IDLE; 899 | break; 900 | 901 | } else { 902 | current_waypoint_id_++; 903 | } 904 | 905 | break; 906 | } 907 | //} 908 | 909 | /* COMPLETE //{ */ 910 | if (waypoints.second == COMPLETE) { 911 | replanning_counter_ = 0; 912 | } 913 | //} 914 | 915 | /* INCOMPLETE //{ */ 916 | if (waypoints.second == INCOMPLETE) { 917 | 918 | if (waypoints.first.size() < 2) { 919 | RCLCPP_WARN(this->get_logger(), "[%s]: path not found", this->get_name()); 920 | replanning_counter_++; 921 | break; 922 | } 923 | 924 | Eigen::Vector3d w_start; 925 | w_start.x() = waypoints.first.front().x(); 926 | w_start.y() = waypoints.first.front().y(); 927 | w_start.z() = waypoints.first.front().z(); 928 | 929 | Eigen::Vector3d w_end; 930 | w_end.x() = waypoints.first.back().x(); 931 | w_end.y() = waypoints.first.back().y(); 932 | w_end.z() = waypoints.first.back().z(); 933 | 934 | double path_start_end_dist = (w_end - w_start).norm(); 935 | 936 | if (path_start_end_dist < 1.1 * planning_tree_resolution_) { 937 | RCLCPP_WARN(this->get_logger(), "[%s]: path too short", this->get_name()); 938 | replanning_counter_++; 939 | break; 940 | } 941 | } 942 | //} 943 | 944 | /* GOAL_IN_OBSTACLE //{ */ 945 | if (waypoints.second == GOAL_IN_OBSTACLE) { 946 | replanning_counter_ = 0; 947 | RCLCPP_WARN(this->get_logger(), "[%s]: Goal [%.2f, %.2f, %.2f, %.2f] is inside an inflated obstacle", this->get_name(), current_goal_[0], 948 | current_goal_[1], current_goal_[2], current_goal_[3]); 949 | } 950 | //} 951 | 952 | /* FAILURE //{ */ 953 | if (waypoints.second == FAILURE) { 954 | RCLCPP_WARN(this->get_logger(), "[%s]: path to goal not found", this->get_name()); 955 | replanning_counter_++; 956 | break; 957 | } 958 | //} 959 | 960 | /* resample path and add yaw //{ */ 961 | std::vector resampled = resamplePath(waypoints.first, desired_pose_.w(), current_goal_.w()); 962 | 963 | bool output_current_goal = waypoints.second == COMPLETE; 964 | for (auto &w : resampled) { 965 | if ((w.head<3>() - desired_pose_.head<3>()).norm() <= replanning_distance_) { 966 | waypoint_out_buffer_.push_back(w); 967 | } else { 968 | RCLCPP_INFO(this->get_logger(), "[%s]: Path exceeding replanning distance", this->get_name()); 969 | output_current_goal = false; 970 | break; 971 | } 972 | } 973 | 974 | if (output_current_goal) { 975 | waypoint_out_buffer_.push_back(current_goal_); 976 | } 977 | //} 978 | 979 | status_ = COMMANDING; 980 | waypoint_status_ = ONGOING; 981 | break; 982 | } 983 | //} 984 | 985 | /* COMMANDING //{ */ 986 | case COMMANDING: { 987 | 988 | if (hover_requested_) { 989 | hover(); 990 | status_ = IDLE; 991 | waypoint_status_ = EMPTY; 992 | break; 993 | } 994 | 995 | if (waypoint_out_buffer_.size() < 1) { 996 | RCLCPP_WARN(this->get_logger(), "[%s]: No waypoints in the output buffer. Replanning", this->get_name()); 997 | replanning_counter_++; 998 | status_ = PLANNING; 999 | break; 1000 | } 1001 | 1002 | RCLCPP_INFO(this->get_logger(), "[%s]: Sending %ld waypoints to the control interface:", this->get_name(), waypoint_out_buffer_.size()); 1003 | for (auto &w : waypoint_out_buffer_) { 1004 | RCLCPP_INFO(this->get_logger(), "[%s]: %.2f, %.2f, %.2f, %.2f", this->get_name(), w.x(), w.y(), w.z(), w.w()); 1005 | } 1006 | visualizePath(waypoint_out_buffer_); 1007 | publishFutureTrajectory(waypoint_out_buffer_); 1008 | auto waypoints_srv = waypointsToPathSrv(waypoint_out_buffer_, false); 1009 | auto call_result = local_path_client_->async_send_request(waypoints_srv); 1010 | status_ = MOVING; 1011 | { 1012 | std::scoped_lock lock(control_diagnostics_mutex_); 1013 | diagnostics_received_ = false; 1014 | } 1015 | break; 1016 | } 1017 | //} 1018 | 1019 | /* MOVING //{ */ 1020 | case MOVING: { 1021 | if (hover_requested_) { 1022 | hover(); 1023 | status_ = IDLE; 1024 | waypoint_status_ = EMPTY; 1025 | break; 1026 | } 1027 | 1028 | if (bumper_enabled_) { 1029 | std::scoped_lock lock(bumper_mutex_); 1030 | bool obstacle_detected = bumperCheckObstacles(*bumper_msg_); 1031 | if (!bumper_active_) { 1032 | if (obstacle_detected) { 1033 | RCLCPP_WARN(this->get_logger(), "[%s]: [Bumper] - obstacle in proximity while moving! Calling hover", this->get_name()); 1034 | hover(); 1035 | status_ = IDLE; 1036 | break; 1037 | } 1038 | } 1039 | } 1040 | 1041 | replanning_counter_ = 0; 1042 | publishFutureTrajectory(waypoint_out_buffer_); 1043 | { 1044 | // Check if diagnostics received after commanding 1045 | std::scoped_lock lock(control_diagnostics_mutex_); 1046 | if (!diagnostics_received_){ 1047 | RCLCPP_WARN(this->get_logger(), "[%s]: Control diagnostics not received after commanding, skipping loop", this->get_name()); 1048 | } else { 1049 | if ((!control_moving_ && goal_reached_)) { 1050 | RCLCPP_INFO(this->get_logger(), "[%s]: End of current segment reached", this->get_name()); 1051 | if (bumper_active_) { 1052 | status_ = AVOIDING; 1053 | } else { 1054 | status_ = PLANNING; 1055 | } 1056 | } 1057 | } 1058 | } 1059 | break; 1060 | } 1061 | //} 1062 | 1063 | /* AVOIDING //{ */ 1064 | case AVOIDING: { 1065 | 1066 | std::scoped_lock lock(bumper_mutex_); 1067 | Eigen::Vector3d avoidance_vector = bumperGetAvoidanceVector(*bumper_msg_); 1068 | 1069 | if (avoidance_vector.norm() == 0) { 1070 | RCLCPP_INFO(this->get_logger(), "[Navigation]: Nothing to avoid"); 1071 | status_ = IDLE; 1072 | break; 1073 | } 1074 | 1075 | Eigen::Vector4d new_goal = uav_pos_; 1076 | new_goal.x() += avoidance_vector.x(); 1077 | new_goal.y() += avoidance_vector.y(); 1078 | new_goal.z() = desired_pose_.z() + avoidance_vector.z(); // TODO this is a temporary workaround until state estimation (odometry pkg) is integrated 1079 | new_goal.w() = desired_pose_.w(); 1080 | 1081 | RCLCPP_INFO(this->get_logger(), "[Bumper]: Avoiding obstacle by moving from [%.2f, %.2f, %.2f, %.2f] to [%.2f, %.2f, %.2f, %.2f]", uav_pos_.x(), 1082 | uav_pos_.y(), uav_pos_.z(), uav_pos_.w(), new_goal.x(), new_goal.y(), new_goal.z(), new_goal.w()); 1083 | 1084 | waypoint_out_buffer_.clear(); 1085 | waypoint_out_buffer_.push_back(desired_pose_); 1086 | waypoint_out_buffer_.push_back(new_goal); 1087 | 1088 | if (!bumper_active_) { 1089 | RCLCPP_WARN(this->get_logger(), "[%s]: Activating bumper", this->get_name()); 1090 | bumper_active_ = true; 1091 | } 1092 | 1093 | status_ = COMMANDING; 1094 | break; 1095 | } 1096 | //} 1097 | } 1098 | } 1099 | 1100 | std_msgs::msg::String msg; 1101 | msg.data = STATUS_STRING[status_]; 1102 | status_publisher_->publish(msg); 1103 | publishDiagnostics(); 1104 | } 1105 | //} 1106 | 1107 | /* bumperCheckObstacles //{ */ 1108 | bool Navigation::bumperCheckObstacles(const fog_msgs::msg::ObstacleSectors &bumper_msg) { 1109 | 1110 | 1111 | for (int i = 0; i < int(bumper_msg.n_horizontal_sectors); i++) { 1112 | 1113 | if (bumper_msg.sectors[i] < 0) { 1114 | continue; 1115 | } 1116 | 1117 | // if the sector is under safe distance 1118 | if (bumper_msg.sectors[i] <= safe_obstacle_distance_ * bumper_distance_factor_) { 1119 | return true; 1120 | } 1121 | } 1122 | 1123 | return false; 1124 | } 1125 | //} 1126 | 1127 | /* bumperGetAvoidanceVector//{ */ 1128 | Eigen::Vector3d Navigation::bumperGetAvoidanceVector(const fog_msgs::msg::ObstacleSectors &bumper_msg) { 1129 | 1130 | double sector_size = (2.0 * M_PI) / double(bumper_msg.n_horizontal_sectors); 1131 | 1132 | Eigen::Vector3d forward = Eigen::Vector3d::UnitX(); 1133 | 1134 | for (int i = 0; i < int(bumper_msg.n_horizontal_sectors); i++) { 1135 | 1136 | if (bumper_msg.sectors[i] < 0) { 1137 | continue; 1138 | } 1139 | 1140 | if (bumper_msg.sectors[i] <= safe_obstacle_distance_ * bumper_distance_factor_) { 1141 | Eigen::Quaterniond q = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) * 1142 | Eigen::AngleAxisd(sector_size * i + M_PI + uav_pos_.w(), Eigen::Vector3d::UnitZ()); 1143 | 1144 | Eigen::Vector3d avoidance_vector = q * ((safe_obstacle_distance_ - bumper_msg.sectors[i] + planning_tree_resolution_) * forward); 1145 | 1146 | return avoidance_vector; 1147 | } 1148 | } 1149 | return Eigen::Vector3d::Zero(); 1150 | } 1151 | //} 1152 | 1153 | /* resamplePath //{ */ 1154 | 1155 | std::vector Navigation::resamplePath(const std::vector &waypoints, const double start_yaw, const double end_yaw) { 1156 | std::vector ret; 1157 | 1158 | if (waypoints.size() < 2) { 1159 | for (auto &w : waypoints) { 1160 | ret.push_back(Eigen::Vector4d(w.x(), w.y(), w.z(), end_yaw)); 1161 | } 1162 | return ret; 1163 | } 1164 | 1165 | ret.push_back(Eigen::Vector4d(waypoints.front().x(), waypoints.front().y(), waypoints.front().z(), 0.0)); 1166 | 1167 | size_t i = 1; 1168 | while (i < waypoints.size()) { 1169 | double dist = std::sqrt(std::pow(ret.back().x() - waypoints[i].x(), 2) + std::pow(ret.back().y() - waypoints[i].y(), 2) + 1170 | std::pow(ret.back().z() - waypoints[i].z(), 2)); 1171 | if (dist > max_waypoint_distance_) { 1172 | Eigen::Vector3d direction; 1173 | direction.x() = waypoints[i].x() - ret.back().x(); 1174 | direction.y() = waypoints[i].y() - ret.back().y(); 1175 | direction.z() = waypoints[i].z() - ret.back().z(); 1176 | direction = direction.normalized() * max_waypoint_distance_; 1177 | 1178 | Eigen::Vector4d padded; 1179 | padded.x() = ret.back().x() + direction.x(); 1180 | padded.y() = ret.back().y() + direction.y(); 1181 | padded.z() = ret.back().z() + direction.z(); 1182 | ret.push_back(padded); 1183 | } else { 1184 | ret.push_back(Eigen::Vector4d(waypoints[i].x(), waypoints[i].y(), waypoints[i].z(), 0.0)); 1185 | i++; 1186 | } 1187 | } 1188 | 1189 | std::cout << "[Navigation]: Padded " << waypoints.size() << " original waypoints to " << ret.size() << " points\n"; 1190 | 1191 | /* add yaw //{ */ 1192 | 1193 | double delta_yaw = std::atan2(std::sin(end_yaw - start_yaw), std::cos(end_yaw - start_yaw)); 1194 | double yaw_step = delta_yaw / ret.size(); 1195 | std::cout << "[Navigation]: Start yaw: " << start_yaw << ", end yaw: " << end_yaw << ", yaw step: " << yaw_step << "\n"; 1196 | 1197 | if (std::abs(yaw_step) <= max_yaw_step_) { 1198 | ret.front().w() = start_yaw; 1199 | for (size_t j = 1; j < ret.size(); j++) { 1200 | ret[j].w() = ret[j - 1].w() + yaw_step; 1201 | } 1202 | } else { 1203 | // resample again to limit yaw rate and avoid fast turning 1204 | int resampling_factor = int(std::abs(yaw_step) / max_yaw_step_) + 1; 1205 | std::cout << "[Navigation]: Yaw step: " << yaw_step << " is greater than max yaw step: " << max_yaw_step_ << "\n"; 1206 | std::cout << "[Navigation]: Resampling factor: " << resampling_factor << "\n"; 1207 | std::vector resampled; 1208 | for (const auto &p : ret) { 1209 | for (int j = 0; j < resampling_factor; j++) { 1210 | Eigen::Vector4d v(p[0], p[1], p[2], p[3]); 1211 | resampled.push_back(v); 1212 | } 1213 | } 1214 | 1215 | ret.clear(); 1216 | ret.insert(ret.end(), resampled.begin(), resampled.end()); 1217 | yaw_step = delta_yaw / ret.size(); 1218 | std::cout << "[Navigation]: New yaw step: " << yaw_step << "\n"; 1219 | ret.front().w() = start_yaw; 1220 | for (size_t j = 1; j < ret.size(); j++) { 1221 | ret[j].w() = ret[j - 1].w() + yaw_step; 1222 | } 1223 | } 1224 | 1225 | 1226 | //} 1227 | 1228 | return ret; 1229 | } 1230 | //} 1231 | 1232 | /* waypointsToPathSrv //{ */ 1233 | std::shared_ptr Navigation::waypointsToPathSrv(const std::vector &waypoints, bool use_first) { 1234 | nav_msgs::msg::Path msg; 1235 | msg.header.stamp = this->get_clock()->now(); 1236 | msg.header.frame_id = parent_frame_; 1237 | size_t i = 0; 1238 | if (!use_first) { 1239 | ++i; 1240 | } 1241 | while (i < waypoints.size()) { 1242 | geometry_msgs::msg::PoseStamped p; 1243 | p.pose.position.x = waypoints[i].x(); 1244 | p.pose.position.y = waypoints[i].y(); 1245 | p.pose.position.z = waypoints[i].z(); 1246 | p.pose.orientation = yawToQuaternionMsg(waypoints[i].w()); 1247 | msg.poses.push_back(p); 1248 | i++; 1249 | } 1250 | auto path_srv = std::make_shared(); 1251 | path_srv->path = msg; 1252 | return path_srv; 1253 | } 1254 | //} 1255 | 1256 | /* hover //{ */ 1257 | void Navigation::hover() { 1258 | waypoint_in_buffer_.clear(); 1259 | current_waypoint_id_ = 0; 1260 | std::vector waypoints; 1261 | waypoints.push_back(desired_pose_); 1262 | auto waypoints_srv = waypointsToPathSrv(waypoints, true); 1263 | auto call_result = local_path_client_->async_send_request(waypoints_srv); 1264 | } 1265 | //} 1266 | 1267 | /* publishDiagnostics //{ */ 1268 | void Navigation::publishDiagnostics() { 1269 | fog_msgs::msg::NavigationDiagnostics msg; 1270 | msg.header.stamp = this->get_clock()->now(); 1271 | msg.header.frame_id = parent_frame_; 1272 | msg.state = STATUS_STRING[status_]; 1273 | msg.current_waypoint_status = WAYPOINT_STATUS_STRING[waypoint_status_]; 1274 | msg.waypoints_in_buffer = waypoint_in_buffer_.size(); 1275 | msg.bumper_active = bumper_active_; 1276 | msg.current_waypoint_id = current_waypoint_id_; 1277 | msg.current_nav_goal[0] = current_goal_.x(); 1278 | msg.current_nav_goal[1] = current_goal_.y(); 1279 | msg.current_nav_goal[2] = current_goal_.z(); 1280 | msg.last_nav_goal[0] = last_goal_.x(); 1281 | msg.last_nav_goal[1] = last_goal_.y(); 1282 | msg.last_nav_goal[2] = last_goal_.z(); 1283 | diagnostics_publisher_->publish(msg); 1284 | } 1285 | //} 1286 | 1287 | /* publishFutureTrajectory //{ */ 1288 | void Navigation::publishFutureTrajectory(std::vector waypoints) { 1289 | fog_msgs::msg::FutureTrajectory msg; 1290 | msg.header.stamp = this->get_clock()->now(); 1291 | msg.header.frame_id = parent_frame_; 1292 | for (const auto &w : waypoints) { 1293 | fog_msgs::msg::Vector4Stamped v; 1294 | v.x = w.x(); 1295 | v.y = w.y(); 1296 | v.z = w.z(); 1297 | v.w = w.w(); 1298 | v.header.frame_id = parent_frame_; 1299 | msg.poses.push_back(v); 1300 | } 1301 | future_trajectory_publisher_->publish(msg); 1302 | } 1303 | //} 1304 | 1305 | /* waypointFutureCallback //{ */ 1306 | bool Navigation::waypointFutureCallback(rclcpp::Client::SharedFuture future) { 1307 | std::shared_ptr result = future.get(); 1308 | 1309 | if (result->success) { 1310 | RCLCPP_INFO(this->get_logger(), "[%s]: Coordinate transform returned: %.2f, %.2f", this->get_name(), result->local_x, result->local_y); 1311 | 1312 | Eigen::Vector4d point; 1313 | point[0] = result->local_x; 1314 | point[1] = result->local_y; 1315 | point[2] = result->local_z; 1316 | point[3] = result->yaw; 1317 | 1318 | if (point.z() < min_altitude_) { 1319 | RCLCPP_WARN(this->get_logger(), "[%s]: Goal Z coordinate (%.2f) is below the minimum allowed altitude (%.2f)", this->get_name(), point.z(), 1320 | min_altitude_); 1321 | return false; 1322 | } 1323 | 1324 | if (point.z() > max_altitude_) { 1325 | RCLCPP_WARN(this->get_logger(), "[%s]: Goal Z coordinate (%.2f) is above the maximum allowed altitude (%.2f)", this->get_name(), point.z(), 1326 | max_altitude_); 1327 | return false; 1328 | } 1329 | 1330 | if ((point.head<3>() - desired_pose_.head<3>()).norm() > max_goal_distance_) { 1331 | RCLCPP_WARN(this->get_logger(), "[%s]: Distance to goal (%.2f) exceeds the maximum allowed distance (%.2f m)", this->get_name(), 1332 | (point.head<3>() - desired_pose_.head<3>()).norm(), max_goal_distance_); 1333 | return false; 1334 | } 1335 | 1336 | waypoint_in_buffer_.push_back(point); 1337 | 1338 | RCLCPP_INFO(this->get_logger(), "[%s]: Waypoint added (LOCAL): %.2f, %.2f, %.2f, %.2f", this->get_name(), point.x(), point.y(), point.z(), point.w()); 1339 | 1340 | RCLCPP_INFO(this->get_logger(), "[%s]: Planning started", this->get_name()); 1341 | status_ = PLANNING; 1342 | 1343 | return true; 1344 | } 1345 | return false; 1346 | } 1347 | //} 1348 | 1349 | /* pathFutureCallback //{ */ 1350 | bool Navigation::pathFutureCallback(rclcpp::Client::SharedFuture future) { 1351 | std::shared_ptr result = future.get(); 1352 | 1353 | if (result->success) { 1354 | RCLCPP_INFO(this->get_logger(), "[%s]: Coordinate transform returned %ld points", this->get_name(), result->path.poses.size()); 1355 | 1356 | for (auto &pose : result->path.poses) { 1357 | 1358 | Eigen::Vector4d point; 1359 | point[0] = pose.pose.position.x; 1360 | point[1] = pose.pose.position.y; 1361 | point[2] = pose.pose.position.z; 1362 | point[3] = getYaw(pose.pose.orientation); 1363 | 1364 | if (point.z() < min_altitude_) { 1365 | RCLCPP_WARN(this->get_logger(), "[%s]: Goal Z coordinate (%.2f) is below the minimum allowed altitude (%.2f)", this->get_name(), point.z(), 1366 | min_altitude_); 1367 | return false; 1368 | } 1369 | 1370 | if (point.z() > max_altitude_) { 1371 | RCLCPP_WARN(this->get_logger(), "[%s]: Goal Z coordinate (%.2f) is above the maximum allowed altitude (%.2f)", this->get_name(), point.z(), 1372 | max_altitude_); 1373 | return false; 1374 | } 1375 | 1376 | if ((point.head<3>() - desired_pose_.head<3>()).norm() > max_goal_distance_) { 1377 | RCLCPP_WARN(this->get_logger(), "[%s]: Distance to goal (%.2f) exceeds the maximum allowed distance (%.2f m)", this->get_name(), 1378 | (point.head<3>() - desired_pose_.head<3>()).norm(), max_goal_distance_); 1379 | return false; 1380 | } 1381 | 1382 | waypoint_in_buffer_.push_back(point); 1383 | RCLCPP_INFO(this->get_logger(), "[%s]: Waypoint added (LOCAL): %.2f, %.2f, %.2f, %.2f", this->get_name(), point.x(), point.y(), point.z(), point.w()); 1384 | } 1385 | 1386 | RCLCPP_INFO(this->get_logger(), "[%s]: Planning started", this->get_name()); 1387 | status_ = PLANNING; 1388 | 1389 | return true; 1390 | } 1391 | return false; 1392 | } 1393 | //} 1394 | 1395 | /* visualization //{ */ 1396 | 1397 | /* visualizeTree //{ */ 1398 | void Navigation::visualizeTree(const octomap::OcTree &tree) { 1399 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Visualizing tree", this->get_name()); 1400 | visualization_msgs::msg::Marker msg; 1401 | msg.header.frame_id = parent_frame_; 1402 | msg.header.stamp = this->get_clock()->now(); 1403 | msg.ns = "tree"; 1404 | msg.type = visualization_msgs::msg::Marker::POINTS; 1405 | msg.id = 8; 1406 | msg.action = visualization_msgs::msg::Marker::ADD; 1407 | msg.pose.orientation.w = 1.0; 1408 | msg.scale.x = tree_points_scale_; 1409 | msg.scale.y = tree_points_scale_; 1410 | 1411 | for (auto it = tree.begin(); it != tree.end(); it++) { 1412 | if (it->getValue() == TreeValue::OCCUPIED) { 1413 | geometry_msgs::msg::Point gp; 1414 | auto color = generateColor(0, 0, 0, 1.0); 1415 | gp.x = it.getX(); 1416 | gp.y = it.getY(); 1417 | gp.z = it.getZ(); 1418 | msg.points.push_back(gp); 1419 | msg.colors.push_back(color); 1420 | } else if (show_unoccupied_) { 1421 | geometry_msgs::msg::Point gp; 1422 | auto color = generateColor(0.7, 0.7, 0.7, 0.7); 1423 | gp.x = it.getX(); 1424 | gp.y = it.getY(); 1425 | gp.z = it.getZ(); 1426 | msg.points.push_back(gp); 1427 | msg.colors.push_back(color); 1428 | } 1429 | } 1430 | binary_tree_publisher_->publish(msg); 1431 | } 1432 | //} 1433 | 1434 | /* visualizeExpansions //{ */ 1435 | void Navigation::visualizeExpansions(const std::unordered_set open, 1436 | const std::unordered_set &closed, const octomap::OcTree &tree) { 1437 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Visualizing open planning expansions", this->get_name()); 1438 | visualization_msgs::msg::Marker msg; 1439 | msg.header.frame_id = parent_frame_; 1440 | msg.header.stamp = this->get_clock()->now(); 1441 | msg.ns = "expansions"; 1442 | msg.type = visualization_msgs::msg::Marker::POINTS; 1443 | msg.id = 8; 1444 | msg.action = visualization_msgs::msg::Marker::ADD; 1445 | msg.pose.orientation.w = 1.0; 1446 | msg.scale.x = expansions_points_scale_; 1447 | msg.scale.y = expansions_points_scale_; 1448 | 1449 | double max_cost = 0; 1450 | double min_cost = 1e6; 1451 | 1452 | for (auto it = open.begin(); it != open.end(); it++) { 1453 | if (it->total_cost > max_cost) { 1454 | max_cost = it->total_cost; 1455 | } 1456 | if (it->total_cost < min_cost) { 1457 | min_cost = it->total_cost; 1458 | } 1459 | } 1460 | 1461 | for (auto it = open.begin(); it != open.end(); it++) { 1462 | auto coords = tree.keyToCoord(it->key); 1463 | geometry_msgs::msg::Point gp; 1464 | double brightness = (it->total_cost - min_cost) / (max_cost - min_cost); 1465 | auto color = generateColor(0.0, brightness, 0.3, 0.8); 1466 | gp.x = coords.x(); 1467 | gp.y = coords.y(); 1468 | gp.z = coords.z(); 1469 | msg.points.push_back(gp); 1470 | msg.colors.push_back(color); 1471 | } 1472 | 1473 | for (auto it = closed.begin(); it != closed.end(); it++) { 1474 | auto coords = tree.keyToCoord(it->key); 1475 | geometry_msgs::msg::Point gp; 1476 | auto color = generateColor(0.8, 0.0, 0, 0.8); 1477 | gp.x = coords.x(); 1478 | gp.y = coords.y(); 1479 | gp.z = coords.z(); 1480 | msg.points.push_back(gp); 1481 | msg.colors.push_back(color); 1482 | } 1483 | expansion_publisher_->publish(msg); 1484 | } 1485 | //} 1486 | 1487 | /* visualizePath //{ */ 1488 | void Navigation::visualizePath(const std::vector &waypoints) { 1489 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Visualizing path", this->get_name()); 1490 | visualization_msgs::msg::Marker msg; 1491 | msg.header.frame_id = parent_frame_; 1492 | msg.header.stamp = this->get_clock()->now(); 1493 | msg.ns = "path"; 1494 | msg.type = visualization_msgs::msg::Marker::LINE_STRIP; 1495 | msg.id = 8; 1496 | msg.action = visualization_msgs::msg::Marker::ADD; 1497 | msg.pose.orientation.w = 1.0; 1498 | msg.scale.x = path_points_scale_; 1499 | 1500 | for (size_t i = 1; i < waypoints.size(); i++) { 1501 | geometry_msgs::msg::Point p1, p2; 1502 | std_msgs::msg::ColorRGBA c; 1503 | p1.x = waypoints[i - 1].x(); 1504 | p1.y = waypoints[i - 1].y(); 1505 | p1.z = waypoints[i - 1].z(); 1506 | p2.x = waypoints[i].x(); 1507 | p2.y = waypoints[i].y(); 1508 | p2.z = waypoints[i].z(); 1509 | c = generateColor(0.1, double(i) / double(waypoints.size()), 0.1, 1); 1510 | msg.points.push_back(p1); 1511 | msg.points.push_back(p2); 1512 | msg.colors.push_back(c); 1513 | msg.colors.push_back(c); 1514 | } 1515 | path_publisher_->publish(msg); 1516 | } 1517 | //} 1518 | 1519 | /* visualizeGoals //{ */ 1520 | void Navigation::visualizeGoals(const std::deque &waypoints) { 1521 | RCLCPP_INFO_ONCE(this->get_logger(), "[%s]: Visualizing goals", this->get_name()); 1522 | visualization_msgs::msg::Marker msg; 1523 | msg.header.frame_id = parent_frame_; 1524 | msg.header.stamp = this->get_clock()->now(); 1525 | msg.ns = "goals"; 1526 | msg.type = visualization_msgs::msg::Marker::POINTS; 1527 | msg.id = 8; 1528 | msg.action = visualization_msgs::msg::Marker::ADD; 1529 | msg.pose.orientation.w = 1.0; 1530 | msg.scale.x = goal_points_scale_; 1531 | msg.scale.y = goal_points_scale_; 1532 | 1533 | for (size_t id = 0; id < waypoints.size(); id++) { 1534 | const Eigen::Vector4d w = waypoints[id]; 1535 | std_msgs::msg::ColorRGBA c; 1536 | 1537 | if (id == current_waypoint_id_) { 1538 | c = generateColor(0.3, 0.5, 1.0, 1.0); 1539 | } else { 1540 | c = generateColor(0.1, 0.3, 0.7, 1.0); 1541 | } 1542 | 1543 | geometry_msgs::msg::Point p; 1544 | p.x = w.x(); 1545 | p.y = w.y(); 1546 | p.z = w.z(); 1547 | msg.points.push_back(p), w.w(); 1548 | msg.colors.push_back(c); 1549 | } 1550 | goal_publisher_->publish(msg); 1551 | } 1552 | //} 1553 | 1554 | /* generateColor//{ */ 1555 | std_msgs::msg::ColorRGBA Navigation::generateColor(const double r, const double g, const double b, const double a) { 1556 | std_msgs::msg::ColorRGBA c; 1557 | c.r = r; 1558 | c.g = g; 1559 | c.b = b; 1560 | c.a = a; 1561 | return c; 1562 | } 1563 | //} 1564 | 1565 | //} 1566 | 1567 | /* parse_param //{ */ 1568 | template 1569 | bool Navigation::parse_param(const std::string ¶m_name, T ¶m_dest) { 1570 | this->declare_parameter(param_name); 1571 | if (!this->get_parameter(param_name, param_dest)) { 1572 | RCLCPP_ERROR(this->get_logger(), "[%s]: Could not load param '%s'", this->get_name(), param_name.c_str()); 1573 | return false; 1574 | } else { 1575 | RCLCPP_INFO_STREAM(this->get_logger(), "[" << this->get_name() << "]: Loaded '" << param_name << "' = '" << param_dest << "'"); 1576 | } 1577 | return true; 1578 | } 1579 | 1580 | /* template bool Navigation::parse_param(const std::string ¶m_name, int ¶m_dest); */ 1581 | /* template bool Navigation::parse_param(const std::string ¶m_name, double ¶m_dest); */ 1582 | /* template bool Navigation::parse_param(const std::string ¶m_name, float ¶m_dest); */ 1583 | /* template bool Navigation::parse_param(const std::string ¶m_name, std::string ¶m_dest); */ 1584 | /* template bool Navigation::parse_param(const std::string ¶m_name, bool ¶m_dest); */ 1585 | /* template bool Navigation::parse_param(const std::string ¶m_name, unsigned int ¶m_dest); */ 1586 | //} 1587 | 1588 | } // namespace navigation 1589 | #include 1590 | RCLCPP_COMPONENTS_REGISTER_NODE(navigation::Navigation) 1591 | --------------------------------------------------------------------------------