├── .github └── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── feature_request.md │ └── support-issue.md ├── .gitignore ├── .travis.yml ├── Jenkinsfile ├── LICENSE.md ├── README.md ├── avoidance ├── CMakeLists.txt ├── include │ └── avoidance │ │ ├── avoidance_node.h │ │ ├── common.h │ │ ├── histogram.h │ │ ├── rviz_world_loader.h │ │ ├── transform_buffer.h │ │ └── usm.h ├── launch │ ├── avoidance_sitl_mavros.launch │ └── avoidance_sitl_stereo.launch ├── package.xml ├── resource │ └── px4_config.yaml ├── sim │ ├── models │ │ ├── distance_sensor │ │ │ ├── distance_sensor.sdf │ │ │ └── model.config │ │ ├── iris_realsense │ │ │ ├── iris_realsense.sdf │ │ │ └── model.config │ │ ├── lamp_with_lines │ │ │ ├── lamp_with_lines.dae │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── line_model │ │ │ ├── line.dae │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── powerline │ │ │ ├── model.config │ │ │ ├── model.sdf │ │ │ └── powerline.dae │ │ └── stereo_camera_vertical │ │ │ ├── model.config │ │ │ └── stereo_camera_vertical.sdf │ └── worlds │ │ ├── boxes1.world │ │ ├── boxes1.yaml │ │ ├── boxes2.world │ │ ├── boxes2.yaml │ │ ├── boxes3.world │ │ ├── boxes3.yaml │ │ ├── boxes4.world │ │ ├── boxes4.yaml │ │ ├── boxes5.world │ │ ├── boxes5.yaml │ │ ├── boxes6.world │ │ ├── boxes6.yaml │ │ ├── moving_boxes.world │ │ ├── outdoor_village.world │ │ ├── outdoor_village_3.world │ │ ├── simple_obstacle.world │ │ ├── simple_obstacle.yaml │ │ ├── test_city_2.world │ │ ├── test_city_4.world │ │ ├── window.world │ │ └── window.yaml ├── src │ ├── avoidance_node.cpp │ ├── common.cpp │ ├── histogram.cpp │ ├── rviz_world_loader.cpp │ └── transform_buffer.cpp └── test │ ├── main.cpp │ ├── test_common.cpp │ ├── test_transform_buffer.cpp │ ├── test_usm.cpp │ └── test_usm.cpp.dot ├── docs ├── lp_goal_height.png ├── lp_goal_rviz.png └── simulation_screenshot.png ├── global_planner ├── CMakeLists.txt ├── cfg │ └── GlobalPlannerNode.cfg ├── include │ └── global_planner │ │ ├── analysis.h │ │ ├── bezier.h │ │ ├── cell.h │ │ ├── common.h │ │ ├── common_ros.h │ │ ├── global_planner.h │ │ ├── global_planner_node.h │ │ ├── node.h │ │ ├── search_tools.h │ │ └── visitor.h ├── launch │ ├── global_planner_depth-camera.launch │ ├── global_planner_octomap.launch │ ├── global_planner_sitl_3cam.launch │ └── global_planner_stereo.launch ├── msg │ └── PathWithRiskMsg.msg ├── package.xml ├── resource │ ├── global_planner.rviz │ ├── parameter_file │ ├── px4_config.yaml │ ├── random_goals │ └── sample_output_from_mock_data ├── src │ ├── library │ │ ├── cell.cpp │ │ ├── global_planner.cpp │ │ └── node.cpp │ └── nodes │ │ ├── global_planner_node.cpp │ │ ├── global_planner_node_main.cpp │ │ ├── mock_data_node.cpp │ │ └── mock_data_node.h └── test │ ├── main.cpp │ └── test_example.cpp ├── local_planner ├── CMakeLists.txt ├── cfg │ ├── LocalPlannerNode.cfg │ └── vehicle.yaml ├── include │ └── local_planner │ │ ├── avoidance_output.h │ │ ├── candidate_direction.h │ │ ├── cost_parameters.h │ │ ├── local_planner.h │ │ ├── local_planner_nodelet.h │ │ ├── local_planner_visualization.h │ │ ├── planner_functions.h │ │ ├── star_planner.h │ │ ├── trajectory_simulator.h │ │ ├── tree_node.h │ │ └── waypoint_generator.h ├── launch │ ├── local_planner_aero.launch │ ├── local_planner_aeroD415.launch │ ├── local_planner_depth-camera.launch │ ├── local_planner_realsense.launch │ ├── local_planner_sitl_3cam.launch │ ├── local_planner_stereo.launch │ ├── log_replay.launch │ ├── mavros.launch │ └── rs_depthcloud.launch ├── nodelets.xml ├── package.xml ├── resource │ ├── custom_rosconsole.conf │ ├── local_planner.rviz │ ├── rqt_param_toggle.sh │ └── stereo_calib.json ├── src │ ├── nodes │ │ ├── local_planner.cpp │ │ ├── local_planner_node_main.cpp │ │ ├── local_planner_nodelet.cpp │ │ ├── local_planner_visualization.cpp │ │ ├── planner_functions.cpp │ │ ├── star_planner.cpp │ │ ├── tree_node.cpp │ │ ├── waypoint_generator.cpp │ │ └── waypoint_generator.cpp.dot │ └── utils │ │ └── trajectory_simulator.cpp └── test │ ├── main.cpp │ ├── test_example.cpp │ ├── test_local_planner.cpp │ ├── test_local_planner_nodelet.cpp │ ├── test_planner_functions.cpp │ ├── test_star_planner.cpp │ ├── test_trajectory_simulator.cpp │ ├── test_waypoint_generator.cpp │ └── valgrind_suppressions.sup ├── safe_landing_planner ├── CMakeLists.txt ├── cfg │ ├── SafeLandingPlannerNode.cfg │ ├── WaypointGeneratorNode.cfg │ ├── slpn.yaml │ └── wpgn.yaml ├── include │ └── safe_landing_planner │ │ ├── grid.hpp │ │ ├── safe_landing_planner.hpp │ │ ├── safe_landing_planner_node.hpp │ │ ├── safe_landing_planner_visualization.hpp │ │ ├── waypoint_generator.hpp │ │ └── waypoint_generator_node.hpp ├── launch │ ├── safe_landing_planner.launch │ ├── safe_landing_planner_launch.launch │ ├── safe_landing_planner_rosbag.launch │ └── safe_landing_planner_vtol.launch ├── msg │ └── SLPGridMsg.msg ├── package.xml ├── resource │ ├── lsd_realsense.rviz │ ├── realsense_params.sh │ ├── rqt_param_toggle.sh │ ├── safe_landing_planner.rviz │ ├── safe_landing_planner_rosbag.rviz │ └── stereo_calib.json ├── src │ └── nodes │ │ ├── safe_landing_planner.cpp │ │ ├── safe_landing_planner_node.cpp │ │ ├── safe_landing_planner_node_main.cpp │ │ ├── safe_landing_planner_visualization.cpp │ │ ├── waypoint_generator.cpp │ │ ├── waypoint_generator.cpp.dot │ │ └── waypoint_generator_node.cpp └── test │ ├── main.cpp │ ├── test_grid.cpp │ ├── test_safe_landing_planner.cpp │ └── test_waypoint_generator.cpp └── tools ├── check_code_format.sh ├── check_state_machine_diagrams.sh ├── fix_style.sh ├── generate_coverage.sh ├── generate_coverage_html.sh ├── generate_flow_diagram.py ├── generate_launchfile.sh.deprecated └── set_up_commit_hooks.sh /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Drone switched on '...' 16 | 2. Uploaded mission '....' (attach QGC mission file) 17 | 3. Took off '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Log Files and Screenshots** 24 | *Always* provide a link to the flight log file: 25 | - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/en/getting_started/flight_reporting.html)) or from the Gazebo simulation (logs are located in `~/.ros/logs/`) 26 | - Share the link to a log showing the problem on [PX4 Flight Review](http://logs.px4.io/). 27 | - Share a link to a rosbag showing the problem 28 | 29 | **Additional context** 30 | Add any other context about the problem here. 31 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | For general questions please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/). 11 | 12 | **Describe problem solved by the proposed feature** 13 | A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ... 14 | 15 | **Describe your preferred solution** 16 | A clear and concise description of what you want to happen. 17 | 18 | **Describe possible alternatives** 19 | A clear and concise description of alternative solutions or features you've considered. 20 | 21 | **Additional context** 22 | Add any other context or screenshots for the feature request here. 23 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/support-issue.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Support Issue 3 | about: 'See [PX4 Discuss](http://discuss.px4.io/) for questions about using PX4. ' 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | We use GitHub issues only to discuss PX4 Avoidance bugs and new features. For 11 | questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/). 12 | 13 | Thanks! 14 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | *.sublime-workspace 3 | *.sublime-project 4 | *.tags 5 | *.tags_sorted_by_file 6 | .clang_complete 7 | *.kdev4 8 | /local_planner/launch/avoidance.launch 9 | coverage_html/* 10 | repo_total.info 11 | */build/* 12 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Based on https://github.com/felixduvallet/ros-travis-integration 2 | # 3 | # vim:set ts=2 sw=2 et: 4 | language: C++ 5 | 6 | matrix: 7 | include: 8 | - dist: xenial 9 | env: ROS_DISTRO=kinetic 10 | - dist: bionic 11 | env: ROS_DISTRO=melodic 12 | sudo: required 13 | compiler: 14 | - gcc 15 | cache: 16 | directories: 17 | - $HOME/.cache/pip 18 | before_cache: 19 | - rm -f $HOME/.cache/pip/log/debug.log 20 | cache: 21 | - apt 22 | - ccache 23 | 24 | env: 25 | global: 26 | - ROS_CI_DESKTOP="$(lsb_release -cs)" 27 | - CI_SOURCE_PATH=$(pwd) 28 | - export COVERALLS_SERVICE_NAME=travis-ci 29 | 30 | before_install: 31 | - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 32 | - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 33 | - sudo apt-get update -qq 34 | - sudo apt-get install -y build-essential git valgrind ccache 35 | - sudo apt-get install -y python-rosinstall python-rosinstall-generator python-wstool python-catkin-tools 36 | # Install ROS Kinetic 37 | - sudo apt-get install -y ros-$ROS_DISTRO-ros-base 38 | # Install yaml-cpp ROS package 39 | - sudo apt-get install -y libyaml-cpp-dev 40 | # Install MavLink and Mavros 41 | - sudo apt-get install -y ros-$ROS_DISTRO-mavlink ros-$ROS_DISTRO-mavros-msgs ros-$ROS_DISTRO-mavros ros-$ROS_DISTRO-mavros-extras 42 | # Source environment 43 | - source /opt/ros/$ROS_DISTRO/setup.bash 44 | # Prepare rosdep 45 | - sudo rosdep init 46 | - rosdep update 47 | 48 | install: 49 | # Create catkin workspace 50 | - mkdir -p ~/catkin_ws 51 | - cd ~/catkin_ws 52 | - catkin config --init --mkdirs 53 | # Pull source depends 54 | - cd src 55 | - wstool init 56 | # Link the repository we are testing to the new workspace 57 | - ln -s $CI_SOURCE_PATH . 58 | # Install dependency 59 | - cd ~/catkin_ws 60 | - rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -q -y 61 | # Install GeographicLib datasets 62 | - sudo -H mkdir -p /usr/share/geographiclib 63 | - wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh 64 | - chmod +x install_geographiclib_datasets.sh 65 | - sudo -H ./install_geographiclib_datasets.sh 66 | 67 | before_script: 68 | # Source environment 69 | - source /opt/ros/$ROS_DISTRO/setup.bash 70 | 71 | script: 72 | - set -e 73 | - cd ~/catkin_ws 74 | - catkin build 75 | - source ~/catkin_ws/devel/setup.bash 76 | # Check for clang format 77 | - cd src/avoidance 78 | - git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*" 79 | - (cd tools && ./check_state_machine_diagrams.sh) 80 | - catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False -DDISABLE_SIMULATION=ON && catkin build 81 | - catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=True && catkin build avoidance local_planner global_planner safe_landing_planner --no-deps -v -i --catkin-make-args tests 82 | - status=0 && for f in ~/catkin_ws/devel/lib/*/*-test; do valgrind --leak-check=full --track-origins=yes --error-exitcode=1 $f || exit 1; done 83 | - if [ $ROS_DISTRO == kinetic ]; then sudo apt-get install -y clang-format-3.8; fi 84 | - if [ $ROS_DISTRO == kinetic ]; then (cd tools && ./check_code_format.sh); fi 85 | - if [ $ROS_DISTRO == kinetic ]; then (roscore &) && for f in ~/catkin_ws/devel/lib/*/*-test-roscore; do valgrind --leak-check=full --track-origins=yes --error-exitcode=1 --suppressions=local_planner/test/valgrind_suppressions.sup $f || status=1; done && (if (( $status > 0 )); then exit $status; fi) ; fi 86 | 87 | after_success: 88 | - if [ $ROS_DISTRO == kinetic ]; then sudo apt-get install -y lcov && sudo gem install coveralls-lcov; fi 89 | - if [ $ROS_DISTRO == kinetic ]; then (cd ~/catkin_ws/src/avoidance && tools/generate_coverage.sh && coveralls-lcov repo_total.info) ; fi 90 | -------------------------------------------------------------------------------- /Jenkinsfile: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env groovy 2 | 3 | pipeline { 4 | agent none 5 | stages { 6 | 7 | stage('Analysis') { 8 | 9 | parallel { 10 | 11 | stage('Catkin build on ROS workspace') { 12 | agent { 13 | docker { 14 | image 'px4io/px4-dev-ros-melodic:2019-10-04' 15 | args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' 16 | } 17 | } 18 | steps { 19 | sh 'ls -l' 20 | sh '''#!/bin/bash -l 21 | echo $0; 22 | mkdir -p catkin_ws/src; 23 | cd catkin_ws; 24 | git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo; 25 | git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo; 26 | git -C ${WORKSPACE}/catkin_ws/src/Firmware fetch --tags; 27 | git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/avoidance.git src -f ${GIT_COMMIT}; 28 | source /opt/ros/melodic/setup.bash; 29 | catkin init; 30 | catkin build -j$(nproc) -l$(nproc); 31 | ''' 32 | } 33 | post { 34 | always { 35 | sh 'rm -rf catkin_ws' 36 | } 37 | failure { 38 | archiveArtifacts(allowEmptyArchive: false, artifacts: '.ros/**/*.xml, .ros/**/*.log') 39 | } 40 | } 41 | options { 42 | checkoutToSubdirectory('catkin_ws/src/Firmware') 43 | } 44 | } 45 | } // prallel 46 | } // stage analysis 47 | } // stages 48 | 49 | environment { 50 | CCACHE_DIR = '/tmp/ccache' 51 | CI = true 52 | GIT_AUTHOR_EMAIL = "bot@px4.io" 53 | GIT_AUTHOR_NAME = "PX4BuildBot" 54 | GIT_COMMITTER_EMAIL = "bot@px4.io" 55 | GIT_COMMITTER_NAME = "PX4BuildBot" 56 | } 57 | options { 58 | buildDiscarder(logRotator(numToKeepStr: '20', artifactDaysToKeepStr: '30')) 59 | timeout(time: 60, unit: 'MINUTES') 60 | } 61 | } -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2016 - 2018, PX4 Development Team 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * 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 | * 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 | -------------------------------------------------------------------------------- /avoidance/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(avoidance) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | ## Find catkin macros and libraries 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | dynamic_reconfigure 11 | tf 12 | pcl_ros 13 | mavros 14 | mavros_extras 15 | mavros_msgs 16 | mavlink 17 | ) 18 | find_package(PCL 1.7 REQUIRED) 19 | 20 | if(DISABLE_SIMULATION) 21 | message(STATUS "Building avoidance without Gazebo Simulation") 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDISABLE_SIMULATION") 23 | else() 24 | message(STATUS "Building avoidance with Gazebo Simulation") 25 | find_package(yaml-cpp REQUIRED) 26 | endif(DISABLE_SIMULATION) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | CATKIN_DEPENDS roscpp rospy std_msgs mavros_msgs geometry_msgs sensor_msgs message_runtime tf 31 | LIBRARIES avoidance 32 | ) 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | 38 | ## CMake Setup 39 | # Build in Release mode if nothing is specified 40 | if(NOT CMAKE_BUILD_TYPE) 41 | set(CMAKE_BUILD_TYPE Release) 42 | endif(NOT CMAKE_BUILD_TYPE) 43 | 44 | ## Specify additional locations of header files 45 | include_directories( 46 | include 47 | ${catkin_INCLUDE_DIRS} 48 | ${PCL_INCLUDE_DIRS} 49 | ${YAML_CPP_INCLUDE_DIR} 50 | ) 51 | 52 | ## Declare a C++ library 53 | set(AVOIDANCE_CPP_FILES "src/common.cpp" 54 | "src/histogram.cpp" 55 | "src/transform_buffer.cpp" 56 | "src/avoidance_node.cpp" 57 | ) 58 | if(NOT DISABLE_SIMULATION) 59 | set(AVOIDANCE_CPP_FILES "${AVOIDANCE_CPP_FILES}" 60 | "src/rviz_world_loader.cpp") 61 | endif() 62 | add_library(avoidance "${AVOIDANCE_CPP_FILES}") 63 | 64 | 65 | ## Add cmake target dependencies of the library 66 | add_dependencies(avoidance ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 67 | 68 | ## Specify libraries to link a library or executable target against 69 | target_link_libraries(avoidance 70 | ${catkin_LIBRARIES} 71 | ${YAML_CPP_LIBRARIES}) 72 | 73 | ############# 74 | ## Install ## 75 | ############# 76 | 77 | install(DIRECTORY include/${PROJECT_NAME}/ 78 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 79 | FILES_MATCHING PATTERN "*.h" 80 | PATTERN ".svn" EXCLUDE) 81 | 82 | ############# 83 | ## Testing ## 84 | ############# 85 | 86 | if(CATKIN_ENABLE_TESTING) 87 | # Add gtest based cpp test target and link libraries 88 | catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp 89 | test/test_common.cpp 90 | test/test_usm.cpp 91 | test/test_transform_buffer.cpp 92 | ) 93 | 94 | if(TARGET ${PROJECT_NAME}-test) 95 | target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} 96 | ${catkin_LIBRARIES} 97 | ${YAML_CPP_LIBRARIES}) 98 | endif() 99 | 100 | 101 | if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage") 102 | SET(CMAKE_CXX_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage --coverage") 103 | SET(CMAKE_C_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage --coverage") 104 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage") 105 | 106 | add_custom_target(${PROJECT_NAME}-test_coverage 107 | COMMAND lcov --zerocounters --directory ${PROJECT_BINARY_DIR} 108 | COMMAND lcov --capture --initial --no-external --directory ${PROJECT_BINARY_DIR} --base-directory ${${PROJECT_NAME}_SOURCE_DIR} --output-file base_coverage.info --rc lcov_branch_coverage=1 109 | COMMAND ${PROJECT_NAME}-test 110 | COMMAND lcov --capture --no-external --directory ${PROJECT_BINARY_DIR} --base-directory ${${PROJECT_NAME}_SOURCE_DIR} --output-file test_coverage.info --rc lcov_branch_coverage=1 111 | COMMAND lcov -a base_coverage.info -a test_coverage.info -o coverage.info --rc lcov_branch_coverage=1 112 | COMMAND lcov --rc lcov_branch_coverage=1 --summary coverage.info 113 | WORKING_DIRECTORY . 114 | DEPENDS ${PROJECT_NAME}-test 115 | ) 116 | add_custom_target(${PROJECT_NAME}-test_coverage_html 117 | COMMAND genhtml coverage.info --output-directory out --branch-coverage 118 | COMMAND x-www-browser out/index.html 119 | WORKING_DIRECTORY . 120 | DEPENDS ${PROJECT_NAME}-test_coverage 121 | ) 122 | endif() 123 | endif() 124 | -------------------------------------------------------------------------------- /avoidance/include/avoidance/avoidance_node.h: -------------------------------------------------------------------------------- 1 | #ifndef AVOIDANCE_AVOIDANCE_NODE_H 2 | #define AVOIDANCE_AVOIDANCE_NODE_H 3 | 4 | #include "ros/callback_queue.h" 5 | #include "ros/ros.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include "avoidance/common.h" 11 | #include "mavros_msgs/CompanionProcessStatus.h" 12 | 13 | #include 14 | 15 | namespace avoidance { 16 | 17 | class AvoidanceNode { 18 | public: 19 | AvoidanceNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 20 | ~AvoidanceNode(); 21 | /** 22 | * @brief check healthiness of the avoidance system to trigger failsafe in 23 | * the FCU 24 | * @param[in] since_last_cloud, time elapsed since the last waypoint was 25 | * published to the FCU 26 | * @param[in] since_start, time elapsed since staring the node 27 | * @param[out] planner_is_healthy, true if the planner is running without 28 | *errors 29 | * @param[out] hover, true if the vehicle is hovering 30 | **/ 31 | void checkFailsafe(ros::Duration since_last_cloud, ros::Duration since_start, bool& hover); 32 | 33 | ModelParameters getPX4Parameters() const; 34 | float getMissionItemSpeed() const { return mission_item_speed_; } 35 | MAV_STATE getSystemStatus(); 36 | 37 | /** 38 | * @brief polls PX4 Firmware paramters every 30 seconds 39 | **/ 40 | void checkPx4Parameters(); 41 | 42 | void setSystemStatus(MAV_STATE state); 43 | void init(); 44 | 45 | /** 46 | * @brief callaback with the list of FCU Mission Items 47 | * @param[in] msg, list of mission items 48 | **/ 49 | void missionCallback(const mavros_msgs::WaypointList& msg); 50 | 51 | private: 52 | ros::NodeHandle nh_; 53 | ros::NodeHandle nh_private_; 54 | 55 | ros::Publisher mavros_system_status_pub_; 56 | 57 | ros::Subscriber px4_param_sub_; 58 | ros::Subscriber mission_sub_; 59 | 60 | ros::ServiceClient get_px4_param_client_; 61 | 62 | ros::Timer cmdloop_timer_, statusloop_timer_; 63 | ros::CallbackQueue cmdloop_queue_, statusloop_queue_; 64 | std::unique_ptr cmdloop_spinner_; 65 | std::unique_ptr statusloop_spinner_; 66 | 67 | MAV_STATE companion_state_ = MAV_STATE::MAV_STATE_STANDBY; 68 | 69 | ModelParameters px4_; // PX4 Firmware paramters 70 | 71 | std::thread worker_; 72 | 73 | double cmdloop_dt_, statusloop_dt_; 74 | double timeout_termination_; 75 | double timeout_critical_; 76 | double timeout_startup_; 77 | 78 | bool position_received_; 79 | bool should_exit_; 80 | 81 | float mission_item_speed_; 82 | 83 | void cmdLoopCallback(const ros::TimerEvent& event); 84 | void statusLoopCallback(const ros::TimerEvent& event); 85 | void publishSystemStatus(); 86 | 87 | /** 88 | * @brief callaback with the list of FCU parameters 89 | * @param[in] msg, list of paramters 90 | **/ 91 | void px4ParamsCallback(const mavros_msgs::Param& msg); 92 | }; 93 | } 94 | #endif // AVOIDANCE_AVOIDANCE_NODE_H 95 | -------------------------------------------------------------------------------- /avoidance/include/avoidance/histogram.h: -------------------------------------------------------------------------------- 1 | #ifndef HISTOGRAM_H 2 | #define HISTOGRAM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace avoidance { 10 | 11 | // Be very careful choosing the resolution! Valid resolutions must fullfill: 12 | // 180 % (2 * ALPHA_RES) = 0 13 | // Examples of valid resolution values: 1, 3, 5, 6, 10, 15, 18, 30, 45, 60 14 | const int ALPHA_RES = 6; 15 | const int GRID_LENGTH_Z = 360 / ALPHA_RES; 16 | const int GRID_LENGTH_E = 180 / ALPHA_RES; 17 | 18 | class Histogram { 19 | int resolution_; 20 | int z_dim_; 21 | int e_dim_; 22 | Eigen::MatrixXf dist_; 23 | 24 | /** 25 | * @brief wraps elevation and azimuth indeces around the histogram 26 | * @param x, elevation angle index 27 | * @param y, azimuth angle index 28 | **/ 29 | inline void wrapIndex(int &x, int &y) const { 30 | x = x % e_dim_; 31 | if (x < 0) x += e_dim_; 32 | y = y % z_dim_; 33 | if (y < 0) y += z_dim_; 34 | } 35 | 36 | public: 37 | Histogram(const int res); 38 | ~Histogram() = default; 39 | 40 | /** 41 | * @brief getter method for histogram cell distance 42 | * @param[in] x, elevation angle index 43 | * @param[in] y, azimuth angle index 44 | * @returns distance to the vehicle of obstacle mapped to (x, y) cell [m] 45 | **/ 46 | inline float get_dist(int x, int y) const { 47 | wrapIndex(x, y); 48 | return dist_(x, y); 49 | } 50 | 51 | /** 52 | * @brief setter method for histogram cell distance 53 | * @param[in] x, elevation angle index 54 | * @param[in] y, azimuth angle index 55 | * @param[in] value, distance to the vehicle of obstacle mapped to (x, y) cell 56 | *[m] 57 | **/ 58 | inline void set_dist(int x, int y, float value) { dist_(x, y) = value; } 59 | 60 | /** 61 | * @brief Compute the upsampled version of the histogram 62 | * @param[in] This object. Needs to be a histogram the larger bin size 63 | *(ALPHA_RES * 2) 64 | * @details The histogram is upsampled to get the same histogram at regular 65 | *bin size (ALPHA_RES). 66 | * This means the histogram matrix will be double the size in each 67 | *dimension 68 | * @returns Modifies the object it is called from to have regular resolution 69 | * @warning Can only be called from a large bin size histogram 70 | **/ 71 | void upsample(); 72 | 73 | /** 74 | * @brief Compute the downsampled version of the histogram 75 | * @param[in] this object. Needs to be a histogram with regular bin size 76 | *(ALPHA_RES) 77 | * @details The histogram is downsampled to get the same histogram at larger 78 | *bin size (ALPHA_RES/2). 79 | * This means the histogram matrix will be half the size in each 80 | *dimension 81 | * @returns Modifies the object it is called from to have larger bins 82 | * @warning Can only be called from a regular bin size histogram 83 | **/ 84 | void downsample(); 85 | 86 | /** 87 | * @brief resets all histogram cells age and distance to zero 88 | **/ 89 | void setZero(); 90 | 91 | /** 92 | * @brief determines whether the histogram is empty (distance layer 93 | * contains no distance bigger than zero) 94 | * @returns whether histogram is empty 95 | **/ 96 | bool isEmpty() const; 97 | }; 98 | } 99 | 100 | #endif // HISTOGRAM_H 101 | -------------------------------------------------------------------------------- /avoidance/include/avoidance/rviz_world_loader.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_WORLD_H 2 | #define RVIZ_WORLD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "yaml-cpp/yaml.h" 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace avoidance { 19 | 20 | // data types 21 | struct world_object { 22 | std::string type; 23 | std::string name; 24 | std::string frame_id; 25 | std::string mesh_resource; 26 | Eigen::Vector3f position; 27 | Eigen::Vector4f orientation; 28 | Eigen::Vector3f scale; 29 | }; 30 | 31 | // yaml file extraction operators 32 | void operator>>(const YAML::Node& node, Eigen::Vector3f& v); 33 | void operator>>(const YAML::Node& node, Eigen::Vector4f& v); 34 | void operator>>(const YAML::Node& node, world_object& item); 35 | 36 | class WorldVisualizer { 37 | private: 38 | /** 39 | * @brief helper function to resolve gazebo model path 40 | **/ 41 | int resolveUri(std::string& uri); 42 | 43 | ros::NodeHandle nh_; 44 | 45 | ros::Timer loop_timer_; 46 | 47 | ros::Subscriber pose_sub_; 48 | ros::Publisher world_pub_; 49 | ros::Publisher drone_pub_; 50 | 51 | std::string world_path_; 52 | std::string nodelet_ns_; 53 | 54 | void loopCallback(const ros::TimerEvent& event); 55 | 56 | public: 57 | WorldVisualizer(const ros::NodeHandle& nh, const std::string& nodelet_ns); 58 | 59 | /** 60 | * @brief initializes all publishers used for local planner visualization 61 | * @param nh, nodehandle to initialize publishers 62 | **/ 63 | void initializePublishers(ros::NodeHandle& nh); 64 | 65 | /** 66 | * @brief callback for subscribing mav pose topic 67 | **/ 68 | void positionCallback(const geometry_msgs::PoseStamped& msg); 69 | 70 | /** 71 | * @brief parse the yaml file and publish world marker 72 | * @param[in] world_path, path of the yaml file describing the world 73 | **/ 74 | int visualizeRVIZWorld(const std::string& world_path); 75 | 76 | /** 77 | * @brief visualize the drone mesh at the current drone position 78 | * @param[in] pose, current drone pose 79 | **/ 80 | int visualizeDrone(const geometry_msgs::PoseStamped& pose); 81 | }; 82 | } 83 | 84 | #endif // RVIZ_WORLD_H 85 | -------------------------------------------------------------------------------- /avoidance/include/avoidance/transform_buffer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace avoidance { 12 | 13 | namespace tf_buffer { 14 | 15 | enum log_level { error, warn, info, debug }; 16 | 17 | class TransformBuffer { 18 | public: 19 | TransformBuffer(float buffer_size_s = 10.0f); 20 | virtual ~TransformBuffer() = default; 21 | 22 | /** 23 | * @brief inserts transform into buffer 24 | * @param[in] source_frame 25 | * @param[in] target_frame 26 | * @param[in] transform 27 | * @returns bool, true if transform was inserted (will not be inserted 28 | * if it is the same or older than the last one that was buffered) 29 | **/ 30 | bool insertTransform(const std::string& source_frame, const std::string& target_frame, 31 | tf::StampedTransform transform); 32 | 33 | /** 34 | * @brief retrieves transform from buffer 35 | * @param[in] source_frame 36 | * @param[in] target_frame 37 | * @param[in] time, timestamp of the requested transform 38 | * @param[out] transform 39 | * @returns bool, true if the transform could be retrieved from the buffer 40 | **/ 41 | bool getTransform(const std::string& source_frame, const std::string& target_frame, const ros::Time& time, 42 | tf::StampedTransform& transform) const; 43 | 44 | protected: 45 | std::unordered_map> buffer_; 46 | mutable std::mutex mutex_; 47 | ros::Duration buffer_size_; 48 | ros::Time startup_time_; 49 | 50 | /** 51 | * @brief gets a key word from two frame names to identify a transform 52 | * @param[in] source_frame, name of the source frame 53 | * @param[in] target_frame, name of the target frame 54 | * @returns key string 55 | **/ 56 | std::string getKey(const std::string& source_frame, const std::string& target_frame) const; 57 | 58 | /** 59 | * @brief interpolates between transforms 60 | * @param[in] tf_earlier 61 | * @param[in] tf_later 62 | * @param[in/out] transform, [in] empty transform stamped with DESIRED TIMESTAMP 63 | * [out] correctly interpolated tf if interpolation is possible 64 | * otherwise transform remains unchanged 65 | * @returns bool, true if the transform was correctly interpolated 66 | **/ 67 | bool interpolateTransform(const tf::StampedTransform& tf_earlier, const tf::StampedTransform& tf_later, 68 | tf::StampedTransform& transform) const; 69 | 70 | /** 71 | * @brief prints a message 72 | * @param[in] level, valid options are: error, warn, info, debug 73 | * @param[in] msg, string that should be printed 74 | **/ 75 | void print(const log_level& level, const std::string& msg) const; 76 | }; 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /avoidance/include/avoidance/usm.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | * Copyright (c) 2019 Julian Kent. All rights reserved. 5 | * 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright notice, this 12 | * list of conditions and the following disclaimer. 13 | * 14 | * * Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * * Neither the name of usm nor the names of its 19 | * contributors may be used to endorse or promote products derived from 20 | * this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 23 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 24 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 30 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * 34 | * 35 | * For usage examples, look at the tests. 36 | */ 37 | 38 | namespace usm { 39 | 40 | enum class Transition { REPEAT, NEXT1, NEXT2, NEXT3, NEXT4, ERROR }; 41 | 42 | template 43 | class StateMachine { 44 | public: 45 | StateMachine(StateEnum startingState); 46 | void iterateOnce(); 47 | 48 | StateEnum getState(); 49 | 50 | protected: 51 | virtual Transition runCurrentState() = 0; // implement using a big switch 52 | virtual StateEnum chooseNextState(StateEnum currentState, Transition transition) = 0; // nested switches 53 | 54 | private: 55 | StateEnum m_currentState; 56 | }; 57 | 58 | /*---------------IMPLEMENTATION------------------*/ 59 | 60 | template 61 | StateMachine::StateMachine(StateEnum startingState) : m_currentState(startingState) {} 62 | 63 | template 64 | void StateMachine::iterateOnce() { 65 | Transition t = runCurrentState(); 66 | if (t != Transition::REPEAT) m_currentState = chooseNextState(m_currentState, t); 67 | } 68 | 69 | template 70 | StateEnum StateMachine::getState() { 71 | return m_currentState; 72 | } 73 | } 74 | 75 | /*---------------MACROS TO MAKE TRANSITION TABLES EASY------------------*/ 76 | 77 | // clang-format off 78 | #define USM_TABLE(current_state, error, ...) \ 79 | switch (current_state) { \ 80 | __VA_ARGS__; \ 81 | default: break; \ 82 | } \ 83 | return error 84 | 85 | #define USM_STATE(transition, start_state, ...) \ 86 | case start_state: \ 87 | switch (transition) { \ 88 | __VA_ARGS__; \ 89 | default: break; \ 90 | } \ 91 | break 92 | 93 | #define USM_MAP(transition, next_state) \ 94 | case transition: return next_state 95 | // clang-format on 96 | -------------------------------------------------------------------------------- /avoidance/launch/avoidance_sitl_mavros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /avoidance/launch/avoidance_sitl_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /avoidance/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | avoidance 4 | 0.2.0 5 | avoidance is a ros package for computer vision based navigtation 6 | 7 | 8 | 9 | 10 | pixhawk 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | dynamic_reconfigure 45 | message_generation 46 | roscpp 47 | rospy 48 | std_msgs 49 | geometry_msgs 50 | sensor_msgs 51 | tf 52 | pcl_ros 53 | mavros 54 | mavros_extras 55 | mavros_msgs 56 | 57 | dynamic_reconfigure 58 | message_runtime 59 | roscpp 60 | rospy 61 | std_msgs 62 | geometry_msgs 63 | sensor_msgs 64 | octomap 65 | tf 66 | pcl_ros 67 | mavros 68 | mavros_extras 69 | mavros_msgs 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /avoidance/resource/px4_config.yaml: -------------------------------------------------------------------------------- 1 | # Common configuration for PX4 autopilot 2 | # 3 | # node: 4 | startup_px4_usb_quirk: true 5 | 6 | # --- system plugins --- 7 | 8 | # sys_status & sys_time connection options 9 | conn: 10 | heartbeat_rate: 1.0 # send hertbeat rate in Hertz 11 | timeout: 10.0 # hertbeat timeout in seconds 12 | timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) 13 | system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) 14 | 15 | # sys_status 16 | sys: 17 | min_voltage: 10.0 # diagnostics min voltage 18 | disable_diag: false # disable all sys_status diagnostics, except heartbeat 19 | 20 | # sys_time 21 | time: 22 | time_ref_source: "fcu" # time_reference source 23 | timesync_avg_alpha: 0.6 # timesync averaging factor 24 | 25 | # --- mavros plugins (alphabetical order) --- 26 | 27 | # 3dr_radio 28 | tdr_radio: 29 | low_rssi: 40 # raw rssi lower level for diagnostics 30 | 31 | # actuator_control 32 | # None 33 | 34 | # command 35 | cmd: 36 | use_comp_id_system_control: false # quirk for some old FCUs 37 | 38 | # dummy 39 | # None 40 | 41 | # ftp 42 | # None 43 | 44 | # global_position 45 | global_position: 46 | frame_id: "fcu" # pose and fix frame_id 47 | rot_covariance: 99999.0 # covariance for attitude? 48 | tf: 49 | send: false # send TF? 50 | frame_id: "local_origin" # TF frame_id 51 | child_frame_id: "fcu_utm" # TF child_frame_id 52 | 53 | # imu_pub 54 | imu: 55 | frame_id: "fcu" 56 | # need find actual values 57 | linear_acceleration_stdev: 0.0003 58 | angular_velocity_stdev: 0.0003490659 // 0.02 degrees 59 | orientation_stdev: 1.0 60 | magnetic_stdev: 0.0 61 | 62 | # local_position 63 | local_position: 64 | frame_id: "local_origin" 65 | tf: 66 | send: true 67 | frame_id: "local_origin" 68 | child_frame_id: "fcu" 69 | send_fcu: false 70 | 71 | # param 72 | # None, used for FCU params 73 | 74 | # rc_io 75 | # None 76 | 77 | # safety_area 78 | safety_area: 79 | p1: {x: 1.0, y: 1.0, z: 1.0} 80 | p2: {x: -1.0, y: -1.0, z: -1.0} 81 | 82 | # setpoint_accel 83 | setpoint_accel: 84 | send_force: false 85 | 86 | # setpoint_attitude 87 | setpoint_attitude: 88 | reverse_throttle: false # allow reversed throttle 89 | tf: 90 | listen: false # enable tf listener (disable topic subscribers) 91 | frame_id: "local_origin" 92 | child_frame_id: "attitude" 93 | rate_limit: 10.0 94 | 95 | # setpoint_position 96 | setpoint_position: 97 | tf: 98 | listen: false # enable tf listener (disable topic subscribers) 99 | frame_id: "local_origin" 100 | child_frame_id: "setpoint" 101 | rate_limit: 50.0 102 | 103 | # setpoint_velocity 104 | # None 105 | 106 | # vfr_hud 107 | # None 108 | 109 | # waypoint 110 | mission: 111 | pull_after_gcs: true # update mission if gcs updates 112 | 113 | # --- mavros extras plugins (same order) --- 114 | 115 | # distance_sensor (PX4 only) 116 | distance_sensor: 117 | hrlv_ez4_pub: 118 | id: 0 119 | frame_id: "hrlv_ez4_sonar" 120 | orientation: ROLL_180 # RPY:{180.0, 0.0, 0.0} 121 | field_of_view: 0.0 # XXX TODO 122 | send_tf: true 123 | sensor_position: {x: 0.0, y: 0.0, z: -0.1} 124 | lidarlite_pub: 125 | id: 1 126 | frame_id: "lidarlite_laser" 127 | orientation: ROLL_180 128 | field_of_view: 0.0 # XXX TODO 129 | send_tf: true 130 | sensor_position: {x: 0.0, y: 0.0, z: -0.1} 131 | sonar_1_sub: 132 | subscriber: true 133 | id: 2 134 | orientation: ROLL_180 135 | laser_1_sub: 136 | subscriber: true 137 | id: 3 138 | orientation: ROLL_180 139 | 140 | ## Currently available orientations: 141 | # Check http://wiki.ros.org/mavros/Enumerations 142 | ## 143 | 144 | # image_pub 145 | image: 146 | frame_id: "px4flow" 147 | 148 | # mocap_pose_estimate 149 | mocap: 150 | # select mocap source 151 | use_tf: true # ~mocap/tf 152 | use_pose: false # ~mocap/pose 153 | 154 | # px4flow 155 | px4flow: 156 | frame_id: "px4flow" 157 | ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter 158 | ranger_min_range: 0.3 # meters 159 | ranger_max_range: 5.0 # meters 160 | 161 | # vision_pose_estimate 162 | vision_pose: 163 | tf: 164 | listen: false # enable tf listener (disable topic subscribers) 165 | frame_id: "local_origin" 166 | child_frame_id: "vision" 167 | rate_limit: 10.0 168 | 169 | # vision_speed_estimate 170 | vision_speed: 171 | listen_twist: false 172 | 173 | # vibration 174 | vibration: 175 | frame_id: "vibration" 176 | 177 | # vim:set ts=2 sw=2 et: 178 | -------------------------------------------------------------------------------- /avoidance/sim/models/distance_sensor/distance_sensor.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.035 0 0 0 5 | 6 | 7 | 0.01 0.025 0.025 0 0 0 8 | 0.01 9 | 10 | 4.15e-6 11 | 0 12 | 0 13 | 2.407e-6 14 | 0 15 | 2.407e-6 16 | 17 | 18 | 19 | 0 0 0 0 1.58 0 20 | 21 | 22 | model://depth_camera_new/meshes/hokuyo.dae 23 | 24 | 25 | 26 | 27 | 0 0 0 0 0 0 28 | false 29 | 5 30 | 31 | 32 | 33 | 720 34 | 1 35 | -3.14 36 | 3.14 37 | 38 | 39 | 40 | 0.40 41 | 30.0 42 | 0.01 43 | 44 | 45 | 46 | 47 | 48 | /scan 49 | laser_link 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /avoidance/sim/models/distance_sensor/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Distance Sensor 5 | 1.0 6 | distance_sensor.sdf 7 | 8 | 9 | Distance Sensor 10 | 11 | 12 | -------------------------------------------------------------------------------- /avoidance/sim/models/iris_realsense/iris_realsense.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | model://iris 5 | 6 | 7 | 8 | model://realsense_camera 9 | 0.1 0 0 0 0 0 10 | 11 | 12 | realsense_camera::link 13 | iris::base_link 14 | 15 | 0 0 1 16 | 17 | 0 18 | 0 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /avoidance/sim/models/iris_realsense/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 3DR Iris 4 | 1.0 5 | iris_realsense.sdf 6 | 7 | 8 | Tanja Baumann 9 | tanja@auterion.com 10 | 11 | 12 | 13 | This is a model of the 3DR Iris Quadrotor with one Realsense R200 camera. The original model has been created by 14 | Thomas Gubler and is maintained by Lorenz Meier. 15 | 16 | 17 | -------------------------------------------------------------------------------- /avoidance/sim/models/lamp_with_lines/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | lamp_with_lines 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /avoidance/sim/models/lamp_with_lines/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 5 | 0 0 3e-06 0 -0 0 6 | 7 | 1 8 | 9 | 0.166667 10 | 0 11 | 0 12 | 0.166667 13 | 0 14 | 0.166667 15 | 16 | 17 | 18 | 0 0 0 0 -0 0 19 | 20 | 21 | //lamp_with_lines.dae 22 | 0.4 0.4 0.4 23 | 24 | 25 | 26 | 1 27 | 31 | 0.3 0.3 0.3 1 32 | 0.7 0.7 0.7 1 33 | 0.01 0.01 0.01 1 34 | 0 0 0 1 35 | 36 | 1 37 | 0 38 | 39 | 40 | 0 41 | 10 42 | 0 0 0 0 -0 0 43 | 44 | 45 | //lamp_with_lines.dae 46 | 47 | 48 | 49 | 50 | 51 | 1 52 | 1 53 | 0 0 0 54 | 0 55 | 0 56 | 57 | 58 | 1 59 | 0 60 | 0 61 | 1 62 | 63 | 0 64 | 65 | 66 | 67 | 68 | 0 69 | 1e+06 70 | 71 | 72 | 0 73 | 1 74 | 1 75 | 76 | 0 77 | 0.2 78 | 1e+13 79 | 1 80 | 0.01 81 | 0 82 | 83 | 84 | 1 85 | -0.01 86 | 0 87 | 0.2 88 | 1e+13 89 | 1 90 | 91 | 92 | 93 | 94 | 95 | 0 96 | 1 97 | 98 | 99 | -------------------------------------------------------------------------------- /avoidance/sim/models/line_model/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | line_model 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /avoidance/sim/models/line_model/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 5 | 0 0 0 0 -0 0 6 | 7 | 1 8 | 9 | 0.166667 10 | 0 11 | 0 12 | 0.166667 13 | 0 14 | 0.166667 15 | 16 | 17 | 18 | 0 0 0 0 -0 0 19 | 20 | 21 | //line.dae 22 | 0.7 0.7 0.7 23 | 24 | 25 | 26 | 1 27 | 31 | 0.3 0.3 0.3 1 32 | 0.7 0.7 0.7 1 33 | 0.01 0.01 0.01 1 34 | 0 0 0 1 35 | 36 | 1 37 | 0 38 | 39 | 40 | 0 41 | 10 42 | 0 0 0 0 -0 0 43 | 44 | 45 | //line.dae 46 | 47 | 48 | 49 | 50 | 51 | 1 52 | 1 53 | 0 0 0 54 | 0 55 | 0 56 | 57 | 58 | 1 59 | 0 60 | 0 61 | 1 62 | 63 | 0 64 | 65 | 66 | 67 | 68 | 0 69 | 1e+06 70 | 71 | 72 | 0 73 | 1 74 | 1 75 | 76 | 0 77 | 0.2 78 | 1e+13 79 | 1 80 | 0.01 81 | 0 82 | 83 | 84 | 1 85 | -0.01 86 | 0 87 | 0.2 88 | 1e+13 89 | 1 90 | 91 | 92 | 93 | 94 | 95 | 0 96 | 1 97 | 98 | 99 | -------------------------------------------------------------------------------- /avoidance/sim/models/powerline/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | powerline 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /avoidance/sim/models/powerline/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 5 | 1e-06 -1e-06 3.2e-05 0 -0 0 6 | 7 | 1 8 | 9 | 0.166667 10 | 0 11 | 0 12 | 0.166667 13 | 0 14 | 0.166667 15 | 16 | 17 | 18 | 0 0 0 0 -0 0 19 | 20 | 21 | //powerline.dae 22 | 0.4 0.4 0.4 23 | 24 | 25 | 26 | 1 27 | 31 | 0.3 0.3 0.3 1 32 | 0.7 0.7 0.7 1 33 | 0.01 0.01 0.01 1 34 | 0 0 0 1 35 | 36 | 1 37 | 0 38 | 39 | 40 | 0 41 | 10 42 | 0 0 0 0 -0 0 43 | 44 | 45 | //powerline.dae 46 | 47 | 48 | 49 | 50 | 51 | 1 52 | 1 53 | 0 0 0 54 | 0 55 | 0 56 | 57 | 58 | 1 59 | 0 60 | 0 61 | 1 62 | 63 | 0 64 | 65 | 66 | 67 | 68 | 0 69 | 1e+06 70 | 71 | 72 | 0 73 | 1 74 | 1 75 | 76 | 0 77 | 0.2 78 | 1e+13 79 | 1 80 | 0.01 81 | 0 82 | 83 | 84 | 1 85 | -0.01 86 | 0 87 | 0.2 88 | 1e+13 89 | 1 90 | 91 | 92 | 93 | 94 | 95 | 0 96 | 1 97 | 98 | 99 | -------------------------------------------------------------------------------- /avoidance/sim/models/stereo_camera_vertical/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Stereo Camera 5 | 1.0 6 | stereo_camera_vertical.sdf 7 | 8 | Nate Koenig 9 | nate@osrfoundation.org 10 | 11 | 12 | 13 | A stereo camera. 14 | 15 | 16 | -------------------------------------------------------------------------------- /avoidance/sim/models/stereo_camera_vertical/stereo_camera_vertical.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.035 0 0 0 5 | 6 | 7 | 0.01 0.025 0.025 0 0 0 8 | 0.01 9 | 10 | 4.15e-6 11 | 0 12 | 0 13 | 2.407e-6 14 | 0 15 | 2.407e-6 16 | 17 | 18 | 19 | 0 0 0 0 1.57 0 20 | 21 | 22 | model://depth_camera_new/meshes/hokuyo.dae 23 | 24 | 25 | 26 | 27 | 7.0 28 | 29 | 0.1 0.035 0 0 0 0 30 | 1.3962634 31 | 32 | 400 33 | 400 34 | R8G8B8 35 | 36 | 37 | 0.02 38 | 300 39 | 40 | 41 | gaussian 42 | 0.0 43 | 0.007 44 | 45 | 46 | 47 | 0.1 -0.035 0 0 0 0 48 | 1.3962634 49 | 50 | 400 51 | 400 52 | R8G8B8 53 | 54 | 55 | 0.02 56 | 300 57 | 58 | 59 | gaussian 60 | 0.0 61 | 0.007 62 | 63 | 64 | 65 | true 66 | 0.0 67 | stereoV 68 | image_raw 69 | camera_info 70 | camera_link 71 | 72 | 0.07 73 | -0.25 74 | 0.12 75 | 0.0 76 | -0.00028 77 | -0.00005 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/boxes1.yaml: -------------------------------------------------------------------------------- 1 | - type: "cube" 2 | name: "box" 3 | frame_id: "local_origin" 4 | mesh_resource: "" 5 | position: [15, 0.0, 2.5] 6 | orientation: [0.0, 0.0, 0.0, 1.0] 7 | scale: [1.0, 10.0, 5.0] 8 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/boxes2.yaml: -------------------------------------------------------------------------------- 1 | - type: "cube" 2 | name: "box1" 3 | frame_id: "local_origin" 4 | mesh_resource: "" 5 | position: [1.5, 0.0, 4.0] 6 | orientation: [0.0, 0.0, 0.0, 1.0] 7 | scale: [1.0, 5.0, 8.0] 8 | 9 | - type: "cube" 10 | name: "box2" 11 | frame_id: "local_origin" 12 | mesh_resource: "" 13 | position: [3.5, 6.0, 4.0] 14 | orientation: [0.0, 0.0, 0.0, 1.0] 15 | scale: [4.0, 2.0, 8.0] 16 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/boxes3.yaml: -------------------------------------------------------------------------------- 1 | - type: "cube" 2 | name: "box1" 3 | frame_id: "local_origin" 4 | mesh_resource: "" 5 | position: [2.5, 0.0, 2.5] 6 | orientation: [0.0, 0.0, 0.0, 1.0] 7 | scale: [2.0, 5.0, 5.0] 8 | 9 | - type: "cube" 10 | name: "box2" 11 | frame_id: "local_origin" 12 | mesh_resource: "" 13 | position: [3.5, 7.0, 4.0] 14 | orientation: [0.0, 0.0, 0.0, 1.0] 15 | scale: [4.0, 2.0, 8.0] 16 | 17 | - type: "cube" 18 | name: "box3" 19 | frame_id: "local_origin" 20 | mesh_resource: "" 21 | position: [6.5, -2.0, 2.5] 22 | orientation: [0.0, 0.0, 0.0, 1.0] 23 | scale: [2.0, 2.0, 5.0] 24 | 25 | - type: "cube" 26 | name: "box4" 27 | frame_id: "local_origin" 28 | mesh_resource: "" 29 | position: [9.0, 4.0, 4.0] 30 | orientation: [0.0, 0.0, 0.0, 1.0] 31 | scale: [2.0, 2.0, 8.0] 32 | 33 | - type: "cube" 34 | name: "box5" 35 | frame_id: "local_origin" 36 | mesh_resource: "" 37 | position: [9.0, -8.0, 4.0] 38 | orientation: [0.0, 0.0, 0.0, 1.0] 39 | scale: [2.0, 2.0, 8.0] 40 | 41 | - type: "cube" 42 | name: "box6" 43 | frame_id: "local_origin" 44 | mesh_resource: "" 45 | position: [9.0, 8.0, 4.0] 46 | orientation: [0.0, 0.0, 0.0, 1.0] 47 | scale: [2.0, 2.0, 8.0] 48 | 49 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/boxes4.yaml: -------------------------------------------------------------------------------- 1 | - type: "cube" 2 | name: "box1" 3 | frame_id: "local_origin" 4 | mesh_resource: "" 5 | position: [3.5, 0.0, 2.5] 6 | orientation: [0.0, 0.0, 0.0, 1.0] 7 | scale: [2.0, 5.0, 5.0] 8 | 9 | - type: "cube" 10 | name: "box2" 11 | frame_id: "local_origin" 12 | mesh_resource: "" 13 | position: [5.5, 7.0, 4.0] 14 | orientation: [0.0, 0.0, 0.0, 1.0] 15 | scale: [6.0, 2.0, 8.0] 16 | 17 | - type: "cube" 18 | name: "box3" 19 | frame_id: "local_origin" 20 | mesh_resource: "" 21 | position: [10.0, -1.0, 2.5] 22 | orientation: [0.0, 0.0, 0.0, 1.0] 23 | scale: [4.0, 3.0, 5.0] 24 | 25 | - type: "cube" 26 | name: "box4" 27 | frame_id: "local_origin" 28 | mesh_resource: "" 29 | position: [13.0, 6.0, 4.0] 30 | orientation: [0.0, 0.0, 0.0, 1.0] 31 | scale: [2.0, 4.0, 8.0] 32 | 33 | - type: "cube" 34 | name: "box5" 35 | frame_id: "local_origin" 36 | mesh_resource: "" 37 | position: [6.0, -7.0, 4.0] 38 | orientation: [0.0, 0.0, 0.0, 1.0] 39 | scale: [2.0, 2.0, 8.0] 40 | 41 | - type: "cube" 42 | name: "box6" 43 | frame_id: "local_origin" 44 | mesh_resource: "" 45 | position: [7.0, 13.5, 4.0] 46 | orientation: [0.0, 0.0, 0.0, 1.0] 47 | scale: [2.0, 4.0, 8.0] 48 | 49 | - type: "cube" 50 | name: "box7" 51 | frame_id: "local_origin" 52 | mesh_resource: "" 53 | position: [11.5, -7.0, 4.0] 54 | orientation: [0.0, 0.0, 0.0, 1.0] 55 | scale: [2.0, 2.0, 8.0] 56 | 57 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/boxes5.yaml: -------------------------------------------------------------------------------- 1 | - type: "cube" 2 | name: "box1" 3 | frame_id: "local_origin" 4 | mesh_resource: "" 5 | position: [20.0, 0.0, 1.25] 6 | orientation: [0.0, 0.0, 0.0, 1.0] 7 | scale: [10.0, 20.0, 2.5] 8 | 9 | - type: "cube" 10 | name: "box2" 11 | frame_id: "local_origin" 12 | mesh_resource: "" 13 | position: [27, 0.0, 2.0] 14 | orientation: [0.0, 0.0, 0.0, 1.0] 15 | scale: [4.0, 20.0, 4.0] 16 | 17 | - type: "cube" 18 | name: "box3" 19 | frame_id: "local_origin" 20 | mesh_resource: "" 21 | position: [33.0, 0.0, 0.5] 22 | orientation: [0.0, 0.0, 0.0, 1.0] 23 | scale: [8.0, 20.0, 1.0] 24 | 25 | - type: "cube" 26 | name: "box4" 27 | frame_id: "local_origin" 28 | mesh_resource: "" 29 | position: [48, 0.0, 0.5] 30 | orientation: [0.0, 0.0, 0.0, 1.0] 31 | scale: [8.0, 20.0, 1.0] 32 | 33 | - type: "cube" 34 | name: "box5" 35 | frame_id: "local_origin" 36 | mesh_resource: "" 37 | position: [18, 0.0, 4.5] 38 | orientation: [0.0, 0.0, 0.0, 1.0] 39 | scale: [2.0, 3.0, 4.0] 40 | 41 | 42 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/boxes6.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 18 | 1 19 | 20 | 21 | 22 | 23 | 0 0 1 24 | 100 100 25 | 26 | 27 | 28 | 29 | 30 | 100 31 | 50 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 10 43 | 44 | 45 | 0 46 | 47 | 48 | 0 0 1 49 | 100 100 50 | 51 | 52 | 53 | 57 | 58 | 59 | 0 60 | 0 61 | 62 | 63 | 64 | 0 0 -9.8 65 | 6e-06 2.3e-05 -4.2e-05 66 | 67 | 68 | 0.004 69 | 1 70 | 250 71 | 72 | 73 | 0.4 0.4 0.4 1 74 | 0.7 0.7 0.7 1 75 | 1 76 | 77 | 78 | EARTH_WGS84 79 | 0 80 | 0 81 | 0 82 | 0 83 | 84 | 85 | 86 | 87 | true 88 | 89 | 20 0 1.25 0 0.5 0 90 | 91 | 92 | 93 | 10 10 2.5 94 | 95 | 96 | 97 | 98 | 99 | 100 | 10 10 2.5 101 | 102 | 103 | 104 | 0.3 0.3 0.3 1 105 | 0.3 0.3 0.3 1 106 | 0.1 0.1 0.1 1 107 | 0 0 0 0 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/boxes6.yaml: -------------------------------------------------------------------------------- 1 | - type: "cube" 2 | name: "box1" 3 | frame_id: "local_origin" 4 | mesh_resource: "" 5 | position: [20.0, 0.0, 1.25] 6 | orientation: [0.0, 0.25, 0.0, 1.0] 7 | scale: [10.0, 10.0, 2.5] 8 | 9 | 10 | models: 11 | - box: 12 | name: "Box1" 13 | frame_id: "local_origin" 14 | position: 15 | x: 20 16 | y: 0 17 | z: 1.25 18 | orientation: 19 | x: 0.0 20 | y: 0.25 21 | z: 0.0 22 | w: 1.0 23 | scale: 24 | x: 10.0 25 | y: 10.0 26 | z: 2.5 27 | 28 | 29 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/moving_boxes.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 10 42 | 43 | 44 | 0 45 | 46 | 47 | 0 0 1 48 | 100 100 49 | 50 | 51 | 52 | 56 | 57 | 58 | 0 59 | 0 60 | 61 | 62 | 0 0 -9.8 63 | 6e-06 2.3e-05 -4.2e-05 64 | 65 | 66 | 0.004 67 | 1 68 | 250 69 | 70 | 71 | 0.4 0.4 0.4 1 72 | 0.7 0.7 0.7 1 73 | 1 74 | 75 | 76 | EARTH_WGS84 77 | 0 78 | 0 79 | 0 80 | 0 81 | 82 | 83 | 84 | 85 | true 86 | 87 | 1.5 0 2.5 0 0 0 88 | 89 | 90 | 91 | 1 20 5 92 | 93 | 94 | 95 | 96 | 97 | 98 | 1 20 5 99 | 100 | 101 | 102 | 0.3 0.3 0.3 1 103 | 0.3 0.3 0.3 1 104 | 0.1 0.1 0.1 1 105 | 0 0 0 0 106 | 107 | 108 | 109 | 110 | 111 | 5 | 6 | 7 | -15 0 4.57585 0 0.317796 0 8 | orbit 9 | 10 | 11 | 12 | 0.4 0.4 0.4 1 13 | 0.7 0.7 0.7 1 14 | 1 15 | 16 | 17 | 0 0 -9.8066 18 | 19 | 20 | quick 21 | 10 22 | 1.0 23 | 0 24 | 25 | 26 | 0 27 | 0.2 28 | 100 29 | 0.001 30 | 31 | 32 | 0.004 33 | 1 34 | 250 35 | 6e-06 2.3e-05 -4.2e-05 36 | 37 | 38 | 39 | 40 | 1 41 | 0 0 10 0 0 0 42 | 0.8 0.8 0.8 1 43 | 0.2 0.2 0.2 1 44 | 45 | 1000 46 | 0.9 47 | 0.01 48 | 0.001 49 | 50 | -0.5 0.1 -0.9 51 | 52 | 53 | 54 | model://vrc_driving_terrain 55 | 56 | 57 | 58 | 59 | pine_tree_1 60 | model://pine_tree 61 | 4 7 0 0 0 0 62 | 63 | 64 | pine_tree_2 65 | model://pine_tree 66 | 6.1 7.6 0 0 0 0 67 | 68 | 69 | pine_tree_3 70 | model://pine_tree 71 | 19.3 -14 0 0 0 0 72 | 73 | 74 | pine_tree_4 75 | model://pine_tree 76 | 16 -11.5 0 0 0 0 77 | 78 | 79 | 80 | oak_tree_1 81 | model://oak_tree 82 | 10 0 0 0 0 0 83 | 84 | 85 | oak_tree_2 86 | model://oak_tree 87 | 10.25 -4.25 0 0 0 0 88 | 89 | 90 | oak_tree_3 91 | model://oak_tree 92 | 20.2 14.4 0 0 0 0 93 | 94 | 95 | oak_tree_4 96 | model://oak_tree 97 | 30 8.5 0 0 0 0 98 | 99 | 100 | oak_tree_5 101 | model://oak_tree 102 | 136.2 -0.4 0 0 0 0 103 | 104 | 105 | oak_tree_6 106 | model://oak_tree 107 | 38 -9.8 0 0 0 0 108 | 109 | 110 | oak_tree_7 111 | model://oak_tree 112 | 38.6 -4.6 0 0 0 0 113 | 114 | 115 | 116 | oak_tree_8 117 | model://oak_tree 118 | 16 30 1.6 0.15 0 0 119 | 120 | 121 | 122 | 123 | 124 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/simple_obstacle.yaml: -------------------------------------------------------------------------------- 1 | 2 | - type: "mesh" 3 | name: "pinetree1" 4 | frame_id: "local_origin" 5 | mesh_resource: "model://pine_tree/meshes/pine_tree.dae" 6 | position: [4.0, 7.0, 0.0] 7 | orientation: [0.0, 0.0, 0.0, 1.0] 8 | scale: [1.0, 1.0, 1.0] 9 | 10 | - type: "mesh" 11 | name: "pinetree2" 12 | frame_id: "local_origin" 13 | mesh_resource: "model://pine_tree/meshes/pine_tree.dae" 14 | position: [6.1, 7.6, 0.0] 15 | orientation: [0.0, 0.0, 0.0, 1.0] 16 | scale: [1.0, 1.0, 1.0] 17 | 18 | - type: "mesh" 19 | name: "pinetree3" 20 | frame_id: "local_origin" 21 | mesh_resource: "model://pine_tree/meshes/pine_tree.dae" 22 | position: [19.3, -14.0, 0.0] 23 | orientation: [0.0, 0.0, 0.0, 1.0] 24 | scale: [1.0, 1.0, 1.0] 25 | 26 | - type: "mesh" 27 | name: "pinetree4" 28 | frame_id: "local_origin" 29 | mesh_resource: "model://pine_tree/meshes/pine_tree.dae" 30 | position: [16.0, -11.5, 0.0] 31 | orientation: [0.0, 0.0, 0.0, 1.0] 32 | scale: [1.0, 1.0, 1.0] 33 | 34 | - type: "mesh" 35 | name: "oaktree1" 36 | frame_id: "local_origin" 37 | mesh_resource: "model://oak_tree/meshes/oak_tree.dae" 38 | position: [10.0, 0.0, 0.0] 39 | orientation: [0.0, 0.0, 0.0, 1.0] 40 | scale: [1.0, 1.0, 1.0] 41 | 42 | - type: "mesh" 43 | name: "oaktree2" 44 | frame_id: "local_origin" 45 | mesh_resource: "model://oak_tree/meshes/oak_tree.dae" 46 | position: [10.25, -4.25, 0.0] 47 | orientation: [0.0, 0.0, 0.0, 1.0] 48 | scale: [1.0, 1.0, 1.0] 49 | 50 | - type: "mesh" 51 | name: "oaktree3" 52 | frame_id: "local_origin" 53 | mesh_resource: "model://oak_tree/meshes/oak_tree.dae" 54 | position: [20.2, 14.4, 0.0] 55 | orientation: [0.0, 0.0, 0.0, 1.0] 56 | scale: [1.0, 1.0, 1.0] 57 | 58 | - type: "mesh" 59 | name: "oaktree4" 60 | frame_id: "local_origin" 61 | mesh_resource: "model://oak_tree/meshes/oak_tree.dae" 62 | position: [30.0, 8.5, 0.0] 63 | orientation: [0.0, 0.0, 0.0, 1.0] 64 | scale: [1.0, 1.0, 1.0] 65 | 66 | - type: "mesh" 67 | name: "oaktree5" 68 | frame_id: "local_origin" 69 | mesh_resource: "model://oak_tree/meshes/oak_tree.dae" 70 | position: [136.2, -0.4, 0.0] 71 | orientation: [0.0, 0.0, 0.0, 1.0] 72 | scale: [1.0, 1.0, 1.0] 73 | 74 | - type: "mesh" 75 | name: "oaktree6" 76 | frame_id: "local_origin" 77 | mesh_resource: "model://oak_tree/meshes/oak_tree.dae" 78 | position: [38.0, -9.8, 0.0] 79 | orientation: [0.0, 0.0, 0.0, 1.0] 80 | scale: [1.0, 1.0, 1.0] 81 | 82 | - type: "mesh" 83 | name: "oaktree7" 84 | frame_id: "local_origin" 85 | mesh_resource: "model://oak_tree/meshes/oak_tree.dae" 86 | position: [38.6, -4.6, 0.0] 87 | orientation: [0.0, 0.0, 0.0, 1.0] 88 | scale: [1.0, 1.0, 1.0] 89 | 90 | - type: "mesh" 91 | name: "oaktree8" 92 | frame_id: "local_origin" 93 | mesh_resource: "model://oak_tree/meshes/oak_tree.dae" 94 | position: [16.0, 30.0, 1.6] 95 | orientation: [0.0, 0.0, 0.0, 1.0] 96 | scale: [1.0, 1.0, 1.0] 97 | 98 | 99 | -------------------------------------------------------------------------------- /avoidance/sim/worlds/window.yaml: -------------------------------------------------------------------------------- 1 | - type: "cube" 2 | name: "left" 3 | frame_id: "local_origin" 4 | mesh_resource: "file://media/materials/scripts/gazebo.material" 5 | position: [5, -0.15, 3.0] 6 | orientation: [0.0, 0.0, 0.0, 1.0] 7 | scale: [0.05, 0.05, 1.8] 8 | - type: "cube" 9 | name: "right" 10 | frame_id: "local_origin" 11 | mesh_resource: "file://media/materials/scripts/gazebo.material" 12 | position: [5.0, 1.05, 3.0] 13 | orientation: [0.0, 0.0, 0.0, 1.0] 14 | scale: [0.05, 0.05, 1.8] 15 | - type: "cube" 16 | name: "bottom" 17 | frame_id: "local_origin" 18 | mesh_resource: "file://media/materials/scripts/gazebo.material" 19 | position: [5.0, 0.45, 2.1] 20 | orientation: [0.7071, 0.7071, 0.0, 0.0] 21 | scale: [1.2, 0.05, 0.05] 22 | - type: "cube" 23 | name: "top" 24 | frame_id: "local_origin" 25 | mesh_resource: "file://media/materials/scripts/gazebo.material" 26 | position: [5.0, 0.45, 3.9] 27 | orientation: [0.7071, 0.7071, 0.0, 0.0] 28 | scale: [1.2, 0.05, 0.05] 29 | - type: "cube" 30 | name: "wall_right" 31 | frame_id: "local_origin" 32 | mesh_resource: "file://media/materials/scripts/gazebo.material" 33 | position: [5.0, -2.68, 4.5] 34 | orientation: [0.0, 0.0, 0.0, 1.0] 35 | scale: [0.05, 5.0, 9.0] 36 | - type: "cube" 37 | name: "wall_left" 38 | frame_id: "local_origin" 39 | mesh_resource: "file://media/materials/scripts/gazebo.material" 40 | position: [5.0, 3.57, 4.5] 41 | orientation: [0.0, 0.0, 0.0, 1.0] 42 | scale: [0.05, 5.0, 9.0] 43 | - type: "cube" 44 | name: "wall_top" 45 | frame_id: "local_origin" 46 | mesh_resource: "file://media/materials/scripts/gazebo.material" 47 | position: [5.0, 0.45, 6.43] 48 | orientation: [0.0, 0.0, 0.0, 1.0] 49 | scale: [0.05, 1.3, 5.0] 50 | - type: "cube" 51 | name: "wall_bottom" 52 | frame_id: "local_origin" 53 | mesh_resource: "file://media/materials/scripts/gazebo.material" 54 | position: [5.0, 0.45, 1.0] 55 | orientation: [0.0, 0.0, 0.0, 1.0] 56 | scale: [0.05, 1.3, 2.2] 57 | -------------------------------------------------------------------------------- /avoidance/src/histogram.cpp: -------------------------------------------------------------------------------- 1 | #include "avoidance/histogram.h" 2 | #include 3 | 4 | namespace avoidance { 5 | Histogram::Histogram(const int res) 6 | : resolution_{res}, z_dim_{360 / resolution_}, e_dim_{180 / resolution_}, dist_(e_dim_, z_dim_) { 7 | setZero(); 8 | } 9 | 10 | void Histogram::upsample() { 11 | if (resolution_ != ALPHA_RES * 2) { 12 | throw std::logic_error( 13 | "Invalid use of function upsample(). This function can only be used on a half resolution histogram."); 14 | } 15 | resolution_ = resolution_ / 2; 16 | z_dim_ = 2 * z_dim_; 17 | e_dim_ = 2 * e_dim_; 18 | Eigen::MatrixXf temp_dist(e_dim_, z_dim_); 19 | 20 | for (int i = 0; i < e_dim_; ++i) { 21 | for (int j = 0; j < z_dim_; ++j) { 22 | int i_lowres = floor(i / 2); 23 | int j_lowres = floor(j / 2); 24 | temp_dist(i, j) = dist_(i_lowres, j_lowres); 25 | } 26 | } 27 | dist_ = temp_dist; 28 | } 29 | 30 | void Histogram::downsample() { 31 | if (resolution_ != ALPHA_RES) { 32 | throw std::logic_error( 33 | "Invalid use of function downsample(). This function can only be used on a full resolution histogram."); 34 | } 35 | resolution_ = 2 * resolution_; 36 | z_dim_ = z_dim_ / 2; 37 | e_dim_ = e_dim_ / 2; 38 | Eigen::MatrixXf temp_dist(e_dim_, z_dim_); 39 | 40 | for (int i = 0; i < e_dim_; ++i) { 41 | for (int j = 0; j < z_dim_; ++j) { 42 | int i_high_res = 2 * i; 43 | int j_high_res = 2 * j; 44 | temp_dist(i, j) = dist_.block(i_high_res, j_high_res, 2, 2).mean(); 45 | } 46 | } 47 | dist_ = temp_dist; 48 | } 49 | 50 | void Histogram::setZero() { dist_.fill(0.f); } 51 | 52 | bool Histogram::isEmpty() const { 53 | int counter = 0; 54 | for (int e = 0; (e < e_dim_) && (0 == counter); e++) { 55 | for (int z = 0; (z < z_dim_) && (0 == counter); z++) { 56 | if (dist_(e, z) > FLT_MIN) { 57 | counter++; 58 | } 59 | } 60 | } 61 | return counter == 0; 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /avoidance/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char **argv) { 6 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error); 7 | ::testing::InitGoogleTest(&argc, argv); 8 | ros::init(argc, argv, "test"); 9 | return RUN_ALL_TESTS(); 10 | } 11 | -------------------------------------------------------------------------------- /avoidance/test/test_usm.cpp.dot: -------------------------------------------------------------------------------- 1 | digraph { 2 | "START" -> "PATH_A_1" [label="NEXT1", style="solid", weight=1] 3 | "START" -> "PATH_B_1" [label="NEXT2", style="solid", weight=1] 4 | "START" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1] 5 | "PATH_A_1" -> "PATH_A_2" [label="NEXT1", style="solid", weight=1] 6 | "PATH_A_1" -> "PATH_B_3" [label="ERROR", style="solid", weight=1] 7 | "PATH_A_2" -> "END" [label="NEXT1", style="solid", weight=1] 8 | "PATH_A_2" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1] 9 | "PATH_B_1" -> "PATH_B_2" [label="NEXT1", style="solid", weight=1] 10 | "PATH_B_1" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1] 11 | "PATH_B_2" -> "PATH_B_3" [label="NEXT1", style="solid", weight=1] 12 | "PATH_B_2" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1] 13 | "PATH_B_3" -> "END" [label="NEXT1", style="solid", weight=1] 14 | "PATH_B_3" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1] 15 | "CLEANUP" -> "END" [label="NEXT1", style="solid", weight=1] 16 | "CLEANUP" -> "CLEANUP" [label="ERROR", style="dotted", weight=0.1] 17 | } -------------------------------------------------------------------------------- /docs/lp_goal_height.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaeyoung-Lim/avoidance/fd72f14aa5a707027c02d57a3d9aec445bf37a58/docs/lp_goal_height.png -------------------------------------------------------------------------------- /docs/lp_goal_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaeyoung-Lim/avoidance/fd72f14aa5a707027c02d57a3d9aec445bf37a58/docs/lp_goal_rviz.png -------------------------------------------------------------------------------- /docs/simulation_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaeyoung-Lim/avoidance/fd72f14aa5a707027c02d57a3d9aec445bf37a58/docs/simulation_screenshot.png -------------------------------------------------------------------------------- /global_planner/cfg/GlobalPlannerNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "global_planner" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # global_planner 9 | gen.add("min_altitude_", int_t, 0, "Minimum planned altitude", 1, 0, 10) 10 | gen.add("max_altitude_", int_t, 0, "Maximum planned altitude", 10, 0, 50) 11 | gen.add("max_cell_risk_", double_t, 0, "Maximum risk allowed per cells", 0.2, 0.0, 1.0) 12 | gen.add("smooth_factor_", double_t, 0, "Cost of turning", 20.0, 0.0, 100.0) 13 | gen.add("vert_to_hor_cost_", double_t, 0, "Cost of changing between horizontal and vertical direction", 3.0, 0.0, 10.0) 14 | gen.add("risk_factor_", double_t, 0, "Cost of crashing", 500.0, 0.0, 1000.0) 15 | gen.add("neighbor_risk_flow_", double_t, 0, "The effect of the risk of neighboring cells", 1.0, 0.0, 1.0) 16 | gen.add("expore_penalty_", double_t, 0, "The cost of unexplored space", 0.005, 0.0, 0.01) 17 | gen.add("up_cost_", double_t, 0, "Cost of ascending 1m", 5.0, 0.0, 10.0) 18 | gen.add("down_cost_", double_t, 0, "Cost of descending 1m", 1.0, 0.0, 10.0) 19 | gen.add("search_time_", double_t, 0, "Time it takes to return a new path", 0.5, 0.0, 1.0) 20 | gen.add("min_overestimate_factor_", double_t, 0, "The minimum overestimation for heuristics", 1.03, 1.0, 1.5) 21 | gen.add("max_overestimate_factor_", double_t, 0, "The minimum overestimation for heuristics", 2.0, 1.0, 5.0) 22 | gen.add("max_iterations_", int_t, 0, "Maximum number of iterations", 2000, 0, 10000) 23 | gen.add("goal_must_be_free_", bool_t, 0, "Don't bother trying to find a path if the exact goal is occupied", True) 24 | gen.add("use_current_yaw_", bool_t, 0, "The current yaw affects the pathfinding", True) 25 | gen.add("use_risk_heuristics_", bool_t, 0, "Use non underestimating heuristics for risk", True) 26 | gen.add("use_speedup_heuristics_", bool_t, 0, "Use non underestimating heuristics for speedup", True) 27 | 28 | # global_planner_node 29 | gen.add("clicked_goal_alt_", double_t, 0, "The altitude of clicked goals", 3.5, 0.0, 10.0) 30 | gen.add("clicked_goal_radius_", double_t, 0, "Minimum allowed distance from path end to goal", 1.0, 0.0, 10.0) 31 | gen.add("simplify_iterations_", int_t, 0, "Maximum number of iterations to simplify a path", 1, 0, 100) 32 | gen.add("simplify_margin_", double_t, 0, "The allowed cost increase for simplifying an edge", 1.01, 0.0, 2.0) 33 | 34 | # cell 35 | gen.add("CELL_SCALE", double_t, 2, "Size of a cell, should be divisable by the OctoMap resolution", 1.0, 0.5, 2.0) 36 | 37 | # node 38 | gen.add("SPEEDNODE_RADIUS", double_t, 4, "Maximum length of edge between two Cells", 5.0, 0.0, 10.0) 39 | 40 | node_type_enum = gen.enum([ gen.const("Node", str_t, "Node", "Normal node"), 41 | gen.const("NodeWithoutSmooth", str_t, "NodeWithoutSmooth", "No smooth cost"), 42 | gen.const("SpeedNode", str_t, "SpeedNode", "Search with speed")], 43 | "Change search mode") 44 | 45 | gen.add("default_node_type_", str_t, 4, "Change search mode", "SpeedNode", edit_method=node_type_enum) 46 | 47 | exit(gen.generate(PACKAGE, "global_planner", "GlobalPlannerNode")) 48 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/cell.h: -------------------------------------------------------------------------------- 1 | #ifndef GLOBAL_PLANNER_CELL 2 | #define GLOBAL_PLANNER_CELL 3 | 4 | #include // abs 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "global_planner/common.h" 11 | 12 | namespace global_planner { 13 | 14 | static double CELL_SCALE = 1.0; 15 | 16 | class Cell { 17 | public: 18 | Cell(); 19 | Cell(std::tuple new_tuple); 20 | Cell(double x, double y, double z); 21 | Cell(double x, double y); 22 | Cell(geometry_msgs::Point point); 23 | // Cell(Eigen::Vector3d point); 24 | 25 | // Get the indices of the Cell 26 | int xIndex() const; 27 | int yIndex() const; 28 | int zIndex() const; 29 | 30 | // Get the coordinates of the center-point of the Cell 31 | double xPos() const; 32 | double yPos() const; 33 | double zPos() const; 34 | 35 | geometry_msgs::Point toPoint() const; 36 | 37 | double manhattanDist(double _x, double _y, double _z) const; 38 | double distance2D(const Cell& b) const; 39 | double distance3D(const Cell& b) const; 40 | double diagDistance2D(const Cell& b) const; 41 | double diagDistance3D(const Cell& b) const; 42 | double angle() const; 43 | 44 | Cell getNeighborFromYaw(double yaw) const; 45 | std::vector getFlowNeighbors(int radius) const; 46 | std::vector getDiagonalNeighbors() const; 47 | std::vector getNeighbors() const; 48 | 49 | std::string asString() const; 50 | 51 | // Member variables 52 | std::tuple tpl_; 53 | }; 54 | 55 | inline bool operator==(const Cell& lhs, const Cell& rhs) { return lhs.tpl_ == rhs.tpl_; } 56 | inline bool operator!=(const Cell& lhs, const Cell& rhs) { return !operator==(lhs, rhs); } 57 | inline bool operator<(const Cell& lhs, const Cell& rhs) { return lhs.tpl_ < rhs.tpl_; } 58 | inline bool operator>(const Cell& lhs, const Cell& rhs) { return operator<(rhs, lhs); } 59 | inline bool operator<=(const Cell& lhs, const Cell& rhs) { return !operator>(lhs, rhs); } 60 | inline bool operator>=(const Cell& lhs, const Cell& rhs) { return !operator<(lhs, rhs); } 61 | 62 | inline Cell operator+(const Cell& lhs, const Cell& rhs) { 63 | Cell res( 64 | std::tuple(lhs.xIndex() + rhs.xIndex(), lhs.yIndex() + rhs.yIndex(), lhs.zIndex() + rhs.zIndex())); 65 | return res; 66 | } 67 | inline Cell operator-(const Cell& lhs, const Cell& rhs) { 68 | Cell res( 69 | std::tuple(lhs.xIndex() - rhs.xIndex(), lhs.yIndex() - rhs.yIndex(), lhs.zIndex() - rhs.zIndex())); 70 | return res; 71 | } 72 | 73 | typedef std::pair CellDistancePair; 74 | 75 | // A GoalCell has a radius and can check if a position or another Cell is inside 76 | // its radius 77 | class GoalCell : public Cell { 78 | public: 79 | GoalCell(Cell cell, double radius = 1.0, bool is_temporary = false) 80 | : Cell(cell), radius_(radius), is_temporary_(is_temporary) {} 81 | 82 | GoalCell(double x, double y, double z, double radius = 1.0, bool is_temporary = false) 83 | : Cell(x, y, z), radius_(radius), is_temporary_(is_temporary) {} 84 | 85 | bool withinPlanRadius(Cell cell) const { 86 | return manhattanDist(cell.xPos(), cell.yPos(), cell.zPos()) < radius_ / 2.0; 87 | } 88 | 89 | template 90 | bool withinPositionRadius(P point) const { 91 | return manhattanDist(point.x, point.y, point.z) < radius_; 92 | } 93 | 94 | double radius_; 95 | bool is_temporary_; 96 | }; 97 | 98 | } // namespace global_planner 99 | 100 | namespace std { 101 | 102 | template <> 103 | struct hash { 104 | std::size_t operator()(const global_planner::Cell& cell) const { 105 | return (std::get<0>(cell.tpl_) << 20) ^ (std::get<1>(cell.tpl_) << 10) ^ std::get<2>(cell.tpl_); 106 | // return (std::get<0>(cell.tpl_) * 18397) + (std::get<1>(cell.tpl_) * 107 | // 20483) + (std::get<2>(cell.tpl_) * 29303); 108 | } 109 | }; 110 | 111 | } // namespace std 112 | 113 | #endif // GLOBAL_PLANNER_CELL 114 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/common.h: -------------------------------------------------------------------------------- 1 | #ifndef GLOBAL_PLANNER_COMMON_H_ 2 | #define GLOBAL_PLANNER_COMMON_H_ 3 | 4 | #include // sqrt 5 | #include 6 | 7 | namespace global_planner { 8 | 9 | // GLOBAL PLANNER 10 | 11 | template 12 | double squared(T x) { 13 | return x * x; 14 | } 15 | 16 | // Returns a weighted average of start and end, where ratio is the weight of 17 | // start 18 | inline double interpolate(double start, double end, double ratio) { return start + (end - start) * ratio; } 19 | 20 | // Returns Map[key] if it exists, default_val otherwise 21 | template 22 | Value getWithDefault(Map& m, const Key& key, const Value& default_val) { 23 | if (m.find(key) != m.end()) { 24 | return m[key]; 25 | } 26 | return default_val; 27 | } 28 | 29 | template 30 | void setPointCoordinates(P& point, double x, double y, double z) { 31 | point.x = x; 32 | point.y = y; 33 | point.z = z; 34 | } 35 | 36 | template 37 | T norm(T x, T y, T z) { 38 | return sqrt(squared(x) + squared(y) + squared(z)); 39 | } 40 | 41 | template 42 | double norm(const P& p) { 43 | return norm(p.x, p.y, p.z); 44 | } 45 | 46 | // Returns a point between p1 and p2, ratio should be between 0 and 1 47 | // ratio=0 -> p1, ratio=1 -> p2 48 | template 49 | P interpolate(const P& p1, const P& p2, double ratio) { 50 | P new_point; 51 | new_point.x = interpolate(p1.x, p2.x, ratio); 52 | new_point.y = interpolate(p1.y, p2.y, ratio); 53 | new_point.z = interpolate(p1.z, p2.z, ratio); 54 | return new_point; 55 | } 56 | 57 | // Returns the point in the middle of the line segment between p1 and p2 58 | template 59 | P middlePoint(const P& p1, const P& p2) { 60 | return interpolate(p1, p2, 0.5); 61 | } 62 | 63 | template 64 | P1 addPoints(const P1& p1, const P2& p2) { 65 | P1 new_p; 66 | new_p.x = p1.x + p2.x; 67 | new_p.y = p1.y + p2.y; 68 | new_p.z = p1.z + p2.z; 69 | return new_p; 70 | } 71 | 72 | template 73 | P1 subtractPoints(const P1& p1, const P2& p2) { 74 | P1 new_p; 75 | new_p.x = p1.x - p2.x; 76 | new_p.y = p1.y - p2.y; 77 | new_p.z = p1.z - p2.z; 78 | return new_p; 79 | } 80 | 81 | template 82 | P scalePoint(const P& point, Float scalar) { 83 | P new_p; 84 | new_p.x = scalar * point.x; 85 | new_p.y = scalar * point.y; 86 | new_p.z = scalar * point.z; 87 | return new_p; 88 | } 89 | 90 | template 91 | double distance(const P& p1, const P& p2) { 92 | return norm((p2.x - p1.x), (p2.y - p1.y), (p2.z - p1.z)); 93 | } 94 | 95 | inline double clocksToMicroSec(std::clock_t start, std::clock_t end) { 96 | return (end - start) / (double)(CLOCKS_PER_SEC / 1000000); 97 | } 98 | 99 | // returns angle in the range [-pi, pi] 100 | inline double angleToRange(double angle) { 101 | angle += M_PI; 102 | angle -= (2 * M_PI) * std::floor(angle / (2 * M_PI)); 103 | angle -= M_PI; 104 | return angle; 105 | } 106 | 107 | inline double posterior(double p, double prior) { 108 | // p and prior are independent measurements of the same event 109 | double prob_obstacle = p * prior; 110 | double prob_free = (1 - p) * (1 - prior); 111 | return prob_obstacle / (prob_obstacle + prob_free + 0.0001); 112 | } 113 | 114 | } // namespace global_planner 115 | 116 | #endif /* GLOBAL_PLANNER_COMMON_H_ */ 117 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/node.h: -------------------------------------------------------------------------------- 1 | #ifndef GLOBAL_PLANNER_NODE 2 | #define GLOBAL_PLANNER_NODE 3 | 4 | #include 5 | #include 6 | 7 | #include "global_planner/cell.h" 8 | #include "global_planner/common.h" 9 | 10 | namespace global_planner { 11 | 12 | class Node { 13 | public: 14 | Node() = default; 15 | Node(const Cell& cell, const Cell& parent) : cell_(cell), parent_(parent) {} 16 | virtual ~Node() = default; 17 | 18 | virtual bool isEqual(const Node& other) const; 19 | virtual bool isSmaller(const Node& other) const; 20 | virtual std::size_t hash() const; 21 | virtual std::shared_ptr nextNode(const Cell& nextCell) const; 22 | virtual std::vector > getNeighbors() const; 23 | virtual std::unordered_set getCells() const; 24 | 25 | virtual double getLength() const; 26 | virtual double getRotation(const Node& other) const; 27 | virtual double getXYRotation(const Node& other) const; 28 | std::string asString() const; 29 | 30 | Cell cell_; 31 | Cell parent_; 32 | }; 33 | 34 | inline bool operator==(const Node& lhs, const Node& rhs) { return lhs.isEqual(rhs); } 35 | inline bool operator<(const Node& lhs, const Node& rhs) { return lhs.isSmaller(rhs); } 36 | inline bool operator!=(const Node& lhs, const Node& rhs) { return !operator==(lhs, rhs); } 37 | inline bool operator>(const Node& lhs, const Node& rhs) { return operator<(rhs, lhs); } 38 | inline bool operator<=(const Node& lhs, const Node& rhs) { return !operator>(lhs, rhs); } 39 | inline bool operator>=(const Node& lhs, const Node& rhs) { return !operator<(lhs, rhs); } 40 | 41 | typedef std::shared_ptr NodePtr; 42 | typedef std::pair NodeDistancePair; 43 | typedef std::pair PointerNodeDistancePair; 44 | 45 | class CompareDist { 46 | public: 47 | bool operator()(const CellDistancePair& n1, const CellDistancePair& n2) { return n1.second > n2.second; } 48 | bool operator()(const NodeDistancePair& n1, const NodeDistancePair& n2) { return n1.second > n2.second; } 49 | bool operator()(const PointerNodeDistancePair& n1, const PointerNodeDistancePair& n2) { 50 | return n1.second > n2.second; 51 | } 52 | }; 53 | 54 | // Node that only represents 3D position, ignores parent 55 | class NodeWithoutSmooth : public Node { 56 | public: 57 | NodeWithoutSmooth() = default; 58 | NodeWithoutSmooth(const Cell& cell, const Cell& parent) : Node(cell, parent) {} 59 | ~NodeWithoutSmooth() = default; 60 | 61 | bool isEqual(const Node& other) const { return cell_ == other.cell_; } 62 | 63 | std::size_t hash() const { return std::hash()(cell_); } 64 | 65 | NodePtr nextNode(const Cell& nextCell) const { return NodePtr(new NodeWithoutSmooth(nextCell, cell_)); } 66 | 67 | double getRotation(const Node& other) const { return 0.0; } 68 | }; 69 | 70 | static double SPEEDNODE_RADIUS = 5.0; 71 | // Node represents 3D position, orientation and speed 72 | // TODO: Needs to check the risk of Cells between cell and parent 73 | class SpeedNode : public Node { 74 | public: 75 | SpeedNode() = default; 76 | SpeedNode(const Cell& cell, const Cell& parent) : Node(cell, parent) {} 77 | ~SpeedNode() = default; 78 | 79 | NodePtr nextNode(const Cell& nextCell) const { return NodePtr(new SpeedNode(nextCell, cell_)); } 80 | 81 | std::vector getNeighbors() const { 82 | std::vector neighbors; 83 | Cell extrapolate_cell = (cell_ - parent_) + cell_; 84 | neighbors.push_back(nextNode(extrapolate_cell)); 85 | for (const Cell& neighborCell : extrapolate_cell.getNeighbors()) { 86 | double dist = cell_.distance3D(neighborCell); 87 | if (dist > 0 && dist < SPEEDNODE_RADIUS) { 88 | neighbors.push_back(nextNode(neighborCell)); 89 | } 90 | } 91 | return neighbors; 92 | } 93 | }; 94 | 95 | struct HashNodePtr { 96 | std::size_t operator()(const std::shared_ptr& node_ptr) const { return node_ptr->hash(); } 97 | }; 98 | 99 | struct EqualsNodePtr { 100 | std::size_t operator()(const std::shared_ptr& node_ptr1, const std::shared_ptr& node_ptr2) const { 101 | return node_ptr1->isEqual(*node_ptr2); 102 | } 103 | }; 104 | 105 | } // namespace global_planner 106 | 107 | namespace std { 108 | 109 | template <> 110 | struct hash { 111 | std::size_t operator()(const global_planner::Node& node) const { return node.hash(); } 112 | }; 113 | 114 | template <> 115 | struct hash { 116 | std::size_t operator()(const global_planner::NodeWithoutSmooth& node) const { return node.hash(); } 117 | }; 118 | 119 | } // namespace std 120 | 121 | #endif // GLOBAL_PLANNER_NODE 122 | -------------------------------------------------------------------------------- /global_planner/include/global_planner/visitor.h: -------------------------------------------------------------------------------- 1 | #ifndef GLOBAL_PLANNER_VISITOR 2 | #define GLOBAL_PLANNER_VISITOR 3 | 4 | namespace global_planner { 5 | 6 | template 7 | class SearchVisitor { 8 | public: 9 | Set seen_; 10 | Map seen_count_; 11 | 12 | SearchVisitor() {} 13 | void initVisitor(Set seen, Map seen_count) {} 14 | void init() { 15 | seen_.clear(); 16 | seen_count_.clear(); 17 | } 18 | void popNode(NodePtr u) {} 19 | void perNeighbor(NodePtr u, NodePtr v) { 20 | seen_count_[v->cell_] = 1.0 + getWithDefault(seen_count_, v->cell_, 0.0); 21 | seen_.insert(v->cell_); 22 | } 23 | }; 24 | 25 | class NullVisitor { 26 | public: 27 | NullVisitor() {} 28 | 29 | void init() {} 30 | 31 | template 32 | void popNode(NodePtr u) {} 33 | 34 | template 35 | void perNeighbor(NodePtr u, NodePtr v) {} 36 | }; 37 | 38 | } // namespace global_planner 39 | 40 | #endif // GLOBAL_PLANNER_VISITOR 41 | -------------------------------------------------------------------------------- /global_planner/launch/global_planner_depth-camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | > 30 | 31 | 33 | 34 | -------------------------------------------------------------------------------- /global_planner/launch/global_planner_octomap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | $(arg pointcloud_topics) 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /global_planner/launch/global_planner_sitl_3cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 15 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | -------------------------------------------------------------------------------- /global_planner/launch/global_planner_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | -------------------------------------------------------------------------------- /global_planner/msg/PathWithRiskMsg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/PoseStamped[] poses 3 | float64[] risks -------------------------------------------------------------------------------- /global_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | global_planner 4 | 0.0.0 5 | Avoidance module doing global planning. 6 | 7 | 8 | 9 | 10 | pixhawk 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | dynamic_reconfigure 45 | message_generation 46 | roscpp 47 | rospy 48 | std_msgs 49 | geometry_msgs 50 | sensor_msgs 51 | octomap 52 | octomap_msgs 53 | tf 54 | pcl_ros 55 | mavros 56 | mavros_extras 57 | avoidance 58 | 59 | 60 | dynamic_reconfigure 61 | message_runtime 62 | roscpp 63 | rospy 64 | std_msgs 65 | geometry_msgs 66 | sensor_msgs 67 | octomap 68 | octomap_msgs 69 | tf 70 | pcl_ros 71 | mavros 72 | mavros_extras 73 | avoidance 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /global_planner/resource/parameter_file: -------------------------------------------------------------------------------- 1 | smooth_factor_ 10.0 2 | risk_factor_ 500.0 3 | up_cost_ 300.0 4 | explore_penalty 0.005 5 | max_cell_risk_ 0.2 6 | neighbor_risk_flow_ 1.0 7 | goal_alt_inc 0.0 8 | -------------------------------------------------------------------------------- /global_planner/resource/px4_config.yaml: -------------------------------------------------------------------------------- 1 | # Common configuration for PX4 autopilot 2 | # 3 | # node: 4 | startup_px4_usb_quirk: true 5 | 6 | # --- system plugins --- 7 | 8 | # sys_status & sys_time connection options 9 | conn: 10 | heartbeat_rate: 1.0 # send hertbeat rate in Hertz 11 | timeout: 10.0 # hertbeat timeout in seconds 12 | timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) 13 | system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) 14 | 15 | # sys_status 16 | sys: 17 | min_voltage: 10.0 # diagnostics min voltage 18 | disable_diag: false # disable all sys_status diagnostics, except heartbeat 19 | 20 | # sys_time 21 | time: 22 | time_ref_source: "fcu" # time_reference source 23 | timesync_avg_alpha: 0.6 # timesync averaging factor 24 | 25 | # --- mavros plugins (alphabetical order) --- 26 | 27 | # 3dr_radio 28 | tdr_radio: 29 | low_rssi: 40 # raw rssi lower level for diagnostics 30 | 31 | # actuator_control 32 | # None 33 | 34 | # command 35 | cmd: 36 | use_comp_id_system_control: false # quirk for some old FCUs 37 | 38 | # dummy 39 | # None 40 | 41 | # ftp 42 | # None 43 | 44 | # global_position 45 | global_position: 46 | frame_id: "fcu" # pose and fix frame_id 47 | rot_covariance: 99999.0 # covariance for attitude? 48 | tf: 49 | send: false # send TF? 50 | frame_id: "local_origin" # TF frame_id 51 | child_frame_id: "fcu_utm" # TF child_frame_id 52 | 53 | # imu_pub 54 | imu: 55 | frame_id: "fcu" 56 | # need find actual values 57 | linear_acceleration_stdev: 0.0003 58 | angular_velocity_stdev: !degrees 0.02 59 | orientation_stdev: 1.0 60 | magnetic_stdev: 0.0 61 | 62 | # local_position 63 | local_position: 64 | frame_id: "local_origin" 65 | tf: 66 | send: true 67 | frame_id: "local_origin" 68 | child_frame_id: "fcu" 69 | send_fcu: false 70 | 71 | # param 72 | # None, used for FCU params 73 | 74 | # rc_io 75 | # None 76 | 77 | # safety_area 78 | safety_area: 79 | p1: {x: 1.0, y: 1.0, z: 1.0} 80 | p2: {x: -1.0, y: -1.0, z: -1.0} 81 | 82 | # setpoint_accel 83 | setpoint_accel: 84 | send_force: false 85 | 86 | # setpoint_attitude 87 | setpoint_attitude: 88 | reverse_throttle: false # allow reversed throttle 89 | tf: 90 | listen: false # enable tf listener (disable topic subscribers) 91 | frame_id: "local_origin" 92 | child_frame_id: "attitude" 93 | rate_limit: 10.0 94 | 95 | # setpoint_position 96 | setpoint_position: 97 | tf: 98 | listen: false # enable tf listener (disable topic subscribers) 99 | frame_id: "local_origin" 100 | child_frame_id: "setpoint" 101 | rate_limit: 50.0 102 | 103 | # setpoint_velocity 104 | # None 105 | 106 | # vfr_hud 107 | # None 108 | 109 | # waypoint 110 | mission: 111 | pull_after_gcs: true # update mission if gcs updates 112 | 113 | # --- mavros extras plugins (same order) --- 114 | 115 | # distance_sensor (PX4 only) 116 | distance_sensor: 117 | hrlv_ez4_pub: 118 | id: 0 119 | frame_id: "hrlv_ez4_sonar" 120 | orientation: ROLL_180 # RPY:{180.0, 0.0, 0.0} 121 | field_of_view: 0.0 # XXX TODO 122 | send_tf: true 123 | sensor_position: {x: 0.0, y: 0.0, z: -0.1} 124 | lidarlite_pub: 125 | id: 1 126 | frame_id: "lidarlite_laser" 127 | orientation: ROLL_180 128 | field_of_view: 0.0 # XXX TODO 129 | send_tf: true 130 | sensor_position: {x: 0.0, y: 0.0, z: -0.1} 131 | sonar_1_sub: 132 | subscriber: true 133 | id: 2 134 | orientation: ROLL_180 135 | laser_1_sub: 136 | subscriber: true 137 | id: 3 138 | orientation: ROLL_180 139 | 140 | ## Currently available orientations: 141 | # Check http://wiki.ros.org/mavros/Enumerations 142 | ## 143 | 144 | # image_pub 145 | image: 146 | frame_id: "px4flow" 147 | 148 | # mocap_pose_estimate 149 | mocap: 150 | # select mocap source 151 | use_tf: true # ~mocap/tf 152 | use_pose: false # ~mocap/pose 153 | 154 | # px4flow 155 | px4flow: 156 | frame_id: "px4flow" 157 | ranger_fov: !degrees 0.0 # XXX TODO 158 | ranger_min_range: 0.3 # meters 159 | ranger_max_range: 5.0 # meters 160 | 161 | # vision_pose_estimate 162 | vision_pose: 163 | tf: 164 | listen: false # enable tf listener (disable topic subscribers) 165 | frame_id: "local_origin" 166 | child_frame_id: "vision" 167 | rate_limit: 10.0 168 | 169 | # vision_speed_estimate 170 | vision_speed: 171 | listen_twist: false 172 | 173 | # vibration 174 | vibration: 175 | frame_id: "vibration" 176 | 177 | # vim:set ts=2 sw=2 et: 178 | -------------------------------------------------------------------------------- /global_planner/resource/random_goals: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaeyoung-Lim/avoidance/fd72f14aa5a707027c02d57a3d9aec445bf37a58/global_planner/resource/random_goals -------------------------------------------------------------------------------- /global_planner/resource/sample_output_from_mock_data: -------------------------------------------------------------------------------- 1 | [ INFO] [1467203468.623922075]: No goal file given. 2 | [ INFO] [1467203478.608638174]: ========== Set goal : (8,4,3) ========== 3 | [ INFO] [1467203478.608707527]: Start planning path. 4 | [ INFO] [1467203478.608757630]: OctoMap memory usage: 0.010 MB 5 | [ INFO] [1467203478.608901955]: Planning a path from (0,2,1) to (8,4,3) 6 | [ INFO] [1467203478.608933362]: currPos: 0.50,2.50,1.50 s: 0.50,2.50,1.50 7 | Average iteration time: 0.162512 ms 8 | overEstimateFactor: 2.00, numIter: 43 path cost: 126.23 (cost: 126.23, dist: 30.83, risk: 75.22, smooth: 20.00) 9 | Average iteration time: 0.132180 ms 10 | overEstimateFactor: 1.25, numIter: 50 path cost: 126.23 (cost: 126.23, dist: 30.83, risk: 75.22, smooth: 20.00) 11 | Average iteration time: 0.145964 ms 12 | overEstimateFactor: 1.06, numIter: 55 path cost: 126.23 (cost: 126.23, dist: 30.83, risk: 75.22, smooth: 20.00) 13 | (0.50, 2.50, 2.50) -> (0.50, 2.50, 3.50) -> (0.50, 2.50, 4.50) -> (0.50, 2.50, 5.50) -> (0.50, 2.50, 6.50) -> (0.50, 2.50, 7.50) -> (1.50, 3.50, 7.50) -> (2.50, 4.50, 7.50) -> (3.50, 4.50, 7.50) -> (4.50, 4.50, 7.50) -> (5.50, 4.50, 7.50) -> (6.50, 4.50, 7.50) -> (7.50, 4.50, 7.50) -> (8.50, 4.50, 7.50) -> (8.50, 4.50, 6.50) -> (8.50, 4.50, 5.50) -> (8.50, 4.50, 4.50) -> (8.50, 4.50, 3.50) -> 14 | -------------------------------------------------------------------------------- /global_planner/src/library/node.cpp: -------------------------------------------------------------------------------- 1 | #include "global_planner/node.h" 2 | 3 | namespace global_planner { 4 | 5 | bool Node::isSmaller(const Node& other) const { 6 | return cell_ < other.cell_ || (cell_ == other.cell_ && parent_ < other.parent_); 7 | } 8 | bool Node::isEqual(const Node& other) const { return cell_ == other.cell_ && parent_ == other.parent_; } 9 | 10 | std::size_t Node::hash() const { 11 | return (std::hash()(cell_) << 1) ^ std::hash()(parent_); 12 | } 13 | 14 | NodePtr Node::nextNode(const Cell& nextCell) const { return NodePtr(new Node(nextCell, cell_)); } 15 | 16 | std::vector Node::getNeighbors() const { 17 | std::vector neighbors; 18 | neighbors.reserve(10); 19 | for (const Cell& neighborCell : cell_.getNeighbors()) { 20 | neighbors.push_back(nextNode(neighborCell)); 21 | } 22 | return neighbors; 23 | } 24 | 25 | std::unordered_set Node::getCells() const { 26 | std::unordered_set cells; 27 | int dx = cell_.xIndex() - parent_.xIndex(); 28 | int dy = cell_.yIndex() - parent_.yIndex(); 29 | int dz = cell_.zIndex() - parent_.zIndex(); 30 | 31 | int steps = 2 * std::max(std::abs(dx), std::max(std::abs(dy), std::abs(dz))); 32 | 33 | double x_step = (cell_.xPos() - parent_.xPos()) / steps; 34 | double y_step = (cell_.yPos() - parent_.yPos()) / steps; 35 | double z_step = (cell_.zPos() - parent_.zPos()) / steps; 36 | 37 | for (int i = 1; i <= steps; ++i) { 38 | double new_x = parent_.xPos() + x_step * i; 39 | double new_y = parent_.yPos() + y_step * i; 40 | double new_z = parent_.zPos() + z_step * i; 41 | cells.insert(Cell(new_x + 0.1, new_y + 0.1, new_z)); 42 | cells.insert(Cell(new_x + 0.1, new_y - 0.1, new_z)); 43 | cells.insert(Cell(new_x - 0.1, new_y + 0.1, new_z)); 44 | cells.insert(Cell(new_x - 0.1, new_y - 0.1, new_z)); 45 | } 46 | return cells; 47 | } 48 | 49 | double Node::getLength() const { return parent_.distance3D(cell_); } 50 | 51 | // The number of 45 degree turns needed to go to other 52 | // Assumes that there is not both horizontal and vertical movement is needed 53 | double Node::getRotation(const Node& other) const { 54 | double this_z_diff = cell_.zIndex() - parent_.zIndex(); 55 | double other_z_diff = other.cell_.zIndex() - other.parent_.zIndex(); 56 | double alt_diff = 0.0; 57 | if ((this_z_diff == 0) ^ (other_z_diff == 0)) { 58 | // TODO: use vert_to_hor_cost_ 59 | alt_diff = 0.5; 60 | // return 0.5; // Change between horizontal and vertical movement 61 | } 62 | return alt_diff + getXYRotation(other); 63 | } 64 | 65 | double Node::getXYRotation(const Node& other) const { 66 | Cell this_diff = (cell_ - parent_); 67 | Cell other_diff = (other.cell_ - other.parent_); 68 | if ((this_diff.xIndex() == 0 && this_diff.yIndex() == 0) || (other_diff.xIndex() == 0 && this_diff.yIndex() == 0)) { 69 | return 0.0; // Vertical movement 70 | } 71 | double this_ang = this_diff.angle(); 72 | double other_ang = other_diff.angle(); 73 | double ang_diff = other_ang - this_ang; 74 | ang_diff = std::fabs(angleToRange(ang_diff)); // Rotation needed 75 | double num_45_deg_turns = ang_diff / (M_PI / 4); // Minimum number of 45-turns to goal 76 | return num_45_deg_turns; 77 | } 78 | 79 | std::string Node::asString() const { 80 | std::string s = "(" + cell_.asString() + " , " + parent_.asString() + ")"; 81 | return s; 82 | } 83 | 84 | } // namespace global_planner 85 | -------------------------------------------------------------------------------- /global_planner/src/nodes/global_planner_node_main.cpp: -------------------------------------------------------------------------------- 1 | #include "global_planner/global_planner_node.h" 2 | 3 | int main(int argc, char** argv) { 4 | ros::init(argc, argv, "global_planner_node"); 5 | 6 | ros::NodeHandle nh("~"); 7 | ros::NodeHandle nh_private(""); 8 | 9 | global_planner::GlobalPlannerNode global_planner_node(nh, nh_private); 10 | 11 | ros::spin(); 12 | return 0; 13 | } 14 | -------------------------------------------------------------------------------- /global_planner/src/nodes/mock_data_node.h: -------------------------------------------------------------------------------- 1 | #ifndef GLOBAL_PLANNER_MOCK_DATA_NODE_H 2 | #define GLOBAL_PLANNER_MOCK_DATA_NODE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "global_planner/common.h" // hasSameYawAndAltitude 13 | 14 | namespace global_planner { 15 | 16 | class MockDataNode { 17 | public: 18 | MockDataNode(); 19 | ~MockDataNode(); 20 | void createWall(int dist, int width, int height); 21 | void sendClickedPoint(); 22 | void receivePath(const nav_msgs::Path& msg); 23 | void sendMockData(); 24 | 25 | std::vector points_{5.5, -0.5, 0.5, 5.5, 0.5, 0.5, 5.5, 1.5, 0.5, 5.5, -0.5, 1.5, 5.5, 0.5, 26 | 1.5, 5.5, 1.5, 1.5, 5.5, -0.5, 2.5, 5.5, 0.5, 2.5, 5.5, 1.5, 2.5}; 27 | 28 | private: 29 | ros::Subscriber path_sub_; 30 | 31 | ros::Publisher local_position_pub_; 32 | ros::Publisher depth_points_pub_; 33 | ros::Publisher global_goal_pub_; 34 | }; 35 | 36 | } // namespace global_planner 37 | 38 | #endif // GLOBAL_PLANNER_MOCK_DATA_NODE_H 39 | -------------------------------------------------------------------------------- /global_planner/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char **argv) { 4 | ::testing::InitGoogleTest(&argc, argv); 5 | return RUN_ALL_TESTS(); 6 | } 7 | -------------------------------------------------------------------------------- /global_planner/test/test_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | using std::string; 5 | 6 | class StringTest : public ::testing::Test { 7 | public: 8 | string actualString; 9 | string wrongString; 10 | string expectString; 11 | 12 | char *actualValTrue; 13 | char *actualValFalse; 14 | char *expectVal; 15 | 16 | // Use this method to set up any state that you need for all of your tests 17 | void SetUp() override { 18 | actualString = "hello gtest"; 19 | wrongString = "hello world"; 20 | expectString = "hello gtest"; 21 | 22 | actualValTrue = new char[actualString.size() + 1]; 23 | strncpy(actualValTrue, actualString.c_str(), actualString.size() + 1); 24 | 25 | actualValFalse = new char[wrongString.size() + 1]; 26 | strncpy(actualValFalse, wrongString.c_str(), wrongString.size() + 1); 27 | 28 | expectVal = new char[expectString.size() + 1]; 29 | strncpy(expectVal, expectString.c_str(), expectString.size() + 1); 30 | } 31 | 32 | // Use this method to clean up any memory, network etc. after each test 33 | void TearDown() override { 34 | delete[] actualValTrue; 35 | delete[] actualValFalse; 36 | delete[] expectVal; 37 | } 38 | }; 39 | 40 | // Example code we are testing: 41 | namespace myNormalCode { 42 | 43 | void reverseInPlace(string &toReverse) { 44 | // NB! this only works for ASCII 45 | for (int i = 0, j = toReverse.size() - 1; i < j; i++, j--) { 46 | char tmp = toReverse[i]; 47 | toReverse[i] = toReverse[j]; 48 | toReverse[j] = tmp; 49 | } 50 | } 51 | } 52 | 53 | TEST_F(StringTest, StrEqual) { 54 | // GIVEN: two strings that are the same 55 | 56 | // THEN: we expect them to be equal 57 | EXPECT_STREQ(actualString.c_str(), expectString.c_str()); 58 | } 59 | 60 | TEST_F(StringTest, CStrNotEqual) { 61 | // GIVEN: two char* that are NOT the same 62 | 63 | // THEN: we expect them to be not equal 64 | EXPECT_STRNE(expectVal, actualValFalse); 65 | } 66 | 67 | TEST_F(StringTest, testReverse) { 68 | // GIVEN: a string, and a manually reversed string as well 69 | string toReverse = actualString; 70 | const string expectedReversed = "tsetg olleh"; 71 | 72 | // WHEN: we reverse the string 73 | myNormalCode::reverseInPlace(toReverse); 74 | 75 | // THEN: they should be the same 76 | EXPECT_STREQ(expectedReversed.c_str(), toReverse.c_str()); 77 | } 78 | -------------------------------------------------------------------------------- /local_planner/cfg/LocalPlannerNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "avoidance" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # local_planner 9 | 10 | gen.add("max_sensor_range_", double_t, 0, "Data points farther away will be discarded", 15.0, 0, 40) 11 | gen.add("min_sensor_range_", double_t, 0, "Discard points closer than that", 0.2, 0, 10) 12 | gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 25.0, 0, 30.0) 13 | gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 3.0, 0, 20.0) 14 | gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 6000, 0.0, 50000.0) 15 | gen.add("obstacle_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 8.5, 0.0, 30.0) 16 | gen.add("tree_heuristic_weight_", double_t, 0, "Weight for the tree heuristic cost", 35.0, 0.0, 50.0) 17 | gen.add("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.0) 18 | gen.add("timeout_startup_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 5, 0, 60) 19 | gen.add("timeout_critical_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 0.5, 0, 10) 20 | gen.add("timeout_termination_", double_t, 0, "After this timeout the companion status is MAV_STATE_FLIGHT_TERMINATION", 15, 0, 1000) 21 | gen.add("max_point_age_s_", double_t, 0, "maximum age of a remembered data point", 20, 0, 500) 22 | gen.add("min_num_points_per_cell_", int_t, 0, "minimum number of points in one area to be kept, if lower they are discarded as noise", 1, 1, 500) 23 | gen.add("smoothing_speed_xy_", double_t, 0, "response speed of the smoothing system in xy (set to 0 to disable)", 10, 0, 30) 24 | gen.add("smoothing_speed_z_", double_t, 0, "response speed of the smoothing system in z (set to 0 to disable)", 3, 0, 30) 25 | gen.add("smoothing_margin_degrees_", double_t, 0, "smoothing radius for obstacle cost in cost histogram", 40, 0, 90) 26 | 27 | # star_planner 28 | gen.add("children_per_node_", int_t, 0, "Branching factor of the search tree", 8, 0, 100) 29 | gen.add("n_expanded_nodes_", int_t, 0, "Number of nodes expanded in complete tree", 40, 0, 200) 30 | gen.add("tree_node_distance_", double_t, 0, "Distance between nodes", 2, 0, 20) 31 | 32 | exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode")) 33 | -------------------------------------------------------------------------------- /local_planner/cfg/vehicle.yaml: -------------------------------------------------------------------------------- 1 | # This is a sample config file for a real vehicle 2 | !!python/object/new:dynamic_reconfigure.encoding.Config 3 | dictitems: 4 | adapt_cost_params_: true 5 | box_radius_: 12.0 6 | children_per_node_: 50 7 | goal_cost_param_: 10.0 8 | goal_z_param: 3.0 9 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 10 | dictitems: 11 | adapt_cost_params_: true 12 | box_radius_: 12.0 13 | children_per_node_: 50 14 | goal_cost_param_: 10.0 15 | goal_z_param: 3.0 16 | groups: !!python/object/new:dynamic_reconfigure.encoding.Config 17 | state: [] 18 | heading_cost_param_: 0.5 19 | id: 0 20 | max_point_age_s_: 20.0 21 | min_num_points_per_cell_: 25 # Tune to your sensor requirements! 22 | min_sensor_range_: 0.2 23 | n_expanded_nodes_: 10 24 | name: Default 25 | no_progress_slope_: -0.0007 26 | parameters: !!python/object/new:dynamic_reconfigure.encoding.Config 27 | state: [] 28 | parent: 0 29 | smooth_cost_param_: 1.5 30 | smoothing_margin_degrees_: 40.0 31 | smoothing_speed_xy_: 10.0 32 | smoothing_speed_z_: 3.0 33 | state: true 34 | timeout_critical_: 0.5 35 | timeout_termination_: 15.0 36 | tree_discount_factor_: 0.8 37 | tree_node_distance_: 1.0 38 | type: '' 39 | state: [] 40 | heading_cost_param_: 0.5 41 | max_point_age_s_: 20.0 42 | min_num_points_per_cell_: 25 # Tune to your sensor requirements! 43 | min_sensor_range_: 0.2 44 | n_expanded_nodes_: 10 45 | no_progress_slope_: -0.0007 46 | smooth_cost_param_: 1.5 47 | smoothing_margin_degrees_: 40.0 48 | smoothing_speed_xy_: 10.0 49 | smoothing_speed_z_: 3.0 50 | timeout_critical_: 0.5 51 | timeout_termination_: 15.0 52 | tree_discount_factor_: 0.8 53 | tree_node_distance_: 1.0 54 | state: [] 55 | -------------------------------------------------------------------------------- /local_planner/include/local_planner/avoidance_output.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace avoidance { 9 | 10 | enum waypoint_choice { hover, tryPath, direct, reachHeight }; 11 | 12 | struct avoidanceOutput { 13 | float cruise_velocity; // mission cruise velocity 14 | ros::Time last_path_time; // finish built time for the VFH+* tree 15 | 16 | std::vector path_node_positions; // array of tree nodes 17 | // position, each node 18 | // is the minimum cost 19 | // node for each tree 20 | // depth level 21 | }; 22 | } 23 | -------------------------------------------------------------------------------- /local_planner/include/local_planner/candidate_direction.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "avoidance/common.h" 3 | 4 | namespace avoidance { 5 | 6 | struct candidateDirection { 7 | float cost; 8 | float elevation_angle; 9 | float azimuth_angle; 10 | 11 | candidateDirection(float c, float e, float z) : cost(c), elevation_angle(e), azimuth_angle(z){}; 12 | 13 | bool operator<(const candidateDirection& y) const { return cost < y.cost; } 14 | 15 | bool operator>(const candidateDirection& y) const { return cost > y.cost; } 16 | 17 | PolarPoint toPolar(float r) const { return PolarPoint(elevation_angle, azimuth_angle, r); } 18 | }; 19 | } 20 | -------------------------------------------------------------------------------- /local_planner/include/local_planner/cost_parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace avoidance { 4 | 5 | struct costParameters { 6 | float yaw_cost_param = 0.5f; 7 | float pitch_cost_param = 3.f; 8 | float velocity_cost_param = 1.5f; 9 | float obstacle_cost_param = 5.0f; 10 | }; 11 | } 12 | -------------------------------------------------------------------------------- /local_planner/include/local_planner/star_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef STAR_PLANNER_H 2 | #define STAR_PLANNER_H 3 | 4 | #include "avoidance/histogram.h" 5 | #include "cost_parameters.h" 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace avoidance { 20 | class TreeNode; 21 | 22 | class StarPlanner { 23 | int children_per_node_ = 1; 24 | int n_expanded_nodes_ = 5; 25 | float tree_node_distance_ = 1.0f; 26 | float max_path_length_ = 4.f; 27 | float smoothing_margin_degrees_ = 30.f; 28 | float tree_heuristic_weight_ = 10.0f; 29 | float max_sensor_range_ = 15.f; 30 | float min_sensor_range_ = 0.2f; 31 | 32 | pcl::PointCloud cloud_; 33 | 34 | Eigen::Vector3f goal_ = Eigen::Vector3f(NAN, NAN, NAN); 35 | Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN); 36 | Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN); 37 | Eigen::Vector3f closest_pt_ = Eigen::Vector3f(NAN, NAN, NAN); 38 | costParameters cost_params_; 39 | 40 | protected: 41 | /** 42 | * @brief computes the heuristic for a node 43 | * @param[in] node_number, sequential number of entry in the tree 44 | * @returns 45 | **/ 46 | float treeHeuristicFunction(int node_number) const; 47 | 48 | public: 49 | std::vector path_node_positions_; 50 | std::vector closed_set_; 51 | std::vector tree_; 52 | 53 | StarPlanner(); 54 | ~StarPlanner() = default; 55 | 56 | /** 57 | * @brief setter method for costMatrix paramters 58 | * @param[in] cost_params, parameters for the histogram cost function 59 | **/ 60 | void setParams(costParameters cost_params); 61 | 62 | /** 63 | * @brief setter method for star_planner pointcloud 64 | * @param[in] cloud, processed data already cropped and combined with history 65 | **/ 66 | void setPointcloud(const pcl::PointCloud& cloud); 67 | 68 | /** 69 | * @brief setter method for vehicle position 70 | * @param[in] vehicle current position 71 | **/ 72 | void setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel); 73 | 74 | /** 75 | * @brief setter method for vehicle position projection on the line between the current and previous goal 76 | * @param[in] closest_pt, projection point 77 | **/ 78 | void setClosestPointOnLine(const Eigen::Vector3f& closest_pt); 79 | 80 | /** 81 | * @brief setter method for current goal 82 | * @param[in] goal, current goal position 83 | **/ 84 | void setGoal(const Eigen::Vector3f& pose); 85 | 86 | /** 87 | * @brief build tree of candidates directions towards the goal 88 | **/ 89 | void buildLookAheadTree(); 90 | 91 | /** 92 | * @brief setter method for server paramters 93 | **/ 94 | void dynamicReconfigureSetStarParams(const avoidance::LocalPlannerNodeConfig& config, uint32_t level); 95 | }; 96 | } 97 | #endif // STAR_PLANNER_H 98 | -------------------------------------------------------------------------------- /local_planner/include/local_planner/trajectory_simulator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace avoidance { 7 | 8 | struct simulation_state { 9 | float time = NAN; 10 | Eigen::Vector3f position = NAN * Eigen::Vector3f::Ones(); 11 | Eigen::Vector3f velocity = NAN * Eigen::Vector3f::Ones(); 12 | Eigen::Vector3f acceleration = NAN * Eigen::Vector3f::Ones(); 13 | }; 14 | 15 | struct simulation_limits { 16 | float max_z_velocity = NAN; 17 | float min_z_velocity = NAN; 18 | float max_xy_velocity_norm = NAN; 19 | float max_acceleration_norm = NAN; 20 | float max_jerk_norm = NAN; 21 | }; 22 | 23 | class TrajectorySimulator { 24 | public: 25 | TrajectorySimulator(const simulation_limits& config, const simulation_state& start, float step_time = 0.1f); 26 | 27 | std::vector generate_trajectory(const Eigen::Vector3f& goal_direction, float simulation_duration); 28 | 29 | protected: 30 | const simulation_limits config_; 31 | const simulation_state start_; 32 | const float step_time_; 33 | 34 | static simulation_state simulate_step_constant_jerk(const simulation_state& state, const Eigen::Vector3f& jerk, 35 | float step_time); 36 | 37 | static Eigen::Vector3f jerk_for_velocity_setpoint(float P_constant, float D_constant, float max_jerk_norm, 38 | const Eigen::Vector3f& desired_velocity, 39 | const simulation_state& state); 40 | }; 41 | 42 | // templated helper function 43 | template 44 | Eigen::Matrix norm_clamp(const Eigen::Matrix& val, float max_norm) { 45 | float norm_sq = val.squaredNorm(); 46 | if (norm_sq > max_norm * max_norm) 47 | return val * (max_norm / std::sqrt(norm_sq)); 48 | else 49 | return val; 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /local_planner/include/local_planner/tree_node.h: -------------------------------------------------------------------------------- 1 | #ifndef TREE_NODE_H 2 | #define TREE_NODE_H 3 | 4 | #include 5 | #include 6 | 7 | namespace avoidance { 8 | 9 | class TreeNode { 10 | Eigen::Vector3f position_; 11 | Eigen::Vector3f velocity_; 12 | 13 | public: 14 | float total_cost_; 15 | float heuristic_; 16 | int origin_; 17 | bool closed_; 18 | 19 | TreeNode(); 20 | TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel); 21 | ~TreeNode() = default; 22 | 23 | /** 24 | * @brief setter method for heuristic and cost of a tree node 25 | * @param[in] h, heuristic 26 | * @param[in] c, cost 27 | **/ 28 | void setCosts(float h, float c); 29 | 30 | /** 31 | * @brief getter method for tree node position 32 | * @returns node position in 3D cartesian coordinates 33 | **/ 34 | Eigen::Vector3f getPosition() const; 35 | Eigen::Vector3f getVelocity() const; 36 | }; 37 | } 38 | 39 | #endif // TREE_NODE_H 40 | -------------------------------------------------------------------------------- /local_planner/launch/local_planner_aero.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | $(arg pointcloud_topics) 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /local_planner/launch/local_planner_aeroD415.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | $(arg pointcloud_topics) 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /local_planner/launch/local_planner_depth-camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | $(arg pointcloud_topics) 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /local_planner/launch/local_planner_realsense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | $(arg pointcloud_topics) 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /local_planner/launch/local_planner_sitl_3cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | $(arg pointcloud_topics) 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /local_planner/launch/local_planner_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | $(arg pointcloud_topics) 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /local_planner/launch/mavros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /local_planner/launch/rs_depthcloud.launch: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /local_planner/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Local planner nodelet 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | local_planner 4 | 0.0.0 5 | Avoidance module doing local planning. 6 | 7 | 8 | 9 | 10 | pixhawk 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | dynamic_reconfigure 45 | message_generation 46 | roscpp 47 | rospy 48 | std_msgs 49 | geometry_msgs 50 | sensor_msgs 51 | tf 52 | pcl_ros 53 | mavros 54 | mavros_extras 55 | mavros_msgs 56 | avoidance 57 | nodelet 58 | 59 | dynamic_reconfigure 60 | message_runtime 61 | roscpp 62 | rospy 63 | std_msgs 64 | geometry_msgs 65 | sensor_msgs 66 | octomap 67 | tf 68 | pcl_ros 69 | mavros 70 | mavros_extras 71 | mavros_msgs 72 | avoidance 73 | nodelet 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /local_planner/resource/custom_rosconsole.conf: -------------------------------------------------------------------------------- 1 | log4j.logger.ros.local_planner=WARN 2 | -------------------------------------------------------------------------------- /local_planner/resource/rqt_param_toggle.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Toggle or set rqt parameters 3 | rosrun dynamic_reconfigure dynparam set /sc_node depth_range_mode 3 4 | 5 | -------------------------------------------------------------------------------- /local_planner/resource/stereo_calib.json: -------------------------------------------------------------------------------- 1 | { 2 | "aux-param-autoexposure-setpoint": "1536", 3 | "aux-param-colorcorrection1": "0.298828", 4 | "aux-param-colorcorrection10": "0", 5 | "aux-param-colorcorrection11": "0", 6 | "aux-param-colorcorrection12": "0", 7 | "aux-param-colorcorrection2": "0.293945", 8 | "aux-param-colorcorrection3": "0.293945", 9 | "aux-param-colorcorrection4": "0.114258", 10 | "aux-param-colorcorrection5": "0", 11 | "aux-param-colorcorrection6": "0", 12 | "aux-param-colorcorrection7": "0", 13 | "aux-param-colorcorrection8": "0", 14 | "aux-param-colorcorrection9": "0", 15 | "aux-param-depthclampmax": "65534", 16 | "aux-param-depthclampmin": "0", 17 | "aux-param-disparityshift": "0", 18 | "controls-autoexposure-auto": "True", 19 | "controls-autoexposure-manual": "8500", 20 | "controls-depth-gain": "16", 21 | "ignoreSAD": "0", 22 | "param-autoexposure-setpoint": "1536", 23 | "param-censusenablereg-udiameter": "6", 24 | "param-censusenablereg-vdiameter": "7", 25 | "param-censususize": "6", 26 | "param-censusvsize": "7", 27 | "param-depthclampmax": "65534", 28 | "param-depthclampmin": "0", 29 | "param-depthunits": "1000", 30 | "param-disableraucolor": "0", 31 | "param-disablesadcolor": "0", 32 | "param-disablesadnormalize": "0", 33 | "param-disablesloleftcolor": "0", 34 | "param-disableslorightcolor": "0", 35 | "param-disparitymode": "0", 36 | "param-disparityshift": "0", 37 | "param-lambdaad": "630", 38 | "param-lambdacensus": "26", 39 | "param-leftrightthreshold": "18", 40 | "param-maxscorethreshb": "2047", 41 | "param-medianthreshold": "347", 42 | "param-minscorethresha": "80", 43 | "param-neighborthresh": "82", 44 | "param-raumine": "3", 45 | "param-rauminn": "1", 46 | "param-rauminnssum": "6", 47 | "param-raumins": "2", 48 | "param-rauminw": "2", 49 | "param-rauminwesum": "3", 50 | "param-regioncolorthresholdb": "0.0254403", 51 | "param-regioncolorthresholdg": "0.954012", 52 | "param-regioncolorthresholdr": "0.0283757", 53 | "param-regionshrinku": "4", 54 | "param-regionshrinkv": "1", 55 | "param-robbinsmonrodecrement": "15", 56 | "param-robbinsmonroincrement": "5", 57 | "param-rsmdiffthreshold": "3.34375", 58 | "param-rsmrauslodiffthreshold": "0.96875", 59 | "param-rsmremovethreshold": "0.666667", 60 | "param-scanlineedgetaub": "442", 61 | "param-scanlineedgetaug": "16", 62 | "param-scanlineedgetaur": "770", 63 | "param-scanlinep1": "21", 64 | "param-scanlinep1onediscon": "3", 65 | "param-scanlinep1twodiscon": "27", 66 | "param-scanlinep2": "182", 67 | "param-scanlinep2onediscon": "41", 68 | "param-scanlinep2twodiscon": "63", 69 | "param-secondpeakdelta": "205", 70 | "param-texturecountthresh": "0", 71 | "param-texturedifferencethresh": "3474", 72 | "param-usersm": "1", 73 | "param-zunits": "1000", 74 | "stream-depth-format": "Z16", 75 | "stream-fps": "15", 76 | "stream-height": "480", 77 | "stream-width": "640" 78 | } 79 | -------------------------------------------------------------------------------- /local_planner/src/nodes/local_planner_node_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char **argv) { 5 | ros::init(argc, argv, "local_planner_nodelet"); 6 | 7 | nodelet::Loader nodelet; 8 | nodelet::M_string remap(ros::names::getRemappings()); 9 | nodelet::V_string nargv; 10 | std::string nodelet_name = ros::this_node::getName(); 11 | nodelet.load(nodelet_name, "LocalPlannerNodelet", remap, nargv); 12 | ros::spin(); 13 | 14 | return 0; 15 | } -------------------------------------------------------------------------------- /local_planner/src/nodes/tree_node.cpp: -------------------------------------------------------------------------------- 1 | #include "local_planner/tree_node.h" 2 | 3 | namespace avoidance { 4 | 5 | TreeNode::TreeNode() : total_cost_{0.0f}, heuristic_{0.0f}, origin_{0}, closed_{false} { 6 | position_ = Eigen::Vector3f::Zero(); 7 | velocity_ = Eigen::Vector3f::Zero(); 8 | } 9 | 10 | TreeNode::TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel) 11 | : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false} { 12 | position_ = pos; 13 | velocity_ = vel; 14 | } 15 | 16 | void TreeNode::setCosts(float h, float c) { 17 | heuristic_ = h; 18 | total_cost_ = c; 19 | } 20 | 21 | Eigen::Vector3f TreeNode::getPosition() const { return position_; } 22 | Eigen::Vector3f TreeNode::getVelocity() const { return velocity_; } 23 | } 24 | -------------------------------------------------------------------------------- /local_planner/src/nodes/waypoint_generator.cpp.dot: -------------------------------------------------------------------------------- 1 | digraph { 2 | "TRY_PATH" -> "ALTITUDE_CHANGE" [label="NEXT1", style="solid", weight=1] 3 | "TRY_PATH" -> "DIRECT" [label="NEXT2", style="solid", weight=1] 4 | "TRY_PATH" -> "LOITER" [label="NEXT3\nERROR", style="solid", weight=1] 5 | "LOITER" -> "TRY_PATH" [label="NEXT1", style="solid", weight=1] 6 | "LOITER" -> "LOITER" [label="ERROR", style="dotted", weight=0.1] 7 | "DIRECT" -> "TRY_PATH" [label="NEXT1", style="solid", weight=1] 8 | "DIRECT" -> "ALTITUDE_CHANGE" [label="NEXT2", style="solid", weight=1] 9 | "DIRECT" -> "LOITER" [label="NEXT3\nERROR", style="solid", weight=1] 10 | "ALTITUDE_CHANGE" -> "TRY_PATH" [label="NEXT1", style="solid", weight=1] 11 | "ALTITUDE_CHANGE" -> "LOITER" [label="NEXT2\nERROR", style="solid", weight=1] 12 | } -------------------------------------------------------------------------------- /local_planner/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char **argv) { 6 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error); 7 | ::testing::InitGoogleTest(&argc, argv); 8 | ros::init(argc, argv, "test"); 9 | return RUN_ALL_TESTS(); 10 | } 11 | -------------------------------------------------------------------------------- /local_planner/test/test_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | using std::string; 5 | 6 | class StringTest : public ::testing::Test { 7 | public: 8 | string actualString; 9 | string wrongString; 10 | string expectString; 11 | 12 | char *actualValTrue; 13 | char *actualValFalse; 14 | char *expectVal; 15 | 16 | // Use this method to set up any state that you need for all of your tests 17 | void SetUp() override { 18 | actualString = "hello gtest"; 19 | wrongString = "hello world"; 20 | expectString = "hello gtest"; 21 | 22 | actualValTrue = new char[actualString.size() + 1]; 23 | strncpy(actualValTrue, actualString.c_str(), actualString.size() + 1); 24 | 25 | actualValFalse = new char[wrongString.size() + 1]; 26 | strncpy(actualValFalse, wrongString.c_str(), wrongString.size() + 1); 27 | 28 | expectVal = new char[expectString.size() + 1]; 29 | strncpy(expectVal, expectString.c_str(), expectString.size() + 1); 30 | } 31 | 32 | // Use this method to clean up any memory, network etc. after each test 33 | void TearDown() override { 34 | delete[] actualValTrue; 35 | delete[] actualValFalse; 36 | delete[] expectVal; 37 | } 38 | }; 39 | 40 | // Example code we are testing: 41 | namespace myNormalCode { 42 | 43 | void reverseInPlace(string &toReverse) { 44 | // NB! this only works for ASCII 45 | for (int i = 0, j = toReverse.size() - 1; i < j; i++, j--) { 46 | char tmp = toReverse[i]; 47 | toReverse[i] = toReverse[j]; 48 | toReverse[j] = tmp; 49 | } 50 | } 51 | } 52 | 53 | TEST_F(StringTest, StrEqual) { 54 | // GIVEN: two strings that are the same 55 | 56 | // THEN: we expect them to be equal 57 | EXPECT_STREQ(actualString.c_str(), expectString.c_str()); 58 | } 59 | 60 | TEST_F(StringTest, CStrNotEqual) { 61 | // GIVEN: two char* that are NOT the same 62 | 63 | // THEN: we expect them to be not equal 64 | EXPECT_STRNE(expectVal, actualValFalse); 65 | } 66 | 67 | TEST_F(StringTest, testReverse) { 68 | // GIVEN: a string, and a manually reversed string as well 69 | string toReverse = actualString; 70 | const string expectedReversed = "tsetg olleh"; 71 | 72 | // WHEN: we reverse the string 73 | myNormalCode::reverseInPlace(toReverse); 74 | 75 | // THEN: they should be the same 76 | EXPECT_STREQ(expectedReversed.c_str(), toReverse.c_str()); 77 | } 78 | -------------------------------------------------------------------------------- /local_planner/test/test_star_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "../include/local_planner/star_planner.h" 4 | #include "../include/local_planner/tree_node.h" 5 | #include "avoidance/common.h" 6 | 7 | using namespace avoidance; 8 | 9 | class StarPlannerBasicTests : public ::testing::Test, public StarPlanner { 10 | void SetUp() override{}; 11 | void TearDown() override{}; 12 | }; 13 | 14 | class StarPlannerTests : public ::testing::Test { 15 | public: 16 | StarPlanner star_planner; 17 | float obstacle_half_height = 0.5f; 18 | float obstacle_min_x = 0.0f; 19 | float obstacle_max_x = 2.5f; 20 | float obstacle_y = 2.0f; 21 | Eigen::Vector3f goal; 22 | Eigen::Vector3f position; 23 | Eigen::Vector3f velocity; 24 | 25 | void SetUp() override { 26 | ros::Time::init(); 27 | 28 | avoidance::LocalPlannerNodeConfig config = avoidance::LocalPlannerNodeConfig::__getDefault__(); 29 | config.children_per_node_ = 2; 30 | config.n_expanded_nodes_ = 10; 31 | config.tree_node_distance_ = 1.0; 32 | star_planner.dynamicReconfigureSetStarParams(config, 1); 33 | 34 | position.x() = 1.2f; 35 | position.y() = 0.4f; 36 | position.z() = 4.0f; 37 | 38 | velocity.x() = 0.0f; 39 | velocity.y() = 0.0f; 40 | velocity.z() = 0.0f; 41 | 42 | goal.x() = 2.0f; 43 | goal.y() = 14.0f; 44 | goal.z() = 4.0f; 45 | 46 | pcl::PointCloud cloud; 47 | for (float x = obstacle_min_x; x < obstacle_max_x; x += 0.05f) { 48 | for (float z = goal.z() - obstacle_half_height; z < goal.z() + obstacle_half_height; z += 0.05f) { 49 | cloud.push_back(toXYZI(x, obstacle_y, z, 0)); 50 | } 51 | } 52 | costParameters cost_params; 53 | 54 | star_planner.setParams(cost_params); 55 | star_planner.setPointcloud(cloud); 56 | star_planner.setPose(position, velocity); 57 | star_planner.setGoal(goal); 58 | star_planner.setClosestPointOnLine(goal); 59 | } 60 | void TearDown() override {} 61 | }; 62 | 63 | TEST_F(StarPlannerTests, buildTree) { 64 | // GIVEN: a vehicle position, a goal, and an obstacle in between the straight 65 | // line position-goal 66 | 67 | float dist_to_goal = 1000.0f; 68 | float min_dist_to_goal = dist_to_goal; 69 | // WHEN: we build the tree for 15 times 70 | for (size_t i = 0; i < 15; i++) { 71 | star_planner.buildLookAheadTree(); 72 | for (auto node : star_planner.tree_) { 73 | // THEN: we expect each tree node position not to be close to the obstacle 74 | Eigen::Vector3f n = node.getPosition(); 75 | bool node_inside_obstacle = n.x() > obstacle_min_x && n.x() < obstacle_max_x && n.y() > obstacle_y - 0.1f && 76 | n.y() < obstacle_y + 0.1f && n.z() > 4.0f - obstacle_half_height && 77 | n.z() < 4.0f + obstacle_half_height; 78 | EXPECT_FALSE(node_inside_obstacle); 79 | } 80 | 81 | // THEN: we expect the distance between the vehicle and the goal to shorten 82 | // at each iteration 83 | float tmp_dist_to_goal = (goal - position).norm(); 84 | EXPECT_LT(tmp_dist_to_goal, min_dist_to_goal * 1.1); 85 | dist_to_goal = tmp_dist_to_goal; 86 | if (dist_to_goal < min_dist_to_goal) min_dist_to_goal = dist_to_goal; 87 | 88 | // we set the vehicle position to be the first node position after the 89 | // origin for the next algorithm iterarion 90 | position = star_planner.tree_[1].getPosition(); 91 | star_planner.setPose(position, velocity); 92 | } 93 | } 94 | 95 | TEST_F(StarPlannerTests, heuristicFunction) {} 96 | -------------------------------------------------------------------------------- /local_planner/test/valgrind_suppressions.sup: -------------------------------------------------------------------------------- 1 | { 2 | Static initialization of nodelets 3 | Memcheck:Leak 4 | match-leak-kinds: definite 5 | fun:_Znwm 6 | fun:_ZN12class_loader20class_loader_private14registerPluginIN9avoidance19LocalPlannerNodeletEN7nodelet7NodeletEEEvRKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESD_ 7 | fun:ProxyExec0 8 | fun:__static_initialization_and_destruction_0 9 | fun:_GLOBAL__sub_I_local_planner_nodelet.cpp 10 | fun:call_init.part.0 11 | fun:call_init 12 | fun:_dl_init 13 | obj:/lib/x86_64-linux-gnu/ld-2.23.so 14 | } 15 | 16 | { 17 | ROS extra thread started for logging 1 18 | Memcheck:Leak 19 | match-leak-kinds: possible 20 | fun:calloc 21 | fun:allocate_dtv 22 | fun:_dl_allocate_tls 23 | fun:allocate_stack 24 | fun:pthread_create@@GLIBC_2.2.5 25 | fun:_ZN5boost6thread21start_thread_noexceptEv 26 | fun:_ZN3ros14ROSOutAppenderC1Ev 27 | fun:_ZN3ros5startEv 28 | fun:_ZN3ros10NodeHandle9constructERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEEb 29 | fun:_ZN3ros10NodeHandleC1ERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEERKSt3mapIS6_S6_St4lessIS6_ESaISt4pairIS7_S6_EEE 30 | fun:_ZN9avoidance19LocalPlannerNodeletC1Ev 31 | fun:_ZN38LocalPlannerNodeletTests_failsafe_Test8TestBodyEv 32 | fun:_ZN7testing8internal38HandleSehExceptionsInMethodIfSupportedINS_4TestEvEET0_PT_MS4_FS3_vEPKc 33 | fun:_ZN7testing8internal35HandleExceptionsInMethodIfSupportedINS_4TestEvEET0_PT_MS4_FS3_vEPKc 34 | } 35 | 36 | { 37 | ROS extra thread 38 | Memcheck:Leak 39 | match-leak-kinds: possible 40 | fun:calloc 41 | fun:allocate_dtv 42 | fun:_dl_allocate_tls 43 | fun:allocate_stack 44 | fun:pthread_create@@GLIBC_2.2.5 45 | fun:_ZN5boost6thread21start_thread_noexceptEv 46 | fun:_ZN5boost6threadC1IRFvvEEEOT_ 47 | fun:_ZN3ros5startEv 48 | fun:_ZN3ros10NodeHandle9constructERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEEb 49 | fun:_ZN3ros10NodeHandleC1ERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEERKSt3mapIS6_S6_St4lessIS6_ESaISt4pairIS7_S6_EEE 50 | fun:_ZN9avoidance19LocalPlannerNodeletC1Ev 51 | fun:_ZN38LocalPlannerNodeletTests_failsafe_Test8TestBodyEv 52 | fun:_ZN7testing8internal38HandleSehExceptionsInMethodIfSupportedINS_4TestEvEET0_PT_MS4_FS3_vEPKc 53 | fun:_ZN7testing8internal35HandleExceptionsInMethodIfSupportedINS_4TestEvEET0_PT_MS4_FS3_vEPKc 54 | } 55 | -------------------------------------------------------------------------------- /safe_landing_planner/cfg/SafeLandingPlannerNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "safe_landing_planner" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("n_points_threshold", double_t, 0, "Minimum number of points to be considered in a cell", 100.0, 0.0, 5000.0) 9 | gen.add("std_dev_threshold", double_t, 0, "Threshold on the cell variance to be considered for landing", 0.2, 0.0, 1.0) 10 | gen.add("smoothing_size", int_t, 0, "2*smoothing_size+1 is the smoothing kernel size", 5, -1, 100) 11 | gen.add("mean_diff_thr", double_t, 0, "Threshold on the mean value difference between two cells", 0.1, 0.0, 1.0) 12 | gen.add("max_n_mean_diff_cells", int_t, 0, "Maxmum number of cells in the neighborhood that can be different more than mean_diff_thr", 70, 0, 100) 13 | gen.add("min_n_land_cells", int_t, 0, "Minimum cell number that need to be flat in the neighborhood", 70, 0, 100) 14 | 15 | gen.add("grid_size", double_t, 0, "Size of the square grid in meters ", 10.0, 1.0, 20.0) 16 | gen.add("cell_size", double_t, 0, "Size of the square cells in the grid in meters ", 0.25, 0.1, 10.0) 17 | gen.add("alpha", double_t, 0, "History parameter on mean/variance temporal smoothing", 0.9, 0.0, 1.0) 18 | 19 | gen.add("timeout_critical", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 0.5, 0, 10) 20 | gen.add("timeout_termination", double_t, 0, "After this timeout the companion status is MAV_STATE_FLIGHT_TERMINATION", 15, 0, 1000) 21 | 22 | 23 | exit(gen.generate(PACKAGE, "safe_landing_planner", "SafeLandingPlannerNode")) 24 | -------------------------------------------------------------------------------- /safe_landing_planner/cfg/WaypointGeneratorNode.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "safe_landing_planner" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("can_land_thr", double_t, 0, "Hysteresis on landable cell", 0.4, 0.0, 1.0) 9 | gen.add("loiter_height", double_t, 0, "Z distance to the pointcloud to start loitering ", 8.0, 1, 12) 10 | gen.add("smoothing_land_cell", int_t, 0, "Kernel size on cell land hysteresis", 6, 0, 100) 11 | gen.add("beta", double_t, 0, "History paramter on land decision per cell", 0.9, 0.0, 1.0) 12 | gen.add("vertical_range_error", double_t, 0, "If the different to loiter_height is greater than this paramter, the vehicle adjust altitude before taking decision", 0.5, 0.0, 4.0) 13 | gen.add("spiral_width", double_t, 0, "Factor to increase squared spiral width", 2.0, 1.0, 10.0) 14 | 15 | 16 | exit(gen.generate(PACKAGE, "safe_landing_planner", "WaypointGeneratorNode")) 17 | -------------------------------------------------------------------------------- /safe_landing_planner/cfg/slpn.yaml: -------------------------------------------------------------------------------- 1 | n_points_threshold: 1.0 2 | -------------------------------------------------------------------------------- /safe_landing_planner/cfg/wpgn.yaml: -------------------------------------------------------------------------------- 1 | loiter_height: 7.0 2 | -------------------------------------------------------------------------------- /safe_landing_planner/include/safe_landing_planner/grid.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace avoidance { 7 | 8 | class Grid { 9 | public: 10 | Grid(const float grid_size, const float cell_size) : grid_size_(grid_size), cell_size_(cell_size) { 11 | resize(grid_size_, cell_size_); 12 | } 13 | ~Grid() = default; 14 | 15 | void reset() { 16 | mean_.fill(0.f); 17 | variance_.fill(0.f); 18 | counter_.fill(0); 19 | land_.fill(0); 20 | } 21 | 22 | void resize(float grid_size, float cell_size) { 23 | grid_size_ = grid_size; 24 | cell_size_ = cell_size; 25 | grid_row_col_size_ = static_cast(std::ceil(grid_size_ / cell_size_)); 26 | mean_.resize(grid_row_col_size_, grid_row_col_size_); 27 | variance_.resize(grid_row_col_size_, grid_row_col_size_); 28 | counter_.resize(grid_row_col_size_, grid_row_col_size_); 29 | land_.resize(grid_row_col_size_, grid_row_col_size_); 30 | reset(); 31 | } 32 | 33 | void setMean(const Eigen::Vector2i &idx, float value) { mean_(idx.x(), idx.y()) = value; } 34 | void setVariance(const Eigen::Vector2i &idx, float value) { variance_(idx.x(), idx.y()) = value; } 35 | void increaseCounter(const Eigen::Vector2i &idx) { counter_(idx.x(), idx.y()) = counter_(idx.x(), idx.y()) + 1; } 36 | void setCounter(const Eigen::Vector2i &idx, int value) { counter_(idx.x(), idx.y()) = value; } 37 | 38 | Eigen::MatrixXf getMean() const { return mean_; } 39 | Eigen::MatrixXf getVariance() const { return variance_; } 40 | Eigen::MatrixXi getCounter() const { return counter_; } 41 | 42 | float getMean(const Eigen::Vector2i &idx) { return mean_(idx.x(), idx.y()); } 43 | float getVariance(const Eigen::Vector2i &idx) { return variance_(idx.x(), idx.y()); } 44 | int getCounter(const Eigen::Vector2i &idx) { return counter_(idx.x(), idx.y()); } 45 | int getRowColSize() const { return grid_row_col_size_; } 46 | float getGridSize() const { return grid_size_; } 47 | float getCellSize() const { return cell_size_; } 48 | 49 | void setFilterLimits(const Eigen::Vector3f &pos) { 50 | corner_min_.x() = pos.x() - grid_size_ / 2.f; 51 | corner_min_.y() = pos.y() - grid_size_ / 2.f; 52 | corner_max_.x() = pos.x() + grid_size_ / 2.f; 53 | corner_max_.y() = pos.y() + grid_size_ / 2.f; 54 | } 55 | 56 | void getGridLimits(Eigen::Vector2f &min, Eigen::Vector2f &max) const { 57 | min = corner_min_; 58 | max = corner_max_; 59 | } 60 | 61 | void combine(const Grid &prev_grid, float alpha) { 62 | mean_ = alpha * prev_grid.mean_ + (1.f - alpha) * mean_; 63 | variance_ = alpha * prev_grid.variance_ + (1.f - alpha) * variance_; 64 | } 65 | 66 | Eigen::MatrixXi land_; 67 | Eigen::MatrixXf mean_; 68 | 69 | private: 70 | Eigen::Vector2f corner_min_; 71 | Eigen::Vector2f corner_max_; 72 | Eigen::MatrixXf variance_; 73 | Eigen::MatrixXi counter_; 74 | 75 | float grid_size_; 76 | float cell_size_; 77 | int grid_row_col_size_; 78 | }; 79 | } 80 | -------------------------------------------------------------------------------- /safe_landing_planner/include/safe_landing_planner/safe_landing_planner.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "grid.hpp" 13 | 14 | namespace avoidance { 15 | 16 | class SafeLandingPlanner { 17 | public: 18 | SafeLandingPlanner() = default; 19 | ~SafeLandingPlanner() = default; 20 | 21 | pcl::PointCloud cloud_; 22 | pcl::PointCloud visualization_cloud_; 23 | 24 | double timeout_critical_ = 0.5; 25 | double timeout_termination_ = 15.0; 26 | 27 | /** 28 | * @brief setter method for current vehicle position and orientation 29 | * @param[in] pos, vehicle position 30 | * @param[in] q, quaternion vehicle orientation 31 | **/ 32 | void setPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& q); 33 | 34 | /** 35 | * @brif runs landing site detection algorithm 36 | **/ 37 | void runSafeLandingPlanner(); 38 | /** 39 | * @brief sets parameters from ROS parameter server 40 | * @param config, struct containing all the parameters 41 | * @param level, bitmask to group together reconfigurable parameters 42 | **/ 43 | void dynamicReconfigureSetParams(const safe_landing_planner::SafeLandingPlannerNodeConfig& config, uint32_t level); 44 | 45 | /** 46 | * @brief based on counter, standard devuation and mean, it decides if a cell 47 | *is landable 48 | **/ 49 | void isLandingPossible(); 50 | 51 | Eigen::Vector2i getPositionIndex() const { return pos_index_; }; 52 | Grid getPreviousGrid() const { return previous_grid_; }; 53 | Grid getGrid() const { return grid_; }; 54 | int getSmoothingSize() const { return smoothing_size_; }; 55 | 56 | safe_landing_planner::SLPGridMsg raw_grid_; 57 | 58 | bool play_rosbag_ = false; 59 | 60 | protected: 61 | Eigen::Vector3f position_ = Eigen::Vector3f::Zero(); 62 | Eigen::Vector2i pos_index_ = Eigen::Vector2i(-1, -1); 63 | 64 | float n_points_thr_ = 1.f; 65 | float std_dev_thr_ = 0.1f; 66 | float grid_size_ = 10.f; 67 | float cell_size_ = 1.f; 68 | float mean_diff_thr_ = 0.3f; 69 | float alpha_ = 0.8f; 70 | int n_lines_padding_ = 1; 71 | int max_n_mean_diff_cells_ = 2; 72 | int grid_seq_ = 0; 73 | int smoothing_size_ = 1; 74 | int min_n_land_cells_ = 9; 75 | bool size_update_ = false; 76 | 77 | Grid grid_ = Grid(10.f, 1.f); 78 | Grid previous_grid_ = Grid(10.f, 1.f); 79 | 80 | /** 81 | * @brief process the pointcloud and calculate mean and variance for points in 82 | *the grid 83 | **/ 84 | void processPointcloud(); 85 | 86 | /** 87 | * @brief checks if a point cloud point is inside the 2D grid 88 | * @param[in] x, x coordinate of the pointcloud point 89 | * @param[in] y, y coordinate of the pointcloud point 90 | * @return true, if point is inside the grid 91 | **/ 92 | bool isInsideGrid(float x, float y); 93 | /** 94 | * @brief computes the grid bin to which a point is mapped to 95 | * @param[in] x, x coordinate of the pointcloud point 96 | * @param[in] y, y coordinate of the pointcloud point 97 | * @returns indexes of the 2D grid 98 | **/ 99 | Eigen::Vector2i computeGridIndexes(float x, float y); 100 | 101 | /** 102 | * @brief computes the online mean and variance of a grid bin on the z value 103 | * @param[in] prev_mean, previous bin mean value 104 | * @param[in] prev_variance, previous bin varinace value 105 | * @param[in] new_value, new data point to be averaged 106 | * @param[in] seq, number of data points already in the bin 107 | * @returns bin mean and variance 108 | **/ 109 | std::pair computeOnlineMeanVariance(float prev_mean, float prev_variance, float new_value, float seq); 110 | /** 111 | * @brief process the grid coming from a rosbag and map it to the datatypes 112 | * such that the algorithm can be run again 113 | **/ 114 | void processRawGrid(); 115 | }; 116 | } 117 | -------------------------------------------------------------------------------- /safe_landing_planner/include/safe_landing_planner/safe_landing_planner_node.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef DISABLE_SIMULATION 4 | // include simulation 5 | #include "avoidance/rviz_world_loader.h" 6 | #endif 7 | 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 | 24 | #include "safe_landing_planner.hpp" 25 | #include "safe_landing_planner_visualization.hpp" 26 | 27 | namespace avoidance { 28 | 29 | class SafeLandingPlannerNode { 30 | public: 31 | SafeLandingPlannerNode(const ros::NodeHandle& nh); 32 | ~SafeLandingPlannerNode() = default; 33 | 34 | std::unique_ptr safe_landing_planner_; 35 | 36 | #ifndef DISABLE_SIMULATION 37 | std::unique_ptr world_visualizer_; 38 | #endif 39 | 40 | std::atomic should_exit_{false}; 41 | 42 | std::thread worker_; 43 | 44 | /** 45 | * @brief spins node 46 | **/ 47 | void startNode(); 48 | 49 | /** 50 | * @brief threads for transforming pointclouds 51 | **/ 52 | void pointCloudTransformThread(); 53 | 54 | private: 55 | ros::Timer cmdloop_timer_; 56 | 57 | std::unique_ptr cloud_msg_mutex_; 58 | std::unique_ptr transformed_cloud_mutex_; 59 | std::unique_ptr cloud_ready_cv_; 60 | std::unique_ptr cmdloop_spinner_; 61 | ros::CallbackQueue cmdloop_queue_; 62 | 63 | sensor_msgs::PointCloud2 newest_cloud_msg_; 64 | 65 | ros::NodeHandle nh_; 66 | 67 | ros::Publisher mavros_system_status_pub_; 68 | ros::Publisher grid_pub_; 69 | 70 | ros::Subscriber pose_sub_; 71 | ros::Subscriber pointcloud_sub_; 72 | ros::Subscriber raw_grid_sub_; 73 | 74 | tf::TransformListener tf_listener_; 75 | 76 | geometry_msgs::PoseStamped current_pose_; 77 | geometry_msgs::PoseStamped previous_pose_; 78 | mavros_msgs::CompanionProcessStatus status_msg_; 79 | 80 | ros::Time start_time_ = ros::Time(0.0); 81 | ros::Time last_algo_time_ = ros::Time(0.0); 82 | ros::Time t_status_sent_ = ros::Time(0.0); 83 | 84 | SafeLandingPlannerVisualization visualizer_; 85 | 86 | bool position_received_ = false; 87 | bool cloud_transformed_ = false; 88 | double spin_dt_ = 0.1; 89 | 90 | dynamic_reconfigure::Server server_; 91 | safe_landing_planner::SafeLandingPlannerNodeConfig rqt_param_config_; 92 | 93 | /** 94 | * @brief main loop callback 95 | * @param[in] event, event timing information 96 | **/ 97 | void cmdLoopCallback(const ros::TimerEvent& event); 98 | 99 | /** 100 | * @brif callback for vehicle position and orientation 101 | * @param[in] msg, pose message coming fro the FCU 102 | **/ 103 | void positionCallback(const geometry_msgs::PoseStamped& msg); 104 | /** 105 | * @brif callback for pointcloud 106 | * @param[in] msg, current frame pointcloud 107 | **/ 108 | void pointCloudCallback(const sensor_msgs::PointCloud2& msg); 109 | /** 110 | * @brief callaback for parameters dynamic reconfigure server 111 | * @param config, struct with all the parameters 112 | * @param level, bitmsak to group together reconfigurable parameters 113 | **/ 114 | void dynamicReconfigureCallback(safe_landing_planner::SafeLandingPlannerNodeConfig& config, uint32_t level); 115 | /** 116 | * @brief callaback for grid coming from rosbag 117 | * @param msg, SLPGridMsg message 118 | **/ 119 | void rawGridCallback(const safe_landing_planner::SLPGridMsg& msg); 120 | /** 121 | * @brif sends out a status to the FCU which will be received as a 122 | *heartbeat 123 | **/ 124 | void publishSystemStatus(); 125 | 126 | /** 127 | * @brief check healthiness of the avoidance system to trigger failsafe in 128 | *the FCU 129 | * @param[in] since_last_algo, time elapsed since the last iteration of the 130 | *landing site detection algorithm 131 | * @param[in] since_start, time elapsed since staring the node 132 | **/ 133 | void checkFailsafe(ros::Duration since_last_algo, ros::Duration since_start); 134 | 135 | /** 136 | * @brief publishes the computed grid fot the waypoint_generator_node to 137 | *use 138 | **/ 139 | void publishSerialGrid(); 140 | }; 141 | } 142 | -------------------------------------------------------------------------------- /safe_landing_planner/include/safe_landing_planner/safe_landing_planner_visualization.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "safe_landing_planner.hpp" 6 | 7 | namespace avoidance { 8 | 9 | class SafeLandingPlannerVisualization { 10 | public: 11 | SafeLandingPlannerVisualization() = default; 12 | ~SafeLandingPlannerVisualization() = default; 13 | 14 | /** 15 | * @brief initializes all publishers used for local planner visualization 16 | **/ 17 | void initializePublishers(ros::NodeHandle& nh); 18 | 19 | /** 20 | * @brief injects into the SafeLandingPlannerVisualization class, all the data 21 | *to be visualized 22 | * @param[in] planner, SafeLandingPlanner class 23 | * @param[in] pos, current vehicle position 24 | * @param[in] last_pos, previous vehicle position 25 | * @param[in] config, dynamic reconfigure parameters 26 | **/ 27 | void visualizeSafeLandingPlanner(const SafeLandingPlanner& planner, const geometry_msgs::Point& pos, 28 | const geometry_msgs::Point& last_pos, 29 | safe_landing_planner::SafeLandingPlannerNodeConfig& config); 30 | 31 | private: 32 | ros::Publisher local_pointcloud_pub_; 33 | ros::Publisher grid_pub_; 34 | ros::Publisher path_actual_pub_; 35 | ros::Publisher mean_std_dev_pub_; 36 | ros::Publisher counter_pub_; 37 | 38 | int path_length_ = 0; 39 | 40 | /** 41 | * @brief Visualization of the actual path of the drone and the path of 42 | *the waypoint 43 | * @params[in] pos, location of the drone at the last timestep 44 | * @params[in] last_pos, location of the drone at the previous timestep 45 | **/ 46 | void publishPaths(const geometry_msgs::Point& pos, const geometry_msgs::Point& last_pos); 47 | 48 | /** 49 | * @brief Visualization of the boolean land grid 50 | * @params[in] grid, grid data structure 51 | * @params[in] pos, vehicle position 52 | * @param[in] smoothing_size, kernel size on cell land hysteresis 53 | **/ 54 | void publishGrid(const Grid& grid, const geometry_msgs::Point& pos, float smoothing_size) const; 55 | 56 | /** 57 | * @brief Visualization of the mean values grid 58 | * @params[in] grid, grid data structure 59 | * @params[in] std_dev_threshold, maximum standard deviation cell value to land 60 | **/ 61 | void publishMeanStdDev(const Grid& grid, float std_dev_threshold); 62 | 63 | /** 64 | * @brief Visualization of the standard deviation values grid 65 | * @params[in] grid, grid data structure 66 | * @params[in] n_points_threshold, minimum number of points per cell parameter 67 | **/ 68 | void publishCounter(const Grid& grid, float n_points_threshold); 69 | 70 | /** 71 | * @brief converts from HSV to RGB color space 72 | * @param[in] hsv, Hue in range [0, 360], Saturation and Value in range [0, 1] 73 | * @return[out] rgb, Red Blue Green, each elemnt in range [0, 1] 74 | */ 75 | std::tuple HSVtoRGB(std::tuple hsv); 76 | }; 77 | } 78 | -------------------------------------------------------------------------------- /safe_landing_planner/include/safe_landing_planner/waypoint_generator_node.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace avoidance { 21 | 22 | class WaypointGeneratorNode final { 23 | public: 24 | WaypointGeneratorNode(const ros::NodeHandle& nh); 25 | ~WaypointGeneratorNode() = default; 26 | 27 | /** 28 | * @brief spins node 29 | **/ 30 | void startNode(); 31 | 32 | protected: 33 | WaypointGenerator waypointGenerator_; 34 | 35 | ros::NodeHandle nh_; 36 | ros::NodeHandle nh_private_; 37 | 38 | ros::Timer cmdloop_timer_; 39 | std::unique_ptr cmdloop_spinner_; 40 | ros::CallbackQueue cmdloop_queue_; 41 | 42 | ros::Subscriber pose_sub_; 43 | ros::Subscriber trajectory_sub_; 44 | 45 | ros::Subscriber grid_sub_; 46 | ros::Subscriber pos_index_sub_; 47 | ros::Subscriber state_sub_; 48 | 49 | ros::Publisher trajectory_pub_; 50 | ros::Publisher land_hysteresis_pub_; 51 | ros::Publisher marker_goal_pub_; 52 | 53 | bool grid_received_ = false; 54 | double spin_dt_ = 0.1; 55 | Eigen::Vector3f goal_visualization_ = Eigen::Vector3f::Zero(); 56 | 57 | dynamic_reconfigure::Server server_; 58 | 59 | /** 60 | * @brief main loop callback 61 | * @param[in] event, event timing information 62 | **/ 63 | void cmdLoopCallback(const ros::TimerEvent& event); 64 | 65 | /** 66 | * @brief sets parameters from ROS parameter server 67 | * @param config, struct containing all the parameters 68 | * @param level, bitmask to group together reconfigurable parameters 69 | **/ 70 | void dynamicReconfigureCallback(safe_landing_planner::WaypointGeneratorNodeConfig& config, uint32_t level); 71 | 72 | /** 73 | * @brif callback for vehicle position and orientation 74 | * @param[in] msg, pose message coming fro the FCU 75 | **/ 76 | void positionCallback(const geometry_msgs::PoseStamped& msg); 77 | 78 | /** 79 | * @brief callaback for setting the goal from the FCU Mission Waypoints 80 | * @param[in] msg, current and next position goals 81 | **/ 82 | void trajectoryCallback(const mavros_msgs::Trajectory& msg); 83 | 84 | /** 85 | * @brief callaback with the grid calculated by the safe_landing_planner 86 | * @param[in] msg, grid 87 | **/ 88 | void gridCallback(const safe_landing_planner::SLPGridMsg& msg); 89 | 90 | /** 91 | * @brief callaback with the vehicle state 92 | * @param[in] msg, FCU vehicle state 93 | **/ 94 | void stateCallback(const mavros_msgs::State& msg); 95 | 96 | /** 97 | * @brief publishes the computed waypoints to the FCU 98 | * @param[in] pos_sp, position setpoint 99 | * @param[in] vel_sp, velocity setpoint 100 | * @param[in] yaw_sp, yaw setpoint 101 | * @param[in] yaw_speed_sp, yaw speed setpoint 102 | **/ 103 | void publishTrajectorySetpoints(const Eigen::Vector3f& pos_sp, const Eigen::Vector3f& vel_sp, float yaw_sp, 104 | float yaw_speed_sp); 105 | 106 | /** 107 | * @brief fills unused waypoints with NANs 108 | * @param[in] point, unused waypoints 109 | **/ 110 | void fillUnusedTrajectorySetpoints(mavros_msgs::PositionTarget& point); 111 | 112 | /** 113 | * @brief visualize landing area decision grid in Rviz 114 | **/ 115 | void landingAreaVisualization(); 116 | 117 | /** 118 | * @brief visualize goal in Rviz 119 | **/ 120 | void goalVisualization(); 121 | }; 122 | } 123 | -------------------------------------------------------------------------------- /safe_landing_planner/launch/safe_landing_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /safe_landing_planner/launch/safe_landing_planner_launch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /safe_landing_planner/launch/safe_landing_planner_rosbag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /safe_landing_planner/launch/safe_landing_planner_vtol.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /safe_landing_planner/msg/SLPGridMsg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | std_msgs/Float64MultiArray mean 3 | std_msgs/Float64MultiArray std_dev 4 | std_msgs/Int64MultiArray counter 5 | std_msgs/Int64MultiArray land 6 | 7 | float64 grid_size 8 | float64 cell_size 9 | 10 | geometry_msgs/Vector3 curr_pos_index 11 | -------------------------------------------------------------------------------- /safe_landing_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | safe_landing_planner 4 | 0.0.0 5 | Safe landing planner module. 6 | 7 | 8 | 9 | 10 | pixhawk 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | dynamic_reconfigure 45 | message_generation 46 | roscpp 47 | rospy 48 | std_msgs 49 | geometry_msgs 50 | mav_msgs 51 | sensor_msgs 52 | tf 53 | pcl_ros 54 | mavros 55 | mavros_extras 56 | mavros_msgs 57 | avoidance 58 | 59 | dynamic_reconfigure 60 | message_runtime 61 | roscpp 62 | rospy 63 | std_msgs 64 | geometry_msgs 65 | mav_msgs 66 | sensor_msgs 67 | tf 68 | pcl_ros 69 | mavros 70 | mavros_extras 71 | mavros_msgs 72 | avoidance 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /safe_landing_planner/resource/realsense_params.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Disable and enable auto-exposure for all cameras as it does not work at startup 3 | rosrun dynamic_reconfigure dynparam set /camera_down/realsense2_camera_manager rs435_depth_enable_auto_exposure 0 4 | rosrun dynamic_reconfigure dynparam set /camera_down/realsense2_camera_manager rs435_depth_enable_auto_exposure 1 5 | 6 | -------------------------------------------------------------------------------- /safe_landing_planner/resource/rqt_param_toggle.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosrun dynamic_reconfigure dynparam set /camera_main_node depth_range_mode 3 4 | -------------------------------------------------------------------------------- /safe_landing_planner/resource/stereo_calib.json: -------------------------------------------------------------------------------- 1 | { 2 | "aux-param-autoexposure-setpoint": "1536", 3 | "aux-param-colorcorrection1": "0.298828", 4 | "aux-param-colorcorrection10": "0", 5 | "aux-param-colorcorrection11": "0", 6 | "aux-param-colorcorrection12": "0", 7 | "aux-param-colorcorrection2": "0.293945", 8 | "aux-param-colorcorrection3": "0.293945", 9 | "aux-param-colorcorrection4": "0.114258", 10 | "aux-param-colorcorrection5": "0", 11 | "aux-param-colorcorrection6": "0", 12 | "aux-param-colorcorrection7": "0", 13 | "aux-param-colorcorrection8": "0", 14 | "aux-param-colorcorrection9": "0", 15 | "aux-param-depthclampmax": "65534", 16 | "aux-param-depthclampmin": "0", 17 | "aux-param-disparityshift": "0", 18 | "controls-autoexposure-auto": "True", 19 | "controls-autoexposure-manual": "8500", 20 | "controls-color-autoexposure-auto": "True", 21 | "controls-color-autoexposure-manual": "166", 22 | "controls-color-backlight-compensation": "0", 23 | "controls-color-brightness": "0", 24 | "controls-color-contrast": "50", 25 | "controls-color-gain": "64", 26 | "controls-color-gamma": "300", 27 | "controls-color-hue": "0", 28 | "controls-color-power-line-frequency": "3", 29 | "controls-color-saturation": "64", 30 | "controls-color-sharpness": "50", 31 | "controls-color-white-balance-auto": "True", 32 | "controls-color-white-balance-manual": "4600", 33 | "controls-depth-gain": "16", 34 | "controls-laserpower": "150", 35 | "controls-laserstate": "on", 36 | "ignoreSAD": "0", 37 | "param-autoexposure-setpoint": "1536", 38 | "param-censusenablereg-udiameter": "6", 39 | "param-censusenablereg-vdiameter": "7", 40 | "param-censususize": "6", 41 | "param-censusvsize": "7", 42 | "param-depthclampmax": "65534", 43 | "param-depthclampmin": "0", 44 | "param-depthunits": "1000", 45 | "param-disableraucolor": "0", 46 | "param-disablesadcolor": "0", 47 | "param-disablesadnormalize": "0", 48 | "param-disablesloleftcolor": "0", 49 | "param-disableslorightcolor": "0", 50 | "param-disparitymode": "0", 51 | "param-disparityshift": "0", 52 | "param-lambdaad": "630", 53 | "param-lambdacensus": "26", 54 | "param-leftrightthreshold": "18", 55 | "param-maxscorethreshb": "2047", 56 | "param-medianthreshold": "347", 57 | "param-minscorethresha": "80", 58 | "param-neighborthresh": "82", 59 | "param-raumine": "3", 60 | "param-rauminn": "1", 61 | "param-rauminnssum": "6", 62 | "param-raumins": "2", 63 | "param-rauminw": "2", 64 | "param-rauminwesum": "3", 65 | "param-regioncolorthresholdb": "0.0254403", 66 | "param-regioncolorthresholdg": "0.954012", 67 | "param-regioncolorthresholdr": "0.0283757", 68 | "param-regionshrinku": "4", 69 | "param-regionshrinkv": "1", 70 | "param-robbinsmonrodecrement": "15", 71 | "param-robbinsmonroincrement": "5", 72 | "param-rsmdiffthreshold": "3.34375", 73 | "param-rsmrauslodiffthreshold": "0.96875", 74 | "param-rsmremovethreshold": "0.666667", 75 | "param-scanlineedgetaub": "442", 76 | "param-scanlineedgetaug": "16", 77 | "param-scanlineedgetaur": "770", 78 | "param-scanlinep1": "21", 79 | "param-scanlinep1onediscon": "3", 80 | "param-scanlinep1twodiscon": "27", 81 | "param-scanlinep2": "182", 82 | "param-scanlinep2onediscon": "41", 83 | "param-scanlinep2twodiscon": "63", 84 | "param-secondpeakdelta": "205", 85 | "param-texturecountthresh": "0", 86 | "param-texturedifferencethresh": "3474", 87 | "param-usersm": "1", 88 | "param-zunits": "1000", 89 | "stream-depth-format": "Z16", 90 | "stream-fps": "15", 91 | "stream-height": "480", 92 | "stream-width": "640" 93 | } 94 | -------------------------------------------------------------------------------- /safe_landing_planner/src/nodes/safe_landing_planner_node_main.cpp: -------------------------------------------------------------------------------- 1 | #include "safe_landing_planner/safe_landing_planner_node.hpp" 2 | 3 | int main(int argc, char **argv) { 4 | using namespace avoidance; 5 | ros::init(argc, argv, "safe_landing_planner_node"); 6 | ros::NodeHandle nh("~"); 7 | SafeLandingPlannerNode NodeSLP(nh); 8 | NodeSLP.startNode(); 9 | 10 | NodeSLP.worker_.join(); 11 | 12 | return 0; 13 | } 14 | -------------------------------------------------------------------------------- /safe_landing_planner/src/nodes/waypoint_generator.cpp.dot: -------------------------------------------------------------------------------- 1 | digraph { 2 | "GOTO" -> "ALTITUDE_CHANGE" [label="NEXT1", style="solid", weight=1] 3 | "GOTO" -> "GOTO" [label="ERROR", style="dotted", weight=0.1] 4 | "ALTITUDE_CHANGE" -> "LOITER" [label="NEXT1", style="solid", weight=1] 5 | "ALTITUDE_CHANGE" -> "GOTO" [label="ERROR", style="dotted", weight=0.1] 6 | "LOITER" -> "EVALUATE_GRID" [label="NEXT1", style="solid", weight=1] 7 | "LOITER" -> "GOTO" [label="ERROR", style="dotted", weight=0.1] 8 | "EVALUATE_GRID" -> "GOTO" [label="NEXT1\nERROR", style="solid", weight=1] 9 | "EVALUATE_GRID" -> "GOTO_LAND" [label="NEXT2", style="solid", weight=1] 10 | "GOTO_LAND" -> "LAND" [label="NEXT1", style="solid", weight=1] 11 | "GOTO_LAND" -> "GOTO" [label="ERROR", style="dotted", weight=0.1] 12 | "LAND" -> "GOTO" [label="ERROR", style="dotted", weight=0.1] 13 | } -------------------------------------------------------------------------------- /safe_landing_planner/test/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char **argv) { 6 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error); 7 | ::testing::InitGoogleTest(&argc, argv); 8 | ros::init(argc, argv, "test"); 9 | return RUN_ALL_TESTS(); 10 | } 11 | -------------------------------------------------------------------------------- /safe_landing_planner/test/test_grid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "../include/safe_landing_planner/grid.hpp" 4 | 5 | using namespace avoidance; 6 | 7 | TEST(GridTest, gridCellSize) { 8 | Grid grid = Grid(6.f, 2.f); 9 | Eigen::Vector3f pos(1.2f, 3.4f, 2.f); 10 | grid.setFilterLimits(pos); 11 | Eigen::Vector2f limit_min, limit_max; 12 | grid.getGridLimits(limit_min, limit_max); 13 | 14 | EXPECT_FLOAT_EQ(-1.8f, limit_min.x()); 15 | EXPECT_FLOAT_EQ(0.4f, limit_min.y()); 16 | EXPECT_FLOAT_EQ(4.2f, limit_max.x()); 17 | EXPECT_FLOAT_EQ(6.4f, limit_max.y()); 18 | } 19 | 20 | TEST(GridTest, gridCellSize2) { 21 | Grid grid = Grid(12.f, 3.f); 22 | Eigen::Vector3f pos(-3.2f, 5.4f, 2.f); 23 | grid.setFilterLimits(pos); 24 | Eigen::Vector2f limit_min, limit_max; 25 | grid.getGridLimits(limit_min, limit_max); 26 | 27 | EXPECT_FLOAT_EQ(-9.2f, limit_min.x()); 28 | EXPECT_FLOAT_EQ(-0.6f, limit_min.y()); 29 | EXPECT_FLOAT_EQ(2.8f, limit_max.x()); 30 | EXPECT_FLOAT_EQ(11.4f, limit_max.y()); 31 | } 32 | -------------------------------------------------------------------------------- /tools/check_code_format.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Fix style recursively in all the repository 4 | sh fix_style.sh .. 5 | 6 | # Print the diff with the remote branch (empty if no diff) 7 | git --no-pager diff -U0 --color 8 | 9 | # Check if there are changes, and failed 10 | if ! git diff-index --quiet HEAD --; then echo "Code style check failed, please run clang-format (e.g. with tools/fix_style.sh)"; exit 1; fi 11 | -------------------------------------------------------------------------------- /tools/check_state_machine_diagrams.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Generate the existing state machine diagrams again 4 | python generate_flow_diagram.py ../avoidance/test/test_usm.cpp 5 | python generate_flow_diagram.py ../safe_landing_planner/src/nodes/waypoint_generator.cpp 6 | python generate_flow_diagram.py ../local_planner/src/nodes/waypoint_generator.cpp 7 | 8 | # Print the diff with the remote branch (empty if no diff) 9 | git --no-pager diff -U0 --color 10 | 11 | # Check if there are changes, and failed 12 | if ! git diff-index --quiet HEAD --; then echo "State machine diagram generation not up to date, run tools/check_state_machine_diagrams.sh"; exit 1; fi 13 | -------------------------------------------------------------------------------- /tools/fix_style.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | STYLE="google" 4 | 5 | if [ "$#" -eq 0 ]; then 6 | echo "Usage: $0 " 7 | echo "" 8 | echo "ERROR: At least one source file or one directory must be provided!" 9 | 10 | exit 1 11 | fi 12 | 13 | for arg in "$@" 14 | do 15 | if [ -f $arg ]; then 16 | clang-format-3.8 -i -style='{BasedOnStyle: google, ColumnLimit: 120}' $arg 17 | elif [ -d $arg ]; then 18 | find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs clang-format-3.8 -i -style='{BasedOnStyle: google, ColumnLimit: 120}' 19 | find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs chmod 644 20 | fi 21 | done 22 | -------------------------------------------------------------------------------- /tools/generate_coverage.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | catkin clean -y 3 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Coverage -DCATKIN_ENABLE_TESTING=True 4 | catkin build avoidance local_planner global_planner safe_landing_planner 5 | catkin build avoidance --no-deps --catkin-make-args avoidance-test_coverage 6 | catkin build local_planner --no-deps --catkin-make-args local_planner-test_coverage 7 | catkin build global_planner --no-deps --catkin-make-args global_planner-test_coverage 8 | catkin build safe_landing_planner --no-deps --catkin-make-args safe_landing_planner-test_coverage 9 | # disable branch coverage, since it isn't useful until the line coverage is better 10 | # lcov -a ~/catkin_ws/build/avoidance/coverage.info -a ~/catkin_ws/build/local_planner/coverage.info -a ~/catkin_ws/build/global_planner/coverage.info -a ~/catkin_ws/build/safe_landing_planner/coverage.info -o repo_total.info --rc lcov_branch_coverage=1 11 | lcov -a ~/catkin_ws/build/avoidance/coverage.info -a ~/catkin_ws/build/local_planner/coverage.info -a ~/catkin_ws/build/global_planner/coverage.info -a ~/catkin_ws/build/safe_landing_planner/coverage.info -o repo_total.info 12 | sed -i "s/$(pwd | sed 's/\//\\\//g')\///g" repo_total.info 13 | -------------------------------------------------------------------------------- /tools/generate_coverage_html.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | bash tools/generate_coverage.sh 4 | 5 | genhtml repo_total.info --output-directory coverage_html --branch-coverage 6 | x-www-browser coverage_html/index.html 7 | -------------------------------------------------------------------------------- /tools/set_up_commit_hooks.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # If run from the tools directory, fail 4 | if [ ! -d .git ] ; then 5 | echo "Commit hooks not installed, run from main directory! (./tools/set_up_commit_hooks)" 6 | exit 1 7 | fi 8 | 9 | #If there are already commit hooks installed 10 | if [ -f .git/hooks/pre-commit ] ; then 11 | echo "You already have commit hooks, manually delete them and run again!" 12 | exit 1 13 | fi 14 | 15 | cat >> .git/hooks/pre-commit <<- EOM 16 | #!/bin/bash 17 | # 18 | # An example hook script to verify what is about to be committed. 19 | # Called by "git commit" with no arguments. The hook should 20 | # exit with non-zero status after issuing an appropriate message if 21 | # it wants to stop the commit. 22 | #!/bin/bash 23 | 24 | STYLE="google" 25 | 26 | format_file() { 27 | file="${1}" 28 | clang-format-3.8 -i -style=${STYLE} ${1} 29 | git add ${1} 30 | } 31 | 32 | case "${1}" in 33 | --about ) 34 | echo "Runs clang-format on source files" 35 | ;; 36 | * ) 37 | for file in `git diff-index --cached --name-only HEAD` ; do 38 | if [[ ${file} == *".cpp" ]] || [[ ${file} == *".h" ]] || [[ ${file} == *".hpp" ]]; then 39 | echo 40 | format_file "${file}" 41 | fi 42 | done 43 | ;; 44 | esac 45 | EOM 46 | echo "Commit hooks successfully installed!" 47 | exit 0 48 | --------------------------------------------------------------------------------