├── .gitignore ├── .gitmodules ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── FindGLM.cmake └── Findrealsense.cmake ├── include └── coav.hh.in ├── samples ├── CMakeLists.txt ├── coav_sample_app.cc ├── detector_sample.cc └── polarhist_sample.cc ├── simulation └── gazebo-models │ ├── CMakeLists.txt │ └── gzsitl_quadcopter_rs │ ├── model.config │ └── model.sdf ├── src ├── avoidance │ ├── Avoidance.hh │ ├── CMakeLists.txt │ ├── QuadCopterShiftAvoidance.cc │ ├── QuadCopterShiftAvoidance.hh │ ├── QuadCopterStopAvoidance.cc │ ├── QuadCopterStopAvoidance.hh │ ├── QuadCopterVFFAvoidance.cc │ └── QuadCopterVFFAvoidance.hh ├── common │ ├── CMakeLists.txt │ ├── common.cc │ ├── common.hh │ ├── math.cc │ └── math.hh ├── detection │ ├── CMakeLists.txt │ ├── DepthImageObstacleDetector.cc │ ├── DepthImageObstacleDetector.hh │ ├── DepthImagePolarHistDetector.cc │ ├── DepthImagePolarHistDetector.hh │ └── Detectors.hh ├── sensors │ ├── CMakeLists.txt │ ├── GazeboRealSenseCamera.cc │ ├── GazeboRealSenseCamera.hh │ ├── RealSenseCamera.cc │ ├── RealSenseCamera.hh │ ├── Sensors.cc │ └── Sensors.hh └── vehicles │ ├── CMakeLists.txt │ ├── MavQuadCopter.cc │ ├── MavQuadCopter.hh │ └── Vehicles.hh ├── testbed ├── coav-sim.sh ├── detect_collision.py ├── detect_takeoff.py ├── eeprom.bin ├── simple_mission.mission ├── terrain │ └── S36E149.DAT ├── testbed.sh └── worlds │ ├── models │ ├── L-wall │ │ ├── model.config │ │ └── model.sdf │ └── office_simple │ │ ├── model.config │ │ └── model.sdf │ ├── simple.sdf │ ├── simple_obstacle.sdf │ └── simple_office.sdf └── tools └── coav-control ├── CMakeLists.txt ├── coav-control.cc ├── coav-control.hh ├── coav-control.service.in ├── coav-control.sh.in ├── parser.cc ├── visual.cc ├── visual.hh ├── visual_depth.cc └── visual_env.cc /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | testbed/logs/ 3 | testbed/output/ 4 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "modules/mavlink_vehicles"] 2 | path = modules/mavlink_vehicles 3 | url = https://github.com/01org/mavlink-vehicles 4 | branch = master 5 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | dist: trusty 3 | sudo: required 4 | cache: pip 5 | matrix: 6 | include: 7 | - os: linux 8 | addons: 9 | apt: 10 | sources: 11 | - ubuntu-toolchain-r-test 12 | packages: 13 | - g++-6 14 | - gcc-6 15 | - python-pip 16 | - cmake 17 | - pkg-config 18 | - libusb-1.0 19 | - libglm-dev 20 | 21 | before_script: 22 | - pip install --user future pymavlink 23 | - sudo add-apt-repository http://packages.ros.org/ros/ubuntu && sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net --recv-key 0xB01FA116 24 | - sudo apt-get update 25 | - sudo apt -y install ros-indigo-librealsense 26 | - git submodule update --init --recursive 27 | - CXX="g++-6" 28 | - CC="gcc-6" 29 | 30 | script: 31 | - mkdir build && cd build && cmake -DREALSENSE_INCLUDE_DIR=/opt/ros/indigo/include/ -DREALSENSE_LIBRARY=/opt/ros/indigo/lib/librealsense.so .. && make -j 32 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | project(COAV VERSION 0.0.1) 3 | 4 | # Set add REQUIRED list to DEP_VAR if FEATURE is ON 5 | function(option_deps DEP_VAR FEATURE REQUIRED) 6 | if (${FEATURE}) 7 | set(${DEP_VAR} ${${DEP_VAR}} ${REQUIRED} ${ARGN} PARENT_SCOPE) 8 | endif() 9 | endfunction() 10 | 11 | # Add option dependency 12 | macro(dependent_option option doc default dependency) 13 | if (${dependency}) 14 | set(enable_${option} true) 15 | endif() 16 | 17 | if (DEFINED enable_${option}) 18 | option(${option} ${doc} ${default}) 19 | else() 20 | set(${option} OFF) 21 | message(WARNING "Option ${option} ignored. Missing required option ${dependency}") 22 | endif() 23 | endmacro() 24 | 25 | # Generate an inclusion list 26 | function(export_headers HEADERS_LIST COMP_NAME) 27 | set(INCLUDE_LIST "\n//${COMP_NAME}\n") 28 | 29 | foreach(HEADER ${HEADERS_LIST}) 30 | set(INCLUDE_LIST "${INCLUDE_LIST}#include \"${HEADER}\"\n") 31 | file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/${HEADER} 32 | DESTINATION ${CMAKE_BINARY_DIR}/include/coav) 33 | endforeach() 34 | 35 | set(INCLUDE_LIST ${INCLUDE_LIST} PARENT_SCOPE) 36 | endfunction() 37 | 38 | # Build Options 39 | option(WITH_REALSENSE "Add realsense support" ON) 40 | option(WITH_SAMPLES "Compile project samples" OFF) 41 | option(WITH_GAZEBO "Enable Gazebo simulation features" OFF) 42 | option(WITH_TOOLS "Enable library tools" OFF) 43 | option(WITH_VDEBUG "Compile visual debugger" OFF) 44 | 45 | # Options definitions and custom setup 46 | if (${WITH_REALSENSE}) 47 | add_definitions(-DHAVE_REALSENSE) 48 | endif() 49 | 50 | if (${WITH_GAZEBO}) 51 | add_definitions(-DHAVE_GAZEBO) 52 | endif() 53 | 54 | if (${WITH_VDEBUG}) 55 | add_definitions(-DWITH_VDEBUG) 56 | endif() 57 | 58 | # Check if submodules have been initialized 59 | if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/modules/mavlink_vehicles/CMakeLists.txt) 60 | message(SEND_ERROR "mavlink_vehicles submodule not found") 61 | return() 62 | endif() 63 | 64 | # Support Yocto SDK 65 | if(DEFINED ENV{SDKTARGETSYSROOT}) 66 | message(STATUS "Using Yocto Environment to build") 67 | set(CMAKE_FIND_ROOT_PATH $ENV{SDKTARGETSYSROOT}) 68 | endif() 69 | 70 | #List required packages for each build option 71 | set(DEPS "GLM") 72 | option_deps(DEPS WITH_GAZEBO "gazebo") 73 | option_deps(DEPS WITH_REALSENSE "realsense") 74 | option_deps(DEPS WITH_VDEBUG "gazebo" "OpenGL" "GLUT") 75 | list(REMOVE_DUPLICATES DEPS) 76 | 77 | # Add module path 78 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 79 | 80 | # Find required packages 81 | foreach(dep ${DEPS}) 82 | find_package(${dep} REQUIRED) 83 | endforeach() 84 | 85 | # Compiler flags 86 | set(CMAKE_CXX_STANDARD 11) 87 | set(CUSTOM_COMPILE_FLAGS "${CUSTOM_COMPILE_FLAGS} -Wall") 88 | set(CFLAGS "${CFLAGS} -fstack-protector-all -fPIE -fPIC -O2 -D_FORTIFY_SOURCE=2 -pthread") 89 | set(CFLAGS "${CFLAGS} -Wformat -Wformat-security") 90 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CFLAGS}") 91 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CUSTOM_COMPILE_FLAGS}") 92 | set(CMAKE_STATIC_LINKER_FLAGS "${LDFLAGS}") 93 | set(CMAKE_SHARED_LINKER_FLAGS "${LDFLAGS} -z noexecstack -z relro -z now") 94 | set(CMAKE_EXE_LINKER_FLAGS "${LDFLAGS} -z noexecstack -z relro -z now") 95 | 96 | include(GNUInstallDirs) 97 | 98 | # Include and Link directories 99 | include_directories(${PROJECT_SOURCE_DIR} 100 | ${CMAKE_BINARY_DIR}/include 101 | modules/mavlink_vehicles/src 102 | src) 103 | 104 | if (${WITH_GAZEBO}) 105 | # Custom installation paths 106 | # TODO: Gazebo 7 didn't provide a ${GAZEBO_MODEL_PATH} variable. 107 | # That's why we are accessing it relatively to ${GAZEBO_MEDIA_PATH}. 108 | # A proper ${GAZEBO_MODEL_PATH} variable might be available in Gazebo 8+. 109 | if (COAVLIB_MODEL_INSTALL_PATH) 110 | GET_FILENAME_COMPONENT(COAVLIB_MODEL_PATH ${COAVLIB_MODEL_INSTALL_PATH} ABSOLUTE) 111 | elseif(GAZEBO_MODEL_PATH) 112 | set(COAVLIB_MODEL_PATH ${GAZEBO_MODEL_PATH}) 113 | else() 114 | GET_FILENAME_COMPONENT(COAVLIB_MODEL_PATH ${GAZEBO_MEDIA_PATH}/../models ABSOLUTE) 115 | endif() 116 | 117 | # Include and Link directories 118 | include_directories(${GAZEBO_INCLUDE_DIRS}) 119 | link_directories(${GAZEBO_LIBRARY_DIRS}) 120 | set(LIBRARIES ${LIBRARIES} ${GAZEBO_LIBRARIES}) 121 | endif() 122 | 123 | if (${WITH_REALSENSE}) 124 | include_directories(${REALSENSE_INCLUDE_DIRS}) 125 | set(LIBRARIES ${LIBRARIES} ${REALSENSE_LIBRARIES}) 126 | endif() 127 | 128 | list(APPEND PUBLIC_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/include/coav.hh) 129 | 130 | # Add subdirectories 131 | add_subdirectory(modules/mavlink_vehicles) 132 | add_subdirectory(src/avoidance) 133 | add_subdirectory(src/common) 134 | add_subdirectory(src/detection) 135 | add_subdirectory(src/sensors) 136 | add_subdirectory(src/vehicles) 137 | 138 | # Add library 139 | add_library(coav SHARED 140 | $ 141 | $ 142 | $ 143 | $ 144 | $) 145 | 146 | set_target_properties(coav PROPERTIES 147 | VERSION ${PROJECT_VERSION} 148 | SOVERSION ${PROJECT_VERSION_MAJOR}) 149 | 150 | target_link_libraries(coav 151 | PUBLIC ${LIBRARIES} 152 | PRIVATE mavlink_vehicles) 153 | 154 | install(TARGETS coav DESTINATION ${CMAKE_INSTALL_LIBDIR}) 155 | 156 | configure_file(include/coav.hh.in include/coav/coav.hh) 157 | install(DIRECTORY ${CMAKE_BINARY_DIR}/include/ 158 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 159 | 160 | # Extra targets 161 | if (${WITH_TOOLS}) 162 | add_subdirectory(tools/coav-control) 163 | endif() 164 | 165 | if (${WITH_SAMPLES}) 166 | add_subdirectory(samples) 167 | endif() 168 | 169 | if (${WITH_GAZEBO}) 170 | add_subdirectory(simulation/gazebo-models) 171 | endif() 172 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | 3 | Version 2.0, January 2004 4 | 5 | http://www.apache.org/licenses/ 6 | 7 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 8 | 9 | 1. Definitions. 10 | 11 | "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. 16 | 17 | "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. 18 | 19 | "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. 20 | 21 | "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. 22 | 23 | "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). 24 | 25 | "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. 26 | 27 | "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." 28 | 29 | "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 30 | 31 | 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 32 | 33 | 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 34 | 35 | 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: 36 | 37 | You must give any other recipients of the Work or Derivative Works a copy of this License; and 38 | You must cause any modified files to carry prominent notices stating that You changed the files; and 39 | You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and 40 | If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. 41 | 42 | You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 43 | 44 | 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 45 | 46 | 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 47 | 48 | 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 49 | 50 | 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 51 | 52 | 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. 53 | 54 | END OF TERMS AND CONDITIONS 55 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | DISCONTINUATION OF PROJECT. 2 | 3 | This project will no longer be maintained by Intel. 4 | 5 | Intel has ceased development and contributions including, but not limited to, maintenance, bug fixes, new releases, or updates, to this project. 6 | 7 | Intel no longer accepts patches to this project. 8 | 9 | If you have an ongoing need to use this project, are interested in independently developing it, or would like to maintain patches for the open source software community, please create your own fork of this project. 10 | # Collision Avoidance Library # 11 | 12 | [![Build Status](https://travis-ci.org/01org/collision-avoidance-library.svg?branch=master)](https://travis-ci.org/01org/collision-avoidance-library) 13 | Coverity Scan Build Status 15 | 16 | 17 | A framework for testing and benchmarking collision avoidance strategies. 18 | 19 | ## Requirements ## 20 | 21 | * CMake 3.1 or newer 22 | * Gazebo 8.0+ (for virtual camera/vehicle support) 23 | * socat 1.7+ (for testbed support) 24 | * GZSitl (for virtual vehicle support) 25 | * GLM (https://github.com/g-truc/glm.git) 26 | * Autopilot, either: 27 | * Ardupilot (https://github.com/ArduPilot/ardupilot) or 28 | * PX4 (https://github.com/PX4/Firmware) 29 | 30 | Collision Avoidance Library (Coav) is developed having drones in mind, so when compiling 31 | the library without additional options, features related to benchmark and simulation 32 | will be OFF by default. This should be the preferred way when you want to ship 33 | the library on your drone target/product. 34 | 35 | ### Library Features and Options ### 36 | 37 | Collision Avoidance Library has support to following features that can be defined 38 | on compile time: 39 | 40 | Feature/Option | Compile Options | Default Value 41 | ----------------------- | --------------- | ------------- 42 | Intel RealSense support | WITH_REALSENSE | ON 43 | Gazebo support | WITH_GAZEBO | OFF 44 | Visual Debugger support | WITH_VDEBUG | OFF (depends on Gazebo) 45 | Coav Tools | WITH_TOOLS | OFF 46 | Compile code samples | WITH_SAMPLES | OFF 47 | 48 | ## Build and Install ## 49 | 50 | ### Method 1 - Embed 'coav-control' on an Intel Aero image ### 51 | 52 | This method is recommended for those who want to use 'coav-control' on an Intel 53 | Aero Drone. 54 | 55 | A Yocto layer containing recipes to build and install coav-control can be found on 56 | the repository under the folder 'meta-coav'. This layer can be easily added to 57 | to the image build by following the steps described by Intel Aero documentation 58 | [here](https://github.com/intel-aero/meta-intel-aero/wiki/Developing-on-Intel-Aero). 59 | 60 | The recipe install the 'coav-control' utility tool as well an init script that runs 61 | the tool on start-up. You can change it's behavior by editing the script file at 62 | any point of the process that seems convenient to you (custom branch, custom recipe 63 | or changing the file on the drone itself). 64 | 65 | ### Method 2 - Compile and Install yourself ### 66 | 67 | This method is recommended for those who will run simulations and tests on the 68 | development environment instead of a real drone. It is also recommend for those 69 | actively writing the library code because makes it easier to switch binaries 70 | for tests during development. If targeting an Intel Aero drone, check [additional 71 | instructions](#deploying-on-intel-aero) about taking advantage of Yocto's SDK support. 72 | 73 | If you're using Ubuntu, before continuing please ensure you have the needed dependencies: 74 | * If you want to use Gazebo, ensure you go through the instructions available [here](http://gazebosim.org/tutorials?tut=install_ubuntu) and ensure you install the libgazebo8-dev package; 75 | * Install all build dependencies (the last two are needed to build librealsense): 76 | 77 | ``` 78 | sudo apt-get install git cmake libglm-dev python-future doxygen libusb-1.0-0-dev libglfw3-dev 79 | ``` 80 | 81 | * Go through the steps to install librealsense (it does not support librealsense2) which can be found [here](https://github.com/IntelRealSense/librealsense/blob/legacy/doc/installation.md) 82 | 83 | The project use CMake as build system and does not support in-tree build. 84 | As such, create a separate folder before building. 85 | 86 | 1. Make sure you have initialized and updated all the required submodules at 87 | least once with: 88 | 89 | ``` 90 | git submodule update --init --recursive 91 | ``` 92 | 93 | 2. Create a "build" folder and build the library using CMake as follows: 94 | 95 | ``` 96 | mkdir build 97 | cd build 98 | cmake .. 99 | sudo make install 100 | cd - 101 | ``` 102 | These instructions will build and install the targets on cmake's 103 | default install path (usually '/usr/local'). To modify the library options, 104 | the following syntax is used when issuing `cmake`: 105 | 106 | ``` 107 | cmake .. -D= -D= 108 | ``` 109 | 110 | Also, the following CMake options may be of value: 111 | 112 | Option | Description 113 | --- | --- 114 | CMAKE_INSTALL_PREFIX | Set a custom installation path. This path is also used for dependency search. 115 | CMAKE_PREFIX_PATH | Add paths to be searched when looking for dependencies 116 | 117 | A more complete explanation of those options can be found on CMake's Documentation. 118 | 119 | Example: 120 | 121 | * Search GLM and Mavlink on 122 | * Change the install path to 123 | * Compile the library additional tools (coav-sim) 124 | 125 | ``` 126 | cmake .. -DCMAKE_INSTALL_PREFIX= -DCMAKE_PREFIX_PATH= -DWITH_TOOLS=ON 127 | ``` 128 | 129 | ## Testing Collision Avoidance Library with *coav-control* ## 130 | 131 | Make sure that the library was compiled with 'Coav Tools' turned on. This will 132 | build a target `coav-control` that can be found in 'tools/coav-control/' inside 133 | the build folder. 134 | 135 | `coav-control` can be used execute a simple collision avoidance system for a 136 | Mavlink controlled Quadcopter that is composed by: a sensor, a detection algorithm 137 | and a detection strategy. It needs interaction with an autopilot and a sensor to work, 138 | so it won't do much when executed alone. 139 | 140 | The following will list the possible options for each component: 141 | 142 | ``` 143 | ./coav-control --help 144 | ``` 145 | 146 | Example: 147 | 148 | Run a collision avoidance system composed by: 149 | * Intel Realsense 150 | * Obstacle detector based on 'Blob extraction' 151 | * 'Stop' avoidance strategy 152 | 153 | ``` 154 | ./coav-control -d DI_OBSTACLE -a QC_STOP -s ST_REALSENSE 155 | ``` 156 | 157 | ## Simulation and Automated tests ## 158 | 159 | For information on how to make use of 'Collision Avoidance Library' on simulated 160 | environment and how to take advantage of tests automation via testbed, please 161 | refer to the [Simulation Docs](https://github.com/01org/collision-avoidance-library/wiki/Quickstart#running-the-simulation). 162 | 163 | ## Deploying on Intel Aero ## 164 | 165 | Intel Aero firmware is based on Yocto, so the Yocto SDK for Intel Aero will be 166 | used to properly compile Collision Avoidance Library for deploy on Intel Aero. 167 | 168 | Instruction on how to build Intel Aero image and the associated SDK can be found 169 | on Intel Aero [Wiki](https://github.com/intel-aero/meta-intel-aero/wiki). 170 | 171 | Intel Aero SDK will be missing one of the Collision Avoidance Library 172 | dependencies: 173 | * GLM 174 | 175 | Since GLM is a "headers only" library, cmake just need to know where to find 176 | the headers in order to successfully "cross-compile" it. This will be 177 | done with "-DCMAKE_PREFIX_PATH" parameter as described by the instructions 178 | bellow. 179 | 180 | Once Intel Aero SDK is successfully installed, the following instructions will 181 | configure the environment and compile the library: 182 | 183 | source /environment-setup-core2-64-poky-linux 184 | 185 | mkdir build 186 | cd build 187 | cmake .. -DCMAKE_PREFIX_PATH="" 188 | make 189 | 190 | After a successful build, you can install Collision Avoidance Library in a 191 | temporary path: 192 | 193 | make install DESTDIR= 194 | 195 | Pack everything: 196 | 197 | cd 198 | tar cvf coav.tar * 199 | 200 | Copy coav.tar to Intel Aero root dir and execute the following on Intel Aero: 201 | 202 | [intel-aero]$ cd / 203 | [intel-aero]$ tar xvf coav.tar 204 | 205 | And Collision Avoidance Library should be successfully installed! 206 | -------------------------------------------------------------------------------- /cmake/FindGLM.cmake: -------------------------------------------------------------------------------- 1 | # FindGLM - attempts to locate the glm matrix/vector library. 2 | # 3 | # This module defines the following variables (on success): 4 | # GLM_INCLUDE_DIRS - where to find glm/glm.hpp 5 | # GLM_FOUND - if the library was successfully located 6 | # 7 | # It is trying a few standard installation locations, but can be customized 8 | # with the following variables: 9 | # GLM_ROOT_DIR - root directory of a glm installation 10 | # Headers are expected to be found in either: 11 | # /glm/glm.hpp OR 12 | # /include/glm/glm.hpp 13 | # This variable can either be a cmake or environment 14 | # variable. Note however that changing the value 15 | # of the environment varible will NOT result in 16 | # re-running the header search and therefore NOT 17 | # adjust the variables set by this module. 18 | 19 | #============================================================================= 20 | # Copyright 2012 Carsten Neumann 21 | # 22 | # Distributed under the OSI-approved BSD License (the "License"); 23 | # see accompanying file Copyright.txt for details. 24 | # 25 | # This software is distributed WITHOUT ANY WARRANTY; without even the 26 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 27 | # See the License for more information. 28 | #============================================================================= 29 | # (To distribute this file outside of CMake, substitute the full 30 | # License text for the above reference.) 31 | 32 | # default search dirs 33 | SET(_glm_HEADER_SEARCH_DIRS 34 | "/usr/include" 35 | "/usr/local/include") 36 | 37 | # check environment variable 38 | SET(_glm_ENV_ROOT_DIR "$ENV{GLM_ROOT_DIR}") 39 | 40 | IF(NOT GLM_ROOT_DIR AND _glm_ENV_ROOT_DIR) 41 | SET(GLM_ROOT_DIR "${_glm_ENV_ROOT_DIR}") 42 | ENDIF(NOT GLM_ROOT_DIR AND _glm_ENV_ROOT_DIR) 43 | 44 | # put user specified location at beginning of search 45 | IF(GLM_ROOT_DIR) 46 | SET(_glm_HEADER_SEARCH_DIRS "${GLM_ROOT_DIR}" 47 | "${GLM_ROOT_DIR}/include" 48 | ${_glm_HEADER_SEARCH_DIRS}) 49 | ENDIF(GLM_ROOT_DIR) 50 | 51 | # locate header 52 | FIND_PATH(GLM_INCLUDE_DIR "glm/glm.hpp" 53 | PATHS ${_glm_HEADER_SEARCH_DIRS}) 54 | 55 | INCLUDE(FindPackageHandleStandardArgs) 56 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(GLM DEFAULT_MSG 57 | GLM_INCLUDE_DIR) 58 | 59 | IF(GLM_FOUND) 60 | add_definitions(-DGLM_ENABLE_EXPERIMENTAL) 61 | include_directories(${GLM_INCLUDE_DIR}) 62 | MESSAGE(STATUS "GLM_INCLUDE_DIR = ${GLM_INCLUDE_DIR}") 63 | ENDIF(GLM_FOUND) 64 | -------------------------------------------------------------------------------- /cmake/Findrealsense.cmake: -------------------------------------------------------------------------------- 1 | # Try to find realsense library 2 | # Once done this will define 3 | # REALSENSE_FOUND - if system found realsense library 4 | # REALSENSE_INCLUDE_DIRS - The realsense include directories 5 | # REALSENSE_LIBRARIES - The libraries needed to use realsense 6 | 7 | find_path(REALSENSE_INCLUDE_DIR "librealsense/rs.h") 8 | find_library(REALSENSE_LIBRARY "librealsense.so") 9 | 10 | include(FindPackageHandleStandardArgs) 11 | find_package_handle_standard_args(realsense DEFAULT_MSG REALSENSE_INCLUDE_DIR REALSENSE_LIBRARY) 12 | 13 | if(REALSENSE_FOUND) 14 | set(REALSENSE_INCLUDE_DIRS "${REALSENSE_INCLUDE_DIR}") 15 | message(STATUS "REALSENSE_INCLUDE_DIR = ${REALSENSE_INCLUDE_DIR}") 16 | set(REALSENSE_LIBRARIES "${REALSENSE_LIBRARY}") 17 | message(STATUS "REALSENSE_LIBRARY = ${REALSENSE_LIBRARY}") 18 | endif() 19 | -------------------------------------------------------------------------------- /include/coav.hh.in: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Intel Corporation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | /* Auto-generated file **DO NOT EDIT** */ 18 | 19 | #pragma once 20 | @COAV_INCLUDE_LIST@ 21 | -------------------------------------------------------------------------------- /samples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | 3 | if (${WITH_GAZEBO}) 4 | add_executable(coav_sample_app coav_sample_app.cc) 5 | target_link_libraries(coav_sample_app coav) 6 | endif() 7 | 8 | if (${WITH_REALSENSE}) 9 | add_executable(detector_sample detector_sample.cc) 10 | target_link_libraries(detector_sample coav) 11 | 12 | add_executable(polarhist_sample polarhist_sample.cc) 13 | target_link_libraries(polarhist_sample coav) 14 | endif() 15 | -------------------------------------------------------------------------------- /samples/coav_sample_app.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | int main(int argc, char **argv) 24 | { 25 | // Initialize Depth Camera 26 | std::shared_ptr depth_camera = 27 | std::make_shared(); 28 | 29 | // Initialize Vehicle 30 | std::shared_ptr vehicle = std::make_shared(); 31 | 32 | // Initialize Detector 33 | std::shared_ptr obstacle_detector = 34 | std::make_shared(); 35 | 36 | // Initialize Avoidance Strategy 37 | std::shared_ptr avoidance = 38 | std::make_shared(vehicle); 39 | 40 | while (true) { 41 | 42 | // Sense and avoid 43 | auto sensed_elements = obstacle_detector->detect(depth_camera->read()); 44 | 45 | // Avoid 46 | avoidance->avoid(sensed_elements); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /samples/detector_sample.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | using namespace std; 26 | 27 | class DepthImageSimpleDetector : public Detector 28 | { 29 | public: 30 | DepthImageSimpleDetector(double threshold_m = 1.5); 31 | 32 | const vector &detect( 33 | shared_ptr data) override; 34 | 35 | private: 36 | vector obstacles = {}; 37 | std::shared_ptr sensor; 38 | double threshold; 39 | }; 40 | 41 | DepthImageSimpleDetector::DepthImageSimpleDetector( 42 | double threshold_m) 43 | { 44 | this->threshold = threshold_m; 45 | } 46 | 47 | const vector &DepthImageSimpleDetector::detect( 48 | shared_ptr data) 49 | { 50 | shared_ptr depth_data = static_pointer_cast(data); 51 | // Obtain camera depth buffer and camera properties 52 | vector depth_buffer = depth_data->depth_buffer; 53 | 54 | unsigned int height = depth_data->height; 55 | unsigned int width = depth_data->width; 56 | 57 | double scale = depth_data->scale; 58 | 59 | double hfov = depth_data->hfov; 60 | double vfov = depth_data->vfov; 61 | double base_phi = (M_PI - hfov) / 2; 62 | double base_theta = (M_PI - vfov) / 2; 63 | 64 | this->obstacles.clear(); 65 | 66 | // Return if depth buffer is empty 67 | if(depth_buffer.size() == 0) { 68 | return this->obstacles; 69 | } 70 | 71 | // Detect "obstacles" in the image 72 | unsigned int min = UINT16_MAX; 73 | unsigned int min_i = 0, min_j = 0; 74 | 75 | for (unsigned int i = 0; i < height; i++) { 76 | for (unsigned int j = 0; j < width; j++) { 77 | uint16_t depth_value = depth_buffer[i * width + j]; 78 | if (depth_value == 0 || 79 | depth_value >= this->threshold / scale) { 80 | continue; 81 | } 82 | 83 | // If no obstacle was found yet, create a new one right in 84 | // front of the vehicle. The distance will be set later. 85 | if (obstacles.size() == 0) { 86 | Obstacle obs = {0, glm::dvec3(0, 0, 0)}; 87 | this->obstacles.push_back(obs); 88 | } 89 | 90 | // save the minimum value 91 | if (depth_value < min) { 92 | min = depth_value; 93 | min_i = i; 94 | min_j = j; 95 | } 96 | } 97 | } 98 | 99 | // If an obstacle was found, set its distance to the minimum distance 100 | if (obstacles.size() != 0) { 101 | obstacles[0].center.x = (double) min * scale; 102 | 103 | // Cartesian to spherical 104 | obstacles[0].center.y = (((double)min_i / height) * vfov) + base_theta; 105 | obstacles[0].center.z = ((1.0 - ((double)min_j / width)) * hfov) + base_phi; 106 | } 107 | 108 | return this->obstacles; 109 | } 110 | 111 | int main(int argc, char **argv) 112 | { 113 | // Initialize Depth Camera 114 | shared_ptr depth_camera = 115 | make_shared(640, 480, 30); 116 | 117 | // Initialize Detector 118 | shared_ptr obstacle_detector = 119 | make_shared(); 120 | 121 | cout << "Sensing the obstacles closer than 1.5 meters." << endl; 122 | while (true) { 123 | 124 | // Sense 125 | auto sensed_elements = obstacle_detector->detect(depth_camera->read()); 126 | 127 | // Print found Obstacle information 128 | if (sensed_elements.size()) { 129 | for (Obstacle o : sensed_elements) { 130 | cout << "Closest [r = " << o.center.x << 131 | " | theta = " << o.center.y << 132 | " | phi = " << o.center.z << 133 | " ]" << endl; 134 | } 135 | } else { 136 | cout << "No Obstacle" << endl; 137 | } 138 | } 139 | } 140 | 141 | -------------------------------------------------------------------------------- /samples/polarhist_sample.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2017 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #define STEP (5.0 * M_PI / 180) 25 | 26 | int main(int argc, char **argv) 27 | { 28 | std::shared_ptr depth_camera = 29 | std::make_shared(640, 480, 30); 30 | 31 | std::shared_ptr obstacle_detector = 32 | std::make_shared(STEP, 1.5, 0.2); 33 | 34 | double fov = depth_camera->get_horizontal_fov(); 35 | unsigned int slices = ceil(fov / STEP); 36 | double fixed_step = fov / slices; 37 | 38 | std::cout << "fov = " << fov << std::endl; 39 | std::cout << "step = " << fixed_step << std::endl; 40 | 41 | while (true) { 42 | std::vector obstacles = obstacle_detector->detect(depth_camera->read()); 43 | 44 | std::vector visual_slices; 45 | visual_slices.resize(slices, false); 46 | 47 | for (Obstacle obs : obstacles) { 48 | double offset = (M_PI / 2) - (fov / 2); 49 | int pos = slices - (obs.center.z - offset) / fixed_step; 50 | visual_slices[pos] = true; 51 | } 52 | 53 | for (unsigned int i = 0; i < slices; i++) 54 | std::cout << (visual_slices[i]?"████":"____"); 55 | 56 | std::cout << "\r"; 57 | std::cout.flush(); 58 | } 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /simulation/gazebo-models/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | 3 | install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/ 4 | DESTINATION ${COAVLIB_MODEL_PATH} 5 | PATTERN "CMakeLists.txt" EXCLUDE) 6 | 7 | -------------------------------------------------------------------------------- /simulation/gazebo-models/gzsitl_quadcopter_rs/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | gzsitl_quadcopter_rs 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /simulation/gazebo-models/gzsitl_quadcopter_rs/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://gzsitl_quadcopter 7 | 8 | 9 | 10 | model://realsense_camera 11 | 0 0.07 0.05 0 0 1.570796 12 | 13 | 14 | 15 | realsense_camera::link 16 | gzsitl_quadcopter::quadrotor::link 17 | 18 | 19 | 20 | 21 | 1 22 | 23 | 24 | 0.65 0.65 0.23 25 | 26 | 27 | 0 0 0.115 0 0 0.785 28 | 29 | 30 | 5 31 | 32 | contact_geometry 33 | 34 | 35 | 36 | 37 | 38 | sensors 39 | gzsitl_quadcopter::quadrotor::link 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/avoidance/Avoidance.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Intel Corporation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include "common/common.hh" 23 | 24 | template 25 | class CollisionAvoidanceStrategy 26 | { 27 | public: 28 | virtual void avoid(const std::vector &elements) = 0; 29 | 30 | protected: 31 | std::shared_ptr vehicle; 32 | }; 33 | -------------------------------------------------------------------------------- /src/avoidance/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | 3 | set(SOURCES 4 | QuadCopterShiftAvoidance.cc 5 | QuadCopterStopAvoidance.cc 6 | QuadCopterVFFAvoidance.cc) 7 | 8 | set(HEADERS 9 | Avoidance.hh 10 | QuadCopterShiftAvoidance.hh 11 | QuadCopterStopAvoidance.hh 12 | QuadCopterVFFAvoidance.hh) 13 | 14 | export_headers("${HEADERS}" "avoidance") 15 | set(COAV_INCLUDE_LIST "${COAV_INCLUDE_LIST}${INCLUDE_LIST}" PARENT_SCOPE) 16 | 17 | add_library(avoidance OBJECT ${SOURCES}) 18 | -------------------------------------------------------------------------------- /src/avoidance/QuadCopterShiftAvoidance.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include "avoidance/QuadCopterShiftAvoidance.hh" 23 | #include "common/common.hh" 24 | #include "common/math.hh" 25 | 26 | namespace defaults 27 | { 28 | const double safe_distance = 8.0; 29 | const double lowest_altitude = 1.0; 30 | const double detour_wp_angle = M_PI/3.0; 31 | } 32 | 33 | QuadCopterShiftAvoidance::QuadCopterShiftAvoidance( 34 | std::shared_ptr quadcopter) 35 | { 36 | this->vehicle = quadcopter; 37 | } 38 | 39 | void QuadCopterShiftAvoidance::avoid(const std::vector &obstacles) 40 | { 41 | Pose vehicle_pose = vehicle->vehicle_pose(); 42 | 43 | if (!vehicle->mav->is_ready()) { 44 | return; 45 | } 46 | 47 | vehicle->mav->set_autorotate_during_mission(true); 48 | vehicle->mav->set_autorotate_during_detour(false); 49 | 50 | switch (this->avoidance_state) { 51 | case avoid_state::detouring: { 52 | // The vehicle is detouring. We need to wait for it to finish the 53 | // detour before checking for obstacles again. 54 | 55 | if(vehicle->detour_finished()) { 56 | this->avoidance_state = avoid_state::moving; 57 | std::cout << "[avoid] state = moving..." << std::endl; 58 | } 59 | 60 | break; 61 | } 62 | case avoid_state::moving: { 63 | // The vehicle is moving to the target. We need to detect if an 64 | // obstacle is found in order to start a detour. 65 | 66 | // If no obstacle was detected, do nothing. 67 | if (obstacles.size() == 0) { 68 | break; 69 | } 70 | 71 | // Get closest obstacle 72 | Obstacle closest; 73 | closest.center.x = defaults::safe_distance + 1; 74 | for (Obstacle obs : obstacles) { 75 | if (obs.center.x < closest.center.x) { 76 | closest = obs; 77 | } 78 | } 79 | 80 | if (closest.center.x > defaults::safe_distance) { 81 | break; 82 | } 83 | 84 | // Check if the current mission waypoint is closer than the closest obstacle 85 | if (mavlink_vehicles::math::ground_dist( 86 | vehicle->mav->get_global_position_int(), 87 | vehicle->mav->get_mission_waypoint()) <= closest.center.x) { 88 | break; 89 | } 90 | 91 | // Check if we are too close to ground 92 | if (vehicle_pose.pos.z < defaults::lowest_altitude) { 93 | break; 94 | } 95 | 96 | // Calculate the lookat vector 97 | glm::dvec3 view_dir = 98 | glm::dvec3(-sin(vehicle_pose.yaw()), cos(vehicle_pose.yaw()), 0); 99 | 100 | // Calculate the direction of the detour waypoint (horizontal rotation 101 | // around lookat vector) 102 | glm::dvec3 wp_dir = 103 | glm::dvec3(view_dir.x * cos(defaults::detour_wp_angle) - 104 | view_dir.y * sin(defaults::detour_wp_angle), 105 | view_dir.x * sin(defaults::detour_wp_angle) + 106 | view_dir.y * cos(defaults::detour_wp_angle), 107 | 0.0); 108 | 109 | // Calculate the global position of the waypoint 110 | Pose wp = 111 | Pose{vehicle_pose.pos + glm::abs(2.0) * glm::normalize(wp_dir), 112 | glm::dquat(0, 0, 0, 0)}; 113 | 114 | // Send the detour waypoint to the vehicle 115 | vehicle->set_target_pose(wp); 116 | 117 | // Update avoidance state 118 | this->avoidance_state = avoid_state::detouring; 119 | 120 | // Print info 121 | std::cout << "[avoid] state = detouring..." << std::endl; 122 | 123 | std::cout << "[avoid] wp (x, y, z): " << wp.pos.x << ", " << wp.pos.y 124 | << "," << wp.pos.z << std::endl; 125 | 126 | std::cout << "[avoid] vh (x, y, z): " << vehicle_pose.pos.x << ", " 127 | << vehicle_pose.pos.y << "," << vehicle_pose.pos.z 128 | << std::endl; 129 | 130 | break; 131 | } 132 | } 133 | 134 | return; 135 | } 136 | 137 | -------------------------------------------------------------------------------- /src/avoidance/QuadCopterShiftAvoidance.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "avoidance/Avoidance.hh" 24 | #include "vehicles/MavQuadCopter.hh" 25 | 26 | class QuadCopterShiftAvoidance 27 | : public CollisionAvoidanceStrategy 28 | { 29 | public: 30 | QuadCopterShiftAvoidance(std::shared_ptr quadcopter); 31 | void avoid(const std::vector &obstacles) override; 32 | 33 | private: 34 | std::chrono::time_point wp_sent_time = 35 | std::chrono::system_clock::from_time_t(0); 36 | 37 | enum class avoid_state { moving, detouring }; 38 | avoid_state avoidance_state = avoid_state::moving; 39 | }; 40 | -------------------------------------------------------------------------------- /src/avoidance/QuadCopterStopAvoidance.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include "avoidance/QuadCopterStopAvoidance.hh" 20 | #include "common/common.hh" 21 | 22 | namespace defaults 23 | { 24 | const float lowest_altitude = 2.0; 25 | } 26 | 27 | QuadCopterStopAvoidance::QuadCopterStopAvoidance( 28 | std::shared_ptr quadcopter, double trigger_distance) 29 | { 30 | this->vehicle = quadcopter; 31 | this->trigger_dst = trigger_distance; 32 | } 33 | 34 | void QuadCopterStopAvoidance::avoid(const std::vector &detection) 35 | { 36 | if(!this->vehicle) { 37 | std::cout << "[avoid] vehicle does not exist" << std::endl; 38 | return; 39 | } 40 | 41 | if(!this->vehicle->mav) { 42 | std::cout << "[avoid] vehicle mav does not exist" << std::endl; 43 | return; 44 | } 45 | 46 | if (!this->vehicle->mav->is_ready()) { 47 | return; 48 | } 49 | 50 | // Vehicle is too close to ground. Do nothing. 51 | if (vehicle->vehicle_pose().pos.z < defaults::lowest_altitude) { 52 | return; 53 | } 54 | 55 | // If no obstacle was detected, do nothing. 56 | if (detection.size() == 0) { 57 | return; 58 | } 59 | 60 | // Send the stop command to the vehicle if any obstacle 61 | // is closer than or at trigger distance. 62 | for (Obstacle o : detection) { 63 | if (o.center.x <= this->trigger_dst) { 64 | if (!this->vehicle->mav->is_brake_active()) { 65 | this->vehicle->mav->brake(false); 66 | std::cout << "[avoid] state = stopping..." << std::endl; 67 | } 68 | } 69 | } 70 | 71 | return; 72 | } 73 | 74 | -------------------------------------------------------------------------------- /src/avoidance/QuadCopterStopAvoidance.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include "avoidance/Avoidance.hh" 23 | #include "vehicles/MavQuadCopter.hh" 24 | 25 | class QuadCopterStopAvoidance 26 | : public CollisionAvoidanceStrategy 27 | { 28 | public: 29 | QuadCopterStopAvoidance(std::shared_ptr quadcopter, 30 | double trigger_distance = 4.0); 31 | 32 | void avoid(const std::vector &detection) override; 33 | 34 | private: 35 | double trigger_dst; 36 | }; 37 | 38 | -------------------------------------------------------------------------------- /src/avoidance/QuadCopterVFFAvoidance.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | #include "avoidance/QuadCopterVFFAvoidance.hh" 21 | #include "common/common.hh" 22 | #include "common/math.hh" 23 | 24 | #define MIN_THETA 0.01 25 | #define MIN_PHI 0.01 26 | #define COAV_CALC_PERIOD_MS 100 27 | #define COAV_TARGET_SEC_MARGIN_M 1.5 28 | 29 | QuadCopterVFFAvoidance::QuadCopterVFFAvoidance( 30 | std::shared_ptr quadcopter) 31 | { 32 | this->vehicle = quadcopter; 33 | } 34 | 35 | double get_closest_obst_dist(const std::vector &obstacles) 36 | { 37 | 38 | // Find the distance from the closest obstacle 39 | double closest_obst_dist = std::numeric_limits::max(); 40 | for (unsigned int i = 0; i < obstacles.size(); ++i) { 41 | Obstacle obst_pos = obstacles[i]; 42 | double length = glm::length(obst_pos.center); 43 | if (length < closest_obst_dist) { 44 | closest_obst_dist = length; 45 | } 46 | } 47 | 48 | return closest_obst_dist; 49 | } 50 | 51 | void QuadCopterVFFAvoidance::avoid(const std::vector &obstacles) 52 | { 53 | /* TODO: WORK IN PROGRESS */ 54 | 55 | /* Get Target relative coordinates */ 56 | Pose target_pose = vehicle->target_pose(); 57 | Pose vehicle_pose = vehicle->vehicle_pose(); 58 | Pose relative_pose = target_pose - vehicle_pose; 59 | 60 | PolarVector target = cartesian_to_spherical( 61 | relative_pose.pos.x, relative_pose.pos.y, relative_pose.pos.z); 62 | 63 | /* Target parameters and variables */ 64 | float kg = 1.0; 65 | float ko = 1.0; 66 | float c1 = 1.0; 67 | float c2 = 1.0; 68 | 69 | float theta_g = target.theta; 70 | float phi_g = target.phi; 71 | float dg = target.r; 72 | 73 | float attract_x = kg * theta_g * (exp(-c1 * dg) + c2); 74 | float attract_y = kg * phi_g * (exp(-c1 * dg) + c2); 75 | 76 | /* Obstacle parameters and variables */ 77 | float repulse_x = 0.0; 78 | float repulse_y = 0.0; 79 | float theta_o = 0.0; 80 | float phi_o = 0.0; 81 | float d_o = 0.0; 82 | float t1 = 1.0; 83 | float t2 = 1.0; 84 | float s1 = 1.0; 85 | float s2 = 1.0; 86 | float c3 = 1.0; 87 | float c4 = 1.0; 88 | 89 | for (unsigned int i = 0; i < obstacles.size(); ++i) { 90 | theta_o = obstacles[i].center.y; 91 | phi_o = obstacles[i].center.z; 92 | d_o = obstacles[i].center.x; 93 | 94 | /* If the angles are below a specific threshold, consider them as equal 95 | * to -/+ threshold. */ 96 | if (fabs(theta_o) < MIN_THETA) 97 | theta_o = copysign(MIN_THETA, theta_o); 98 | if (fabs(phi_o) < MIN_PHI) 99 | phi_o = copysign(MIN_PHI, phi_o); 100 | 101 | repulse_x += -ko * sign(theta_o) * 102 | sigmoid(s1 * (1.0 - fabs(phi_o) / s2)) * exp(-c3 * d_o) * 103 | exp(-c4 * fabs(theta_o)); 104 | repulse_y += -ko * sign(phi_o) * 105 | sigmoid(t1 * (1.0 - fabs(theta_o) / t2)) * exp(-c3 * d_o) * 106 | exp(-c4 * fabs(phi_o)); 107 | } 108 | 109 | // std::cout << "[QuadCopterVFFAvoidance] " 110 | // << "Rates: " << (attract_x + repulse_x) << ", " 111 | // << (attract_y + repulse_y) << std::endl; 112 | 113 | std::tuple rates(attract_x + repulse_x, attract_y + repulse_y); 114 | double dist = get_closest_obst_dist(obstacles); 115 | 116 | // std::cout << "Num obstacles, dist: " << obstacles.size() << dist << std::endl; 117 | 118 | // This is the code for drone control 119 | using namespace std::chrono; 120 | 121 | // Check if its time to calculate the coav waypoint 122 | time_point curr_time = system_clock::now(); 123 | if (duration_cast(curr_time - last_calc_time).count() < 124 | COAV_CALC_PERIOD_MS) { 125 | return; 126 | } 127 | 128 | // Get Vehicle and Target Pose 129 | PolarVector target_polar_pos = cartesian_to_spherical( 130 | relative_pose.pos.x, relative_pose.pos.y, relative_pose.pos.z); 131 | 132 | glm::dvec3 coav_wp = calculate_coav_wp(vehicle_pose, 133 | std::get<0>(rates), std::get<1>(rates), dist, target_polar_pos.r); 134 | 135 | // Set new Target Position According to COAV 136 | target_pose = Pose{coav_wp, glm::dquat(0, 0, 0, 0)}; 137 | 138 | vehicle->set_target_pose(target_pose); 139 | 140 | last_calc_time = curr_time; 141 | } 142 | 143 | glm::dvec3 144 | QuadCopterVFFAvoidance::calculate_coav_wp(Pose pose, double turn_rate, 145 | double climb_rate, double closest_obst_dist, 146 | double target_dist) 147 | { 148 | // The new waypoint is positioned on the suface of an imaginary sphere of 149 | // radius R centender in the Drone's current position. Phi and theta are 150 | // calculated according to the turn rate and the climb rate. R is given by 151 | // min(closest_obst_dist, target_dist). 152 | double R = 0; 153 | 154 | if (closest_obst_dist < target_dist) { 155 | R = closest_obst_dist - COAV_TARGET_SEC_MARGIN_M; 156 | } else { 157 | R = target_dist; 158 | } 159 | 160 | double theta = turn_rate; 161 | 162 | // Considering that the look-at vector can be represented 163 | // as the axis X rotated around Z by the YAW angle, then. 164 | glm::dvec3 view_dir( 165 | R * cos(glm::yaw(pose.rot)), 166 | R * sin(glm::yaw(pose.rot)), 167 | 0 168 | ); 169 | 170 | glm::dvec3 new_waypoint_dir( 171 | view_dir.x * cos(theta) - view_dir.y * sin(theta), 172 | view_dir.x * sin(theta) + view_dir.y * cos(theta), 173 | 0 174 | ); 175 | 176 | return pose.pos + fabs(R) * glm::normalize(new_waypoint_dir); 177 | } 178 | 179 | -------------------------------------------------------------------------------- /src/avoidance/QuadCopterVFFAvoidance.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include "avoidance/Avoidance.hh" 26 | #include "vehicles/MavQuadCopter.hh" 27 | 28 | class QuadCopterVFFAvoidance : public CollisionAvoidanceStrategy 29 | { 30 | public: 31 | QuadCopterVFFAvoidance(std::shared_ptr quadcopter); 32 | void avoid(const std::vector &obstacles) override; 33 | 34 | private: 35 | bool coav_enabled = true; 36 | std::chrono::time_point last_calc_time = 37 | std::chrono::system_clock::from_time_t(0); 38 | 39 | glm::dvec3 calculate_coav_wp(Pose pose, double turn_rate, double climb_rate, 40 | double closest_obst_dist, double target_dist); 41 | }; 42 | 43 | -------------------------------------------------------------------------------- /src/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | 3 | set(SOURCES 4 | common.cc 5 | math.cc) 6 | 7 | set(HEADERS common.hh) 8 | 9 | export_headers("${HEADERS}" "common") 10 | set(COAV_INCLUDE_LIST "${COAV_INCLUDE_LIST}${INCLUDE_LIST}" PARENT_SCOPE) 11 | 12 | add_library(common OBJECT ${SOURCES}) 13 | -------------------------------------------------------------------------------- /src/common/common.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Intel Corporation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include "common/common.hh" 20 | #include "common/math.hh" 21 | 22 | Pose operator-(const Pose &a, const Pose &b) 23 | { 24 | glm::dquat tmp = {0.0, a.pos.x - b.pos.x, a.pos.y - b.pos.y, 25 | a.pos.z - b.pos.z}; 26 | tmp = glm::inverse(b.rot) * (tmp * b.rot); 27 | 28 | return Pose{{tmp.x, tmp.y, tmp.z}, 29 | glm::normalize(glm::inverse(b.rot) * a.rot)}; 30 | } 31 | 32 | void Pose::set_rot(float pitch, float roll, float yaw) 33 | { 34 | double phi, the, psi; 35 | 36 | phi = roll / 2.0; 37 | the = pitch / 2.0; 38 | psi = yaw / 2.0; 39 | 40 | this->rot.w = 41 | cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); 42 | this->rot.x = 43 | sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); 44 | this->rot.y = 45 | cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); 46 | this->rot.z = 47 | cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); 48 | } 49 | 50 | double Pose::pitch() 51 | { 52 | // GLM uses an unusual convention to convert quaternions to euler angles 53 | // with values swapped 54 | return glm::yaw(rot); 55 | } 56 | 57 | double Pose::roll() 58 | { 59 | // GLM uses an unusual convention to convert quaternions to euler angles 60 | // with values swapped 61 | return glm::pitch(rot); 62 | } 63 | 64 | double Pose::yaw() 65 | { 66 | // GLM uses an unusual convention to convert quaternions to euler angles 67 | // with values swapped 68 | return glm::roll(rot); 69 | } 70 | -------------------------------------------------------------------------------- /src/common/common.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | struct Obstacle { 26 | uint id; 27 | 28 | /** 29 | * Spherical coordinates as in ISO 31-11. 30 | * 31 | * x = r (Radial) 32 | * y = theta (Polar Angle) 33 | * z = phi (Azimuthal Angle) 34 | * 35 | * When returned from a Sensor, the origin is the sensor itself 36 | * "looking" along the 'y' axis. 37 | */ 38 | glm::dvec3 center; 39 | }; 40 | 41 | struct Pose { 42 | glm::dvec3 pos; 43 | glm::dquat rot; 44 | 45 | double pitch(); 46 | double roll(); 47 | double yaw(); 48 | void set_rot(float pitch, float roll, float yaw); 49 | }; 50 | 51 | Pose operator-(const Pose& a, const Pose &b); 52 | -------------------------------------------------------------------------------- /src/common/math.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include "common/math.hh" 20 | 21 | int sign(double x) 22 | { 23 | return (x > 0) - (x < 0); 24 | } 25 | 26 | double sigmoid(double x) 27 | { 28 | return 1.0 / (1.0 + exp(-x)); 29 | } 30 | 31 | PolarVector cartesian_to_spherical(double x, double y, double z) 32 | { 33 | PolarVector p; 34 | 35 | p.r = sqrt(x * x + y * y + z * z); 36 | p.theta = acos(z / p.r); 37 | p.phi = atan2(y, x); 38 | 39 | return p; 40 | } 41 | -------------------------------------------------------------------------------- /src/common/math.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #define inbounds(X, A, B) ((X) >= A && (X) <= B) 20 | 21 | int sign(double x); 22 | 23 | double sigmoid(double x); 24 | 25 | /** 26 | * @brief Spherical coordinates as in ISO 31-11. 27 | */ 28 | struct PolarVector { 29 | double r; /**< Radial distance */ 30 | double theta; /**< Polar Angle */ 31 | double phi; /**< Azimuthal Angle */ 32 | }; 33 | 34 | PolarVector cartesian_to_spherical(double x, double y, double z); 35 | -------------------------------------------------------------------------------- /src/detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | 3 | set(SOURCES 4 | DepthImageObstacleDetector.cc 5 | DepthImagePolarHistDetector.cc) 6 | 7 | set(HEADERS 8 | Detectors.hh 9 | DepthImageObstacleDetector.hh 10 | DepthImagePolarHistDetector.hh) 11 | 12 | export_headers("${HEADERS}" "detection") 13 | set(COAV_INCLUDE_LIST "${COAV_INCLUDE_LIST}${INCLUDE_LIST}" PARENT_SCOPE) 14 | 15 | add_library(detection OBJECT ${SOURCES}) 16 | -------------------------------------------------------------------------------- /src/detection/DepthImageObstacleDetector.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "DepthImageObstacleDetector.hh" 22 | #include "common/common.hh" 23 | 24 | #define MAX_NUM_LABELS UINT16_MAX 25 | #define BACKGROUND 0 26 | 27 | struct Set { 28 | Set *parent; 29 | int rank = 0; 30 | 31 | Set(); 32 | Set *repr(); 33 | static void join(Set *a, Set *b); 34 | }; 35 | 36 | class PPTree 37 | { 38 | public: 39 | PPTree(int num_nodes); 40 | ~PPTree(); 41 | 42 | void ds_union(int node_x_id, int node_y_id); 43 | int ds_find(int node_id); 44 | 45 | private: 46 | Set *find(int node_id); 47 | std::vector sets; 48 | }; 49 | 50 | Set::Set() 51 | : parent(this) 52 | { 53 | } 54 | 55 | Set *Set::repr() 56 | { 57 | if (this->parent == this) 58 | return this; 59 | 60 | return this->parent = this->parent->repr(); 61 | } 62 | 63 | PPTree::PPTree(int num_nodes) 64 | : sets(num_nodes) 65 | { 66 | } 67 | 68 | int PPTree::ds_find(int node_id) 69 | { 70 | return this->find(node_id) - this->sets.data(); 71 | } 72 | 73 | Set *PPTree::find(int node_id) 74 | { 75 | return this->sets[node_id].repr(); 76 | } 77 | 78 | 79 | void PPTree::ds_union(int a, int b) 80 | { 81 | Set::join(&this->sets[a], &this->sets[b]); 82 | } 83 | 84 | PPTree::~PPTree() 85 | { 86 | } 87 | 88 | void Set::join(Set *a, Set *b) 89 | { 90 | a = a->repr(); 91 | b = b->repr(); 92 | 93 | if (a == b) { 94 | /* Already united. */ 95 | return; 96 | } 97 | 98 | /* Unite the roots of the nodes. */ 99 | if (a->rank < b->rank) { 100 | a->parent = b; 101 | return; 102 | } 103 | 104 | b->parent = a; 105 | if (a->rank == b->rank) 106 | a->rank += 1; 107 | } 108 | 109 | DepthImageObstacleDetector::DepthImageObstacleDetector(double threshold_meters) 110 | { 111 | this->obstacles.resize(MAX_NUM_LABELS); 112 | this->threshold = threshold_meters; 113 | } 114 | 115 | const std::vector &DepthImageObstacleDetector::detect(std::shared_ptr data) 116 | { 117 | std::shared_ptr depth_data = std::static_pointer_cast(data); 118 | // Store current camera frame data 119 | this->depth_frame = depth_data->depth_buffer; 120 | this->height = depth_data->height; 121 | this->width = depth_data->width; 122 | this->scale = depth_data->scale; 123 | this->hfov = depth_data->hfov; 124 | this->vfov = depth_data->vfov; 125 | this->base_phi = (M_PI - hfov) / 2; 126 | this->base_theta = (M_PI - vfov) / 2; 127 | 128 | // Detect obstacles from current depth buffer 129 | this->extract_blobs(); 130 | 131 | return this->obstacles; 132 | } 133 | 134 | inline bool DepthImageObstacleDetector::is_valid(const uint16_t depth) 135 | { 136 | uint16_t threshold_scaled = (uint16_t) (this->threshold / this->scale); 137 | return (depth != BACKGROUND && (threshold_scaled ? depth < threshold_scaled : true)); 138 | } 139 | 140 | inline bool DepthImageObstacleDetector::is_in_range(const uint16_t d1, const uint16_t d2) 141 | { 142 | return (abs(d1 - d2) <= this->tolerance); 143 | } 144 | 145 | int DepthImageObstacleDetector::get_neighbors_label(const int i, const int j, std::vector &neigh_labels) 146 | { 147 | int pixel_idx, north_idx; 148 | int neighbor_idx = 0; 149 | 150 | if (i < 0 || i >= this->height || j < 0 || j >= this->width) { 151 | return 0; 152 | } 153 | 154 | pixel_idx = i * this->width + j; 155 | north_idx = pixel_idx - this->width; 156 | 157 | /* Check west */ 158 | if (j > 0) { 159 | if (is_in_range(depth_frame[pixel_idx - 1], depth_frame[pixel_idx])) { 160 | neigh_labels[neighbor_idx] = this->labels[pixel_idx - 1]; 161 | neighbor_idx++; 162 | } 163 | } 164 | 165 | /* Check northwest */ 166 | if (j > 0 && i > 0) { 167 | if (is_in_range(depth_frame[north_idx - 1], depth_frame[pixel_idx])) { 168 | neigh_labels[neighbor_idx] = this->labels[north_idx - 1]; 169 | neighbor_idx++; 170 | } 171 | } 172 | 173 | /* Check north */ 174 | if (i > 0) { 175 | if (is_in_range(depth_frame[north_idx], depth_frame[pixel_idx])) { 176 | neigh_labels[neighbor_idx] = this->labels[north_idx]; 177 | neighbor_idx++; 178 | } 179 | } 180 | 181 | /* Check northeast */ 182 | if (i > 0 && j < (this->width - 1)) { 183 | if (is_in_range(depth_frame[north_idx + 1], depth_frame[pixel_idx])) { 184 | neigh_labels[neighbor_idx] = this->labels[north_idx + 1]; 185 | neighbor_idx++; 186 | } 187 | } 188 | 189 | /* Find lowest id and put it in front */ 190 | int lowest_id = neigh_labels[0]; 191 | for (int k = 1; k < neighbor_idx; k++) { 192 | if (neigh_labels[k] < lowest_id) { 193 | lowest_id = neigh_labels[k]; 194 | neigh_labels[k] = neigh_labels[0]; 195 | neigh_labels[0] = lowest_id; 196 | } 197 | } 198 | 199 | return neighbor_idx; 200 | } 201 | 202 | void init_obstacle_array(std::vector &obstacles) 203 | { 204 | obstacles.resize(MAX_NUM_LABELS); 205 | for (unsigned int i = 0; i < obstacles.size(); ++i) { 206 | obstacles[i].id = -1; 207 | obstacles[i].center = glm::dvec3(-1, -1, -1); 208 | } 209 | } 210 | 211 | int DepthImageObstacleDetector::extract_blobs() 212 | { 213 | int row_offset; 214 | 215 | // Check if the current stored depth frame is valid 216 | if (depth_frame.size() == 0) { 217 | obstacles.resize(0); 218 | return 0; 219 | } 220 | 221 | // Instantiate disjoint data set 222 | PPTree ds_tree(MAX_NUM_LABELS); 223 | 224 | // Store number of pixels for each blob 225 | int blob_num_pixels[MAX_NUM_LABELS] = {0}; 226 | 227 | // Blob to Obstacle Vector 228 | int blob_to_obstacle[MAX_NUM_LABELS]; 229 | int num_obstacles = 0; 230 | uint16_t curr_label = 1; 231 | 232 | // Init Obstacles vector and labels vector 233 | init_obstacle_array(obstacles); 234 | this->labels.resize(depth_frame.size()); 235 | 236 | // First Pass 237 | for (int i = 0; i < this->height; i++) { 238 | row_offset = i * this->width; 239 | for (int j = 0; j < this->width; j++) { 240 | if (is_valid(this->depth_frame[row_offset + j])) { 241 | std::vector neigh_labels(4); 242 | 243 | int num_neighbors = get_neighbors_label(i, j, neigh_labels); 244 | if (num_neighbors) { 245 | this->labels[row_offset + j] = neigh_labels[0]; 246 | for (int k = 1; k < num_neighbors; k++) { 247 | ds_tree.ds_union(neigh_labels[0], neigh_labels[k]); 248 | } 249 | } else { 250 | this->labels[row_offset + j] = (curr_label <= MAX_NUM_LABELS) ? curr_label++ : 0; 251 | } 252 | } else { 253 | this->labels[row_offset + j] = 0; 254 | } 255 | } 256 | } 257 | 258 | /* Second Pass. */ 259 | for (unsigned int i = 0; i < labels.size(); i++) { 260 | if (this->labels[i]) { 261 | this->labels[i] = ds_tree.ds_find(this->labels[i]); 262 | blob_num_pixels[this->labels[i]]++; 263 | } 264 | } 265 | 266 | /* Initialize blob to obstacle vector. */ 267 | for (int i = 0; i < MAX_NUM_LABELS; i++) { 268 | blob_to_obstacle[i] = -1; 269 | } 270 | 271 | /* Third Pass */ 272 | 273 | for (int i = 0; i < this->height; i++) { 274 | row_offset = i * this->width; 275 | for (int j = 0; j < this->width; j++) { 276 | int label = this->labels[row_offset + j]; 277 | 278 | if (!label || blob_num_pixels[label] < this->min_num_pixels) 279 | continue; 280 | 281 | if (blob_to_obstacle[label] == -1) { 282 | if (num_obstacles >= this->max_num_obstacles) 283 | continue; 284 | 285 | blob_to_obstacle[label] = num_obstacles++; 286 | obstacles[blob_to_obstacle[label]].id = label; 287 | obstacles[blob_to_obstacle[label]].center.x = DBL_MAX; 288 | } 289 | 290 | Obstacle *o = &obstacles[blob_to_obstacle[label]]; 291 | o->center += glm::dvec3(0, i, j); 292 | o->center.x = (depth_frame[row_offset + j] < o->center.x) ? 293 | depth_frame[row_offset + j] : o->center.x; 294 | } 295 | } 296 | 297 | this->obstacles.resize(num_obstacles); 298 | for (Obstacle &o : obstacles) { 299 | o.center.x *= this->scale; 300 | o.center.y /= blob_num_pixels[o.id]; 301 | o.center.z /= blob_num_pixels[o.id]; 302 | 303 | // Cartesian to spherical 304 | o.center.y = ((o.center.y / this->height) * vfov) + base_theta; 305 | o.center.z = ((1.0 - (o.center.z / this->width)) * hfov) + base_phi; 306 | } 307 | 308 | return num_obstacles; 309 | } 310 | 311 | -------------------------------------------------------------------------------- /src/detection/DepthImageObstacleDetector.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include "detection/Detectors.hh" 23 | #include "sensors/Sensors.hh" 24 | 25 | class DepthImageObstacleDetector : public Detector 26 | { 27 | public: 28 | DepthImageObstacleDetector(double threshold_meters = 0.0); 29 | const std::vector &detect(std::shared_ptr data) override; 30 | 31 | private: 32 | std::vector obstacles; 33 | std::vector depth_frame; 34 | std::vector labels; 35 | int width; 36 | int height; 37 | double hfov; 38 | double vfov; 39 | double scale; 40 | double base_theta; 41 | double base_phi; 42 | 43 | bool is_valid(const uint16_t depth); 44 | bool is_in_range(const uint16_t d1, const uint16_t d2); 45 | 46 | int get_neighbors_label(const int i, const int j, std::vector &neigh_labels); 47 | 48 | int extract_blobs(); 49 | 50 | double calc_pixel_area(int i, int j, uint16_t depth_value); 51 | 52 | int max_num_obstacles = 1000; 53 | int tolerance = 20; 54 | int min_num_pixels = 400; // equivalent area of a 20x20 square 55 | 56 | double threshold = 0; 57 | }; 58 | -------------------------------------------------------------------------------- /src/detection/DepthImagePolarHistDetector.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "DepthImagePolarHistDetector.hh" 21 | 22 | namespace defaults 23 | { 24 | const unsigned int vertical_sweep_pixels = 10; 25 | } 26 | 27 | DepthImagePolarHistDetector::DepthImagePolarHistDetector(double angle_step, 28 | double threshold, double density) 29 | { 30 | this->step = glm::radians(angle_step); 31 | this->threshold = threshold; 32 | this->density = density; 33 | } 34 | 35 | const std::vector &DepthImagePolarHistDetector::detect( 36 | std::shared_ptr data) 37 | { 38 | std::shared_ptr depth_data = std::static_pointer_cast(data); 39 | std::vector histogram; 40 | std::vector density_count; 41 | 42 | // Obtain camera depth buffer and camera properties 43 | std::vector depth_buffer = depth_data->depth_buffer; 44 | unsigned int height = depth_data->height; 45 | unsigned int width = depth_data->width; 46 | double fov = depth_data->hfov; 47 | double scale = depth_data->scale; 48 | 49 | unsigned int middle_row = height / 2; 50 | 51 | this->obstacles.clear(); 52 | 53 | // Return if depth buffer is empty 54 | if(depth_buffer.size() == 0) { 55 | return this->obstacles; 56 | } 57 | 58 | // Make sure the chosen vertical sweep area is within the limits of the 59 | // depth buffer 60 | unsigned int vertical_sweep_pixels = 61 | glm::min(defaults::vertical_sweep_pixels, middle_row); 62 | 63 | // Create one entry for each slice of the fov and initialize to max distance 64 | histogram.resize(glm::ceil(fov / this->step), UINT16_MAX * scale); 65 | density_count.resize(histogram.size(), 0); 66 | 67 | // Sweep a slice of the depth buffer filling up the histogram with the 68 | // closest distance found in a given direction 69 | for (unsigned int i = (middle_row - vertical_sweep_pixels); 70 | i < (middle_row + vertical_sweep_pixels); i++) { 71 | for (unsigned int j = 0; j < width; j++) { 72 | unsigned int pos = ((double) j / (double) width) * histogram.size(); 73 | 74 | uint16_t depth_value = depth_buffer[i * width + j]; 75 | if (depth_value == 0) { 76 | depth_value = UINT16_MAX; 77 | } 78 | 79 | if (depth_value * scale < this->threshold) 80 | density_count[pos]++; 81 | 82 | histogram[pos] = glm::min(depth_value * scale, histogram[pos]); 83 | } 84 | } 85 | 86 | // we use equal sized slices, so the actual step may be smaller than the provided 87 | // step if fov is not a multiple of it. 88 | double fixed_step = fov / histogram.size(); 89 | unsigned int slice_pixel_count = (vertical_sweep_pixels * 2) * (width / histogram.size()); 90 | 91 | // Assuming the drone is always looking down the y axis, calculate 92 | // the max phi it can see 93 | double max_phi = (fov + M_PI) / 2; 94 | 95 | for (size_t i = 0; i < histogram.size(); i++) { 96 | if (histogram[i] > this->threshold || 97 | ((double) density_count[i]) / slice_pixel_count < this->density) 98 | continue; 99 | // Scan is made from left to right, so position 0 will be at the biggest phi 100 | // We center the phi in the middle of the slice 101 | double phi = max_phi - (i * fixed_step) - (fixed_step / 2); 102 | 103 | Obstacle obs; 104 | obs.center = glm::dvec3(histogram[i], M_PI / 2, phi); 105 | this->obstacles.push_back(obs); 106 | } 107 | 108 | return this->obstacles; 109 | } 110 | 111 | -------------------------------------------------------------------------------- /src/detection/DepthImagePolarHistDetector.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include "detection/Detectors.hh" 23 | #include "sensors/Sensors.hh" 24 | 25 | class DepthImagePolarHistDetector : public Detector 26 | { 27 | public: 28 | DepthImagePolarHistDetector(double angle_step, 29 | double threshold = 5.0, double density = 0.1); 30 | const std::vector &detect(std::shared_ptr data) override; 31 | 32 | private: 33 | double step; 34 | double threshold; 35 | double density; 36 | std::vector obstacles; 37 | }; 38 | -------------------------------------------------------------------------------- /src/detection/Detectors.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Intel Corporation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include "common/common.hh" 23 | 24 | class Detector 25 | { 26 | public: 27 | virtual const std::vector &detect(std::shared_ptr data) = 0; 28 | }; 29 | -------------------------------------------------------------------------------- /src/sensors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | 3 | set(SOURCES Sensors.cc) 4 | set(HEADERS Sensors.hh) 5 | 6 | if (${WITH_GAZEBO}) 7 | set(SOURCES ${SOURCES} GazeboRealSenseCamera.cc) 8 | set(HEADERS ${HEADERS} GazeboRealSenseCamera.hh) 9 | endif() 10 | 11 | if (${WITH_REALSENSE}) 12 | set(SOURCES ${SOURCES} RealSenseCamera.cc) 13 | set(HEADERS ${HEADERS} RealSenseCamera.hh) 14 | endif() 15 | 16 | export_headers("${HEADERS}" "sensors") 17 | set(COAV_INCLUDE_LIST "${COAV_INCLUDE_LIST}${INCLUDE_LIST}" PARENT_SCOPE) 18 | 19 | add_library(sensors OBJECT ${SOURCES}) 20 | -------------------------------------------------------------------------------- /src/sensors/GazeboRealSenseCamera.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Intel Corporation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include "sensors/GazeboRealSenseCamera.hh" 20 | 21 | // TODO: The camera configuration should be aggregated on the gazebo .sdf model 22 | // file or retrieved from the librealsense. This feature is not implemented 23 | // yet, so those values are stored here until then. 24 | #define GZ_RS_STREAM_DEPTH_TOPIC "~/gzsitl_quadcopter_rs/rs/stream/depth" 25 | #define DEPTH_CAM_WIDTH 640 26 | #define DEPTH_CAM_HEIGHT 480 27 | #define DEPTH_CAM_HFOV M_PI / 3.0 28 | #define DEPTH_CAM_VFOV 0.757608 29 | #define DEPTH_CAM_SCALE 0.001 30 | // ============= 31 | // GazeboContext 32 | // ============= 33 | 34 | class GazeboContext 35 | { 36 | public: 37 | ~GazeboContext(); 38 | gazebo::transport::Node *node(); 39 | static std::shared_ptr instance(); 40 | 41 | private: 42 | GazeboContext(); 43 | }; 44 | 45 | GazeboContext::GazeboContext() 46 | { 47 | gazebo::client::setup(); 48 | std::cout << "[GazeboContext] Gazebo client has been set up" << std::endl; 49 | } 50 | 51 | GazeboContext::~GazeboContext() 52 | { 53 | gazebo::client::shutdown(); 54 | std::cout << "[GazeboContext] Gazebo client has been shut down" << std::endl; 55 | } 56 | 57 | std::shared_ptr GazeboContext::instance() { 58 | static std::weak_ptr _instance; 59 | if (auto ptr = _instance.lock()) { // .lock() returns a shared_ptr and increments the refcount 60 | return ptr; 61 | } 62 | // Does not support std::make_shared because of 63 | // the Resource private constructor. 64 | auto ptr = std::shared_ptr(new GazeboContext()); 65 | _instance = ptr; 66 | return ptr; 67 | } 68 | 69 | gazebo::transport::Node *GazeboContext::node() 70 | { 71 | return new gazebo::transport::Node(); 72 | } 73 | 74 | // ===================== 75 | // GazeboRealSenseCamera 76 | // ===================== 77 | 78 | GazeboRealSenseCamera::GazeboRealSenseCamera() 79 | { 80 | // Start communication with Gazebo 81 | std::cout << "[GazeboRealSenseCamera] Waiting for Gazebo..." << std::endl; 82 | this->gazebo_context = GazeboContext::instance(); 83 | 84 | // Create our node for communication 85 | this->gznode.reset(gazebo_context->node()); 86 | this->gznode->Init(); 87 | 88 | std::cout << "[GazeboRealSenseCamera] Gazebo Initialized" << std::endl; 89 | 90 | // TODO: Retrieve camera data straight from topic or camera plugin 91 | this->width = DEPTH_CAM_WIDTH; 92 | this->height = DEPTH_CAM_HEIGHT; 93 | this->hfov = DEPTH_CAM_HFOV; 94 | this->vfov = DEPTH_CAM_VFOV; 95 | this->scale = DEPTH_CAM_SCALE; 96 | 97 | // TODO: Find RealSense camera topic and parameters automatically 98 | this->rs_depth_sub = this->gznode->Subscribe( 99 | GZ_RS_STREAM_DEPTH_TOPIC, &GazeboRealSenseCamera::on_stream_depth_recvd, 100 | this); 101 | } 102 | 103 | GazeboRealSenseCamera::~GazeboRealSenseCamera() 104 | { 105 | } 106 | 107 | std::vector &GazeboRealSenseCamera::get_depth_buffer() 108 | { 109 | std::lock_guard locker(depth_buffer_mtx); 110 | return depth_buffer[this->current_buffer ^= 1]; 111 | } 112 | 113 | void GazeboRealSenseCamera::on_stream_depth_recvd(ConstImageStampedPtr &_msg) 114 | { 115 | if (!this->camera_exists) { 116 | this->camera_exists = true; 117 | std::cout << "[GazeboRealSenseCamera] Real Sense Initialized" << std::endl; 118 | } 119 | 120 | std::lock_guard locker(depth_buffer_mtx); 121 | 122 | uint16_t *data = (uint16_t *) _msg->image().data().c_str(); 123 | uint buffer_size = _msg->image().width() * _msg->image().height(); 124 | depth_buffer[this->current_buffer ^ 1] = std::vector(data, data + buffer_size); 125 | } 126 | -------------------------------------------------------------------------------- /src/sensors/GazeboRealSenseCamera.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include "sensors/Sensors.hh" 27 | 28 | class GazeboContext; 29 | 30 | class GazeboRealSenseCamera: public DepthCamera 31 | { 32 | public: 33 | GazeboRealSenseCamera(); 34 | ~GazeboRealSenseCamera(); 35 | 36 | std::vector &get_depth_buffer() override; 37 | 38 | private: 39 | void on_stream_depth_recvd(ConstImageStampedPtr &_msg); 40 | 41 | std::mutex depth_buffer_mtx; 42 | std::vector depth_buffer[2] = {{}, {}}; 43 | uint current_buffer = 0; 44 | std::shared_ptr gazebo_context; 45 | gazebo::transport::NodePtr gznode; 46 | gazebo::transport::SubscriberPtr rs_depth_sub; 47 | bool camera_exists = false; 48 | }; 49 | -------------------------------------------------------------------------------- /src/sensors/RealSenseCamera.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include "sensors/RealSenseCamera.hh" 18 | 19 | #include 20 | 21 | #include 22 | 23 | #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) 24 | 25 | static const rs::option r200_opts[] = { 26 | rs::option::r200_lr_auto_exposure_enabled, 27 | rs::option::r200_emitter_enabled, 28 | }; 29 | 30 | static const double r200_values[] = { 31 | 1.0, 32 | 1.0, 33 | }; 34 | 35 | RealSenseCamera::RealSenseCamera(size_t width, size_t height, unsigned int fps) try 36 | { 37 | this->width = width; 38 | this->height = height; 39 | 40 | this->ctx = std::make_shared(); 41 | if (this->ctx->get_device_count() == 0) { 42 | std::cerr << "[RealSenseCamera] No RealSense device found." << std::endl; 43 | return; 44 | } 45 | 46 | this->dev = this->ctx->get_device(0); 47 | 48 | this->dev->enable_stream(rs::stream::depth, this->width, this->height, 49 | rs::format::z16, fps); 50 | 51 | auto intrinsics = this->dev->get_stream_intrinsics(rs::stream::depth); 52 | this->hfov = glm::radians(intrinsics.hfov()); 53 | this->vfov = glm::radians(intrinsics.vfov()); 54 | 55 | this->scale = this->dev->get_depth_scale(); 56 | 57 | if (this->dev->supports_option(r200_opts[0])) { 58 | this->dev->set_options(r200_opts, ARRAY_SIZE(r200_opts), r200_values); 59 | } 60 | 61 | this->dev->start(); 62 | 63 | this->depth_buffer.resize(this->width * this->height); 64 | } 65 | catch(const rs::error &e) 66 | { 67 | std::cerr << "[RealSenseCamera] Error: " << e.get_failed_function().c_str() 68 | << e.get_failed_args().c_str() << std::endl; 69 | } 70 | 71 | std::vector &RealSenseCamera::get_depth_buffer() try 72 | { 73 | if (this->dev == nullptr) { 74 | std::cerr << "[RealSenseCamera] Error: No device available." << std::endl; 75 | return this->depth_buffer; 76 | } 77 | 78 | this->dev->wait_for_frames(); 79 | 80 | const uint16_t *frame = reinterpret_cast 81 | (this->dev->get_frame_data(rs::stream::depth)); 82 | 83 | std::lock_guard locker(depth_buffer_mtx); 84 | for (size_t i = 0; i < this->width * this->height; i++) 85 | this->depth_buffer[i] = frame[i]; 86 | 87 | return this->depth_buffer; 88 | } 89 | catch(const rs::error &e) 90 | { 91 | std::cerr << "[RealSenseCamera] Error: " << e.get_failed_function().c_str() 92 | << e.get_failed_args().c_str() << std::endl; 93 | 94 | return this->depth_buffer; 95 | } 96 | -------------------------------------------------------------------------------- /src/sensors/RealSenseCamera.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include "sensors/Sensors.hh" 27 | 28 | class RealSenseCamera: public DepthCamera 29 | { 30 | public: 31 | RealSenseCamera(size_t width, size_t height, unsigned int fps); 32 | 33 | std::vector &get_depth_buffer() override; 34 | 35 | private: 36 | std::mutex depth_buffer_mtx; 37 | std::vector depth_buffer; 38 | 39 | std::shared_ptr ctx; 40 | rs::device *dev = nullptr; 41 | }; 42 | -------------------------------------------------------------------------------- /src/sensors/Sensors.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include "Sensors.hh" 18 | 19 | #include 20 | 21 | unsigned int DepthCamera::get_height() 22 | { 23 | return height; 24 | } 25 | 26 | unsigned int DepthCamera::get_width() 27 | { 28 | return width; 29 | } 30 | 31 | double DepthCamera::get_scale() 32 | { 33 | return scale; 34 | } 35 | 36 | std::shared_ptr DepthCamera::read() 37 | { 38 | std::shared_ptr data = std::make_shared(); 39 | 40 | data->height = this->get_height(); 41 | data->width = this->get_width(); 42 | data->scale = this->get_scale(); 43 | data->hfov = this->get_horizontal_fov(); 44 | data->vfov = this->get_vertical_fov(); 45 | data->depth_buffer = this->get_depth_buffer(); 46 | 47 | return data; 48 | } 49 | -------------------------------------------------------------------------------- /src/sensors/Sensors.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | struct DepthData 24 | { 25 | unsigned int height; 26 | unsigned int width; 27 | double scale; 28 | double hfov; 29 | double vfov; 30 | std::vector depth_buffer; 31 | }; 32 | 33 | class DepthCamera 34 | { 35 | public: 36 | unsigned int get_height(); 37 | unsigned int get_width(); 38 | double get_scale(); 39 | std::shared_ptr read(); 40 | 41 | virtual std::vector &get_depth_buffer() = 0; 42 | virtual double get_horizontal_fov() { return hfov; }; 43 | virtual double get_vertical_fov() { return vfov; }; 44 | 45 | protected: 46 | unsigned int height = 0; 47 | unsigned int width = 0; 48 | double scale = 0; 49 | double hfov = 0.0; 50 | double vfov = 0.0; 51 | }; 52 | -------------------------------------------------------------------------------- /src/vehicles/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | 3 | set(SOURCES MavQuadCopter.cc) 4 | set(HEADERS 5 | Vehicles.hh 6 | MavQuadCopter.hh) 7 | 8 | export_headers("${HEADERS}" "vehicles") 9 | set(COAV_INCLUDE_LIST "${COAV_INCLUDE_LIST}${INCLUDE_LIST}" PARENT_SCOPE) 10 | 11 | add_library(vehicles OBJECT ${SOURCES}) 12 | -------------------------------------------------------------------------------- /src/vehicles/MavQuadCopter.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include "MavQuadCopter.hh" 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace defaults 26 | { 27 | const uint16_t local_port = 15557; 28 | const double wp_equal_dist_m = 0.001; 29 | } 30 | 31 | MavQuadCopter::MavQuadCopter() : MavQuadCopter(defaults::local_port) 32 | { 33 | } 34 | 35 | MavQuadCopter::MavQuadCopter(uint16_t local_port) 36 | { 37 | // Socket Initialization 38 | this->sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); 39 | if (sock == -1) { 40 | perror("[MavQuadCopter] error opening socket"); 41 | exit(EXIT_FAILURE); 42 | } 43 | 44 | this->local_addr.sin_family = AF_INET; 45 | this->local_addr.sin_addr.s_addr = INADDR_ANY; 46 | this->local_addr.sin_port = htons(local_port); 47 | 48 | if (bind(sock, (struct sockaddr *)&local_addr, sizeof(struct sockaddr)) == 49 | -1) { 50 | perror("[MavQuadCopter] error binding to port"); 51 | close(sock); 52 | exit(EXIT_FAILURE); 53 | } 54 | 55 | // Attempt to make it non blocking 56 | if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) { 57 | perror("[MavQuadCopter] error setting socket as nonblocking"); 58 | close(this->sock); 59 | exit(EXIT_FAILURE); 60 | } 61 | 62 | // Instantiate mav_vehicle 63 | this->mav = std::make_shared(sock); 64 | std::cout << "[MavQuadCopter] mavlink_vehicle instantiated" << std::endl; 65 | 66 | // Initialize mav_vehicle update thread 67 | this->thread_run = true; 68 | this->thread = std::thread(&MavQuadCopter::run, this); 69 | } 70 | 71 | MavQuadCopter::~MavQuadCopter() 72 | { 73 | this->thread_run = false; 74 | } 75 | 76 | void MavQuadCopter::run() 77 | { 78 | while (this->thread_run) { 79 | 80 | // Update vehicle state 81 | this->mav->update(); 82 | 83 | // Check if vehicle is ready 84 | if (!this->mav->is_ready()) { 85 | continue; 86 | } 87 | 88 | mavlink_vehicles::arm_status arm_stat = this->mav->get_arm_status(); 89 | mavlink_vehicles::status status = this->mav->get_status(); 90 | mavlink_vehicles::mode mode = this->mav->get_mode(); 91 | 92 | switch (this->vehicle_state) { 93 | case INIT: { 94 | 95 | if (status == mavlink_vehicles::status::STANDBY) { 96 | this->vehicle_state = INIT_ON_GROUND; 97 | } else if (status == mavlink_vehicles::status::ACTIVE) { 98 | this->vehicle_state = ACTIVE_AIRBORNE; 99 | } 100 | 101 | continue; 102 | } 103 | case INIT_ON_GROUND: { 104 | 105 | if (!this->autotakeoff) { 106 | this->vehicle_state = ACTIVE_ON_GROUND; 107 | continue; 108 | } 109 | 110 | // Execute takeoff procedures 111 | if (mode != mavlink_vehicles::mode::GUIDED) { 112 | this->mav->set_mode(mavlink_vehicles::mode::GUIDED); 113 | continue; 114 | } 115 | 116 | if (arm_stat != mavlink_vehicles::arm_status::ARMED) { 117 | this->mav->arm_throttle(); 118 | continue; 119 | } 120 | 121 | if (status != mavlink_vehicles::status::ACTIVE) { 122 | this->mav->takeoff(); 123 | continue; 124 | } 125 | 126 | // Takeoff succeeded 127 | this->vehicle_state = ACTIVE_AIRBORNE; 128 | continue; 129 | } 130 | case ACTIVE_ON_GROUND: { 131 | 132 | if (status == mavlink_vehicles::status::ACTIVE) { 133 | this->vehicle_state = ACTIVE_AIRBORNE; 134 | } 135 | 136 | continue; 137 | } 138 | case ACTIVE_AIRBORNE: { 139 | 140 | continue; 141 | } 142 | } 143 | } 144 | } 145 | 146 | Pose MavQuadCopter::target_pose() 147 | { 148 | using namespace mavlink_vehicles; 149 | 150 | // Convert from global_pos_int to local_pos_enu and return 151 | local_pos target_pos_ned = mavlink_vehicles::math::global_to_local_ned( 152 | this->mav->get_mission_waypoint(), this->mav->get_home_position_int()); 153 | 154 | Pose target_pose; 155 | target_pose.pos.x = target_pos_ned.x; 156 | target_pose.pos.y = target_pos_ned.y; 157 | target_pose.pos.z = -target_pos_ned.z; 158 | target_pose.rot.x = 0; 159 | target_pose.rot.y = 0; 160 | target_pose.rot.z = 0; 161 | target_pose.rot.w = 0; 162 | 163 | return target_pose; 164 | } 165 | 166 | Pose MavQuadCopter::vehicle_pose() 167 | { 168 | using namespace mavlink_vehicles; 169 | 170 | // Convert from global_pos_int to local_pos_enu and return 171 | local_pos vehicle_pos_ned = this->mav->get_local_position_ned(); 172 | attitude vehicle_att = this->mav->get_attitude(); 173 | Pose vehicle_pose; 174 | 175 | // Convert position from NED to ENU and set 176 | vehicle_pose.pos.x = vehicle_pos_ned.y; 177 | vehicle_pose.pos.y = vehicle_pos_ned.x; 178 | vehicle_pose.pos.z = -vehicle_pos_ned.z; 179 | 180 | // Converting rotation from NED to ENU 181 | std::swap(vehicle_att.roll, vehicle_att.pitch); 182 | vehicle_att.yaw = -vehicle_att.yaw; 183 | 184 | // Set rotation 185 | vehicle_pose.set_rot(vehicle_att.roll, vehicle_att.pitch, vehicle_att.yaw); 186 | 187 | return vehicle_pose; 188 | } 189 | 190 | void MavQuadCopter::set_target_pose(Pose pose) 191 | { 192 | // Convert from local coordinates to global coordinates 193 | mavlink_vehicles::global_pos_int global_coord = 194 | mavlink_vehicles::math::local_ned_to_global( 195 | mavlink_vehicles::local_pos(pose.pos.y, pose.pos.x, -pose.pos.z), 196 | this->mav->get_home_position_int()); 197 | 198 | this->mav->send_detour_waypoint(global_coord); 199 | } 200 | 201 | void MavQuadCopter::rotate(double angle_deg) 202 | { 203 | this->mav->rotate(angle_deg); 204 | } 205 | 206 | bool MavQuadCopter::detour_finished() 207 | { 208 | return this->mav->get_mode() == mavlink_vehicles::mode::AUTO; 209 | } 210 | 211 | -------------------------------------------------------------------------------- /src/vehicles/MavQuadCopter.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include "vehicles/Vehicles.hh" 26 | 27 | class MavQuadCopter : public QuadCopter 28 | { 29 | public: 30 | MavQuadCopter(uint16_t local_port); 31 | MavQuadCopter(uint16_t local_port, bool autorotate, bool autotakeoff); 32 | MavQuadCopter(); 33 | ~MavQuadCopter(); 34 | 35 | Pose target_pose() override; 36 | Pose vehicle_pose() override; 37 | void set_target_pose(Pose pose) override; 38 | void rotate(double angle_deg); 39 | bool detour_finished(); 40 | 41 | // Mavlink vehicle 42 | std::shared_ptr mav; 43 | 44 | private: 45 | 46 | // Connection 47 | int sock = 0; 48 | socklen_t fromlen = {0}; 49 | struct sockaddr_in local_addr = {0}; 50 | struct sockaddr_in remote_addr = {0}; 51 | 52 | // Run Thread 53 | void run(); 54 | std::thread thread; 55 | bool thread_run = false; 56 | 57 | // Vehicle state 58 | enum vstate { INIT, INIT_ON_GROUND, ACTIVE_ON_GROUND, ACTIVE_AIRBORNE }; 59 | vstate vehicle_state = INIT; 60 | 61 | bool autotakeoff = false; 62 | }; 63 | 64 | -------------------------------------------------------------------------------- /src/vehicles/Vehicles.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Intel Corporation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include "common/common.hh" 20 | 21 | class QuadCopter 22 | { 23 | public: 24 | virtual Pose target_pose() = 0; 25 | virtual Pose vehicle_pose() = 0; 26 | virtual void set_target_pose(Pose pose) = 0; 27 | }; 28 | -------------------------------------------------------------------------------- /testbed/coav-sim.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # Copyright (c) 2017 Intel Corporation 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | set -e 17 | 18 | SCRIPT_DIR=$(dirname $(realpath ${BASH_SOURCE[0]})) 19 | 20 | # Set autopilot 21 | # Supported Autopilots: AP_PX4 and AP_APM 22 | AUTOPILOT=${AUTOPILOT:-"AP_PX4"} 23 | 24 | # 0: Log only 25 | # 1: Log + stdout 26 | # 2: stdout only 27 | # 3: no output 28 | VERBOSE_LEVEL=${VERBOSE_LEVEL:-0} 29 | 30 | # ArduCopter Variables 31 | APM_CMD="${APM_DIR:+"${APM_DIR}/"}arducopter --model x --defaults ./copter.parm" 32 | APM_TCP_PORT_1=5760 33 | APM_TCP_PORT_2=5762 34 | APM_PARM_URL=${APM_PARM_URL:-"https://raw.githubusercontent.com/ArduPilot/ardupilot/master/Tools/autotest/default_params/copter.parm"} 35 | 36 | # PX4 Variables 37 | PX4_DIR=${PX4_DIR:-"~/px4/Firmware"} 38 | PX4_CMD="setsid make posix_sitl_default jmavsim" 39 | PX4_UDP_PORT_1=14550 40 | PX4_UDP_PORT_2=14540 41 | 42 | # Simulation Parameters 43 | WORLD=${WORLD:-"simple_obstacle.sdf"} 44 | LOGDIR=${LOGDIR:-"${SCRIPT_DIR}/logs"} 45 | 46 | # Collision Avoidance and Gazebo variables 47 | GZSITL_UDP_PORT=15556 48 | COAV_GCS_UDP_PORT=15557 49 | 50 | run_and_log() { 51 | cmd=$1 52 | log_out="${2}.log" 53 | log_err="${2}_err.log" 54 | 55 | # Log to file 56 | if [ "$VERBOSE_LEVEL" -eq 0 ]; then 57 | $cmd > "${LOGDIR}/${log_out}" 2> "${LOGDIR}/${log_err}" & 58 | # Log to file and stdout 59 | elif [ "$VERBOSE_LEVEL" -eq 1 ]; then 60 | $cmd > >(tee "${LOGDIR}/${log_out}") 2> >(tee "${LOGDIR}/${log_err}") & 61 | # Log to stdout only 62 | elif [ "$VERBOSE_LEVEL" -eq 2 ]; then 63 | $cmd & 64 | # No output 65 | else 66 | $cmd > /dev/null 2> /dev/null & 67 | fi 68 | } 69 | 70 | run_autopilot () { 71 | # Run SITL Simulator 72 | if [ "$AUTOPILOT" = "AP_PX4" ]; then 73 | cd $PX4_DIR 74 | 75 | run_and_log "$PX4_CMD" "autopilot" 76 | 77 | SITLID=-$! 78 | cd - > /dev/null 79 | 80 | # Wait for sitl 81 | sleep 15 82 | elif [ "$AUTOPILOT" = "AP_APM" ]; then 83 | cd $SCRIPT_DIR # sitl must run in the same dir of "eeprom.bin" 84 | 85 | # Fetch copter parameters file 86 | if [ ! -f copter.parm ]; then 87 | wget ${APM_PARM_URL} 88 | echo "WPNAV_SPEED 100" >> ./copter.parm 89 | wait $! 90 | 91 | if [ ! -f copter.parm ]; then 92 | echo "Couldn't fetch ardupilot's parm file." 93 | exit 1 94 | fi 95 | fi 96 | 97 | run_and_log "$APM_CMD" "autopilot" 98 | 99 | SITLID=$! 100 | cd - > /dev/null 101 | 102 | # Wait for sitl 103 | sleep 5 104 | fi 105 | } 106 | 107 | run_gazebo () { 108 | # Gazebo engine without GUI. 109 | # The log can be played through `gazebo -p logfile` 110 | SDFFILE="${SCRIPT_DIR}/worlds/${WORLD}" 111 | GZCMD="gzserver --record_path $LOGDIR --verbose $SDFFILE" 112 | run_and_log "$GZCMD" "gzserver" 113 | GZID=$! 114 | 115 | # Wait until gazebo is up and running 116 | sleep 8 117 | } 118 | 119 | run_coav_control() { 120 | # Run the collision avoidance 121 | COAVCMD="../build/tools/coav-control/coav-control "$@"" 122 | run_and_log "$COAVCMD" "coav-control" 123 | COAVID=$! 124 | 125 | # Wait until is up and running 126 | sleep 1 127 | } 128 | 129 | simulate () { 130 | run_coav_control "$@" 131 | run_autopilot 132 | run_gazebo 133 | 134 | # Bidirectional bridge between autopilot and gazebo-sitl 135 | SOCAT_ARG_2="udp:localhost:$GZSITL_UDP_PORT" 136 | if [ "$AUTOPILOT" = "AP_PX4" ]; then 137 | SOCAT_ARG_1="udp-listen:$PX4_UDP_PORT_1" 138 | elif [ "$AUTOPILOT" = "AP_APM" ]; then 139 | SOCAT_ARG_1="tcp:localhost:$APM_TCP_PORT_1" 140 | fi 141 | 142 | socat $SOCAT_ARG_1 $SOCAT_ARG_2 & 143 | GZSITL_SOCATID=$! 144 | 145 | # Wait for gzsitl-sitl connection to be established 146 | sleep 2 147 | 148 | # Bidirectional bridge between the autopilot and the coav_gcs 149 | SOCAT_ARG_2="udp:localhost:$COAV_GCS_UDP_PORT" 150 | if [ "$AUTOPILOT" = "AP_PX4" ]; then 151 | SOCAT_ARG_1="udp-listen:$PX4_UDP_PORT_2" 152 | elif [ "$AUTOPILOT" = "AP_APM" ]; then 153 | SOCAT_ARG_1="tcp:localhost:$APM_TCP_PORT_2" 154 | fi 155 | 156 | socat $SOCAT_ARG_1 $SOCAT_ARG_2 & 157 | COAV_SOCATID=$! 158 | 159 | # Wait for child process 160 | wait 161 | } 162 | 163 | silentkill () { 164 | if [ ! -z $2 ]; then 165 | kill $2 $1 > /dev/null 2>&1 || true 166 | else 167 | kill -KILL $1 > /dev/null 2>&1 || true 168 | fi 169 | } 170 | 171 | cleanup () { 172 | silentkill $GZSITL_SOCATID # Kill gzsitl socat 173 | silentkill $COAV_SOCATID # Kill coav_gcs socat 174 | silentkill $COAVID # Kill coav-control sitl 175 | silentkill $SITLID # Kill sitl 176 | silentkill $GZID -INT && sleep 3 # Wait gzserver to save the log 177 | silentkill $GZID # Kill gzserver 178 | } 179 | 180 | test_dep () { 181 | command -v $1 > /dev/null 2>&1 || { echo >&2 "Error: command '$1' not found"; exit 1; } 182 | } 183 | 184 | check_deps () { 185 | test_dep gzserver 186 | test_dep socat 187 | test_dep wget 188 | if [ "$AUTOPILOT" = "AP_APM" ]; then 189 | test_dep $APM_CMD 190 | fi 191 | } 192 | 193 | test_dir () { 194 | [ -d $1 ] > /dev/null 2>&1 || { echo >&2 "Error: directory '$1' not found"; exit 1; } 195 | } 196 | 197 | check_dirs () { 198 | test_dir $SCRIPT_DIR 199 | if [ "$AUTOPILOT" = "AP_PX4" ]; then 200 | test_dir $PX4_DIR 201 | fi 202 | } 203 | 204 | trap cleanup EXIT 205 | trap "exit 0" SIGHUP SIGINT SIGTERM 206 | 207 | check_deps 208 | check_dirs 209 | 210 | if [ ! -d ${LOGDIR} ]; then 211 | # Create logdir if it does not exist 212 | mkdir -p ${LOGDIR} 213 | fi 214 | 215 | simulate "$@" 216 | -------------------------------------------------------------------------------- /testbed/detect_collision.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 Intel Corporation 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import os 17 | import signal 18 | import sys 19 | 20 | 21 | if __name__ == '__main__': 22 | timeout = int(sys.argv[1]) 23 | 24 | def handler(signum, frame): 25 | sys.exit(0) 26 | 27 | signal.signal(signal.SIGALRM, handler) 28 | signal.signal(signal.SIGINT, handler) 29 | signal.alarm(timeout) 30 | 31 | data = "" 32 | while "contact" not in str(data): # 'data' has type 'bytes' on python3 33 | # using os.read istead of readiline because of the bug: 34 | # https://bugs.python.org/issue9504 35 | data = os.read(sys.stdin.fileno(), 4096) 36 | 37 | sys.exit(1) 38 | -------------------------------------------------------------------------------- /testbed/detect_takeoff.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 Intel Corporation 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import math 17 | import sys 18 | import re 19 | import signal 20 | 21 | def dist(x, y, z): 22 | return math.sqrt(x * x + y * y + z * z) 23 | 24 | if __name__ == '__main__': 25 | takeoff_distance = float(sys.argv[1]) 26 | takeoff_timeout = int(sys.argv[2]) 27 | pos_re = re.compile("position\ \{\ *x\:(.+)y\:(.+)z\:(.+)\}\ orientation.*") 28 | 29 | def handler(signum, frame): 30 | sys.exit(0) 31 | 32 | def timeout_handler(signum, frame): 33 | raise Exception("Takeoff not detected before timeout") 34 | 35 | signal.signal(signal.SIGINT, handler) 36 | signal.signal(signal.SIGALRM, timeout_handler) 37 | signal.alarm(takeoff_timeout) 38 | 39 | try: 40 | for line in sys.stdin: 41 | m = pos_re.match(line) 42 | if not m: 43 | continue 44 | x, y, z = m.groups() 45 | if dist(float(x), float(y), float(z)) >= takeoff_distance: 46 | exit(0) 47 | except Exception as e: 48 | exit(0) 49 | 50 | exit(1) 51 | -------------------------------------------------------------------------------- /testbed/eeprom.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/collision-avoidance-library/11620d99f3ce6587f24173f03185167aed595a81/testbed/eeprom.bin -------------------------------------------------------------------------------- /testbed/simple_mission.mission: -------------------------------------------------------------------------------- 1 | { 2 | "MAV_AUTOPILOT": 3, 3 | "complexItems": [ 4 | ], 5 | "groundStation": "QGroundControl", 6 | "items": [ 7 | { 8 | "autoContinue": true, 9 | "command": 22, 10 | "coordinate": [ 11 | -35.363231480000003, 12 | 149.16499868, 13 | 2 14 | ], 15 | "frame": 2, 16 | "id": 1, 17 | "param1": 15, 18 | "param2": 0, 19 | "param3": 0, 20 | "param4": 0, 21 | "type": "missionItem" 22 | }, 23 | { 24 | "autoContinue": true, 25 | "command": 16, 26 | "coordinate": [ 27 | -35.363065237186049, 28 | 149.16492895048492, 29 | 2 30 | ], 31 | "frame": 3, 32 | "id": 2, 33 | "param1": 0, 34 | "param2": 0, 35 | "param3": 0, 36 | "param4": 0, 37 | "type": "missionItem" 38 | }, 39 | { 40 | "autoContinue": true, 41 | "command": 16, 42 | "coordinate": [ 43 | -35.362920870000004, 44 | 149.16526153999999, 45 | 2 46 | ], 47 | "frame": 3, 48 | "id": 3, 49 | "param1": 0, 50 | "param2": 0, 51 | "param3": 0, 52 | "param4": 0, 53 | "type": "missionItem" 54 | }, 55 | { 56 | "autoContinue": true, 57 | "command": 16, 58 | "coordinate": [ 59 | -35.362759009999998, 60 | 149.16501478000001, 61 | 2 62 | ], 63 | "frame": 3, 64 | "id": 4, 65 | "param1": 0, 66 | "param2": 0, 67 | "param3": 0, 68 | "param4": 0, 69 | "type": "missionItem" 70 | }, 71 | { 72 | "autoContinue": true, 73 | "command": 16, 74 | "coordinate": [ 75 | -35.3625884, 76 | 149.16522398999999, 77 | 2 78 | ], 79 | "frame": 3, 80 | "id": 5, 81 | "param1": 0, 82 | "param2": 0, 83 | "param3": 0, 84 | "param4": 0, 85 | "type": "missionItem" 86 | }, 87 | { 88 | "autoContinue": true, 89 | "command": 16, 90 | "coordinate": [ 91 | -35.362448409999999, 92 | 149.16495577000001, 93 | 2 94 | ], 95 | "frame": 3, 96 | "id": 6, 97 | "param1": 0, 98 | "param2": 0, 99 | "param3": 0, 100 | "param4": 0, 101 | "type": "missionItem" 102 | }, 103 | { 104 | "autoContinue": true, 105 | "command": 16, 106 | "coordinate": [ 107 | -35.362304039999998, 108 | 149.16519717, 109 | 2 110 | ], 111 | "frame": 3, 112 | "id": 7, 113 | "param1": 0, 114 | "param2": 0, 115 | "param3": 0, 116 | "param4": 0, 117 | "type": "missionItem" 118 | }, 119 | { 120 | "autoContinue": true, 121 | "command": 16, 122 | "coordinate": [ 123 | -35.362164049999997, 124 | 149.16494503999999, 125 | 2 126 | ], 127 | "frame": 3, 128 | "id": 8, 129 | "param1": 0, 130 | "param2": 0, 131 | "param3": 0, 132 | "param4": 0, 133 | "type": "missionItem" 134 | } 135 | ], 136 | "plannedHomePosition": { 137 | "autoContinue": true, 138 | "command": 16, 139 | "coordinate": [ 140 | -35.3632621, 141 | 149.1652374, 142 | 584.08000000000004 143 | ], 144 | "frame": 0, 145 | "id": 0, 146 | "param1": 0, 147 | "param2": 0, 148 | "param3": 0, 149 | "param4": 0, 150 | "type": "missionItem" 151 | }, 152 | "version": "1.0" 153 | } 154 | -------------------------------------------------------------------------------- /testbed/terrain/S36E149.DAT: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/intel/collision-avoidance-library/11620d99f3ce6587f24173f03185167aed595a81/testbed/terrain/S36E149.DAT -------------------------------------------------------------------------------- /testbed/testbed.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # Copyright (c) 2016 Intel Corporation 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | SCRIPT_DIR=$(dirname $(realpath ${BASH_SOURCE[0]})) 17 | SIM_ARGS="-d DI_OBSTACLE -a QC_SHIFT_AVOIDANCE -s ST_GAZEBO_REALSENSE" 18 | 19 | silentkill () { 20 | if [ ! -z $2 ]; then 21 | kill $2 $1 > /dev/null 2>&1 || true 22 | else 23 | kill -KILL $1 > /dev/null 2>&1 && wait $1 2> /dev/null || true 24 | fi 25 | } 26 | 27 | sleep_until_takeoff () { 28 | gz topic -u -e /gazebo/default/gzsitl_quadcopter_rs/vehicle_pose \ 29 | | python detect_takeoff.py $1 $2 # Detect takeoff on distance $1 30 | } 31 | 32 | listen_collision () { 33 | SLEEPTIME=$1 34 | TOPIC=/gazebo/default/gzsitl_quadcopter_rs/sensors/contact/contacts 35 | gz topic -u -e $TOPIC | python detect_collision.py $SLEEPTIME 36 | } 37 | 38 | testcase () { 39 | WORLD=$1 40 | SLEEPTIME=$2 41 | 42 | WORLD=$WORLD LOGDIR="${SCRIPT_DIR}/output/${1}" ./coav-sim.sh $SIM_ARGS > /dev/null 2>&1 & 43 | COAVSIM_ID=$! 44 | sleep 30 45 | 46 | # Detect takeoff on a distance from origin >= 0.5 meters 47 | # Fail test if no takeoff is detect after 5 seconds 48 | sleep_until_takeoff 0.5 10 49 | if [ $? -ne 0 ]; then 50 | echo "[${WORLD}] FAIL! Takeoff not detected in time" 51 | cleanup 52 | return 53 | fi 54 | 55 | # Maximum acceptable time for this particular mission 56 | listen_collision $SLEEPTIME \ 57 | && echo "[${WORLD}] OK!" || echo "[${WORLD}] FAIL! Collision detected" 58 | 59 | cleanup 60 | } 61 | 62 | runtests () { 63 | mkdir -p "${SCRIPT_DIR}/output" 64 | 65 | testcase simple.sdf 30 66 | testcase simple_obstacle.sdf 40 67 | } 68 | 69 | replay () { 70 | gazebo -p "${SCRIPT_DIR}/output/${1}/state.log" 71 | } 72 | 73 | cleanup () { 74 | silentkill $COAVSIM_ID -TERM # Terminate coav-sim.sh 75 | wait $COAVSIM_ID # Just because we know TERM will exit coav-sim.sh 76 | } 77 | 78 | cleanup_and_exit () { 79 | cleanup 80 | exit 0 81 | } 82 | 83 | trap cleanup_and_exit SIGINT SIGTERM 84 | 85 | if [ -z "$1" ]; then 86 | # TODO: add help text 87 | runtests # default subcommand 88 | else 89 | $@ # replay scene-name 90 | fi 91 | -------------------------------------------------------------------------------- /testbed/worlds/models/L-wall/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | L-wall 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /testbed/worlds/models/L-wall/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.459536 0.004219 1.4 0 -0 0 6 | 0 7 | 0 8 | 0 9 | 10 | 11 | nan 12 | 0 13 | 0 14 | nan 15 | 0 16 | nan 17 | 18 | 0 19 | 0 0 0 0 -0 0 20 | 21 | 22 | 0 23 | 24 | 25 | 8.40378 0.2 2.8 26 | 27 | 28 | 29 | 34 | 35 | 0 0 0 0 -0 0 36 | 0 37 | 38 | 39 | 0 40 | 10 41 | 0 0 0 0 -0 0 42 | 43 | 44 | 8.40378 0.2 2.8 45 | 46 | 47 | 48 | 49 | 50 | 1 51 | 1 52 | 0 0 0 53 | 0 54 | 0 55 | 56 | 57 | 1 58 | 0 59 | 0 60 | 1 61 | 62 | 0 63 | 64 | 65 | 66 | 67 | 0 68 | 1e+06 69 | 70 | 71 | 0 72 | 1 73 | 1 74 | 75 | 0 76 | 0.2 77 | 1e+13 78 | 1 79 | 0.01 80 | 0 81 | 82 | 83 | 1 84 | -0.01 85 | 0 86 | 0.2 87 | 1e+13 88 | 1 89 | 90 | 91 | 92 | 93 | 94 | 95 | -3.68857 -9.95622 1.4 0 0 -1.5747 96 | 0 97 | 0 98 | 0 99 | 100 | 101 | nan 102 | 0 103 | 0 104 | nan 105 | 0 106 | nan 107 | 108 | 0 109 | 0 0 0 0 -0 0 110 | 111 | 112 | 0 113 | 114 | 115 | 19.8548 0.2 2.8 116 | 117 | 118 | 119 | 124 | 125 | 126 | 0 0 0 0 -0 0 127 | 0 128 | 129 | 130 | 0 131 | 10 132 | 0 0 0 0 -0 0 133 | 134 | 135 | 19.8548 0.2 2.8 136 | 137 | 138 | 139 | 140 | 141 | 1 142 | 1 143 | 0 0 0 144 | 0 145 | 0 146 | 147 | 148 | 1 149 | 0 150 | 0 151 | 1 152 | 153 | 0 154 | 155 | 156 | 157 | 158 | 0 159 | 1e+06 160 | 161 | 162 | 0 163 | 1 164 | 1 165 | 166 | 0 167 | 0.2 168 | 1e+13 169 | 1 170 | 0.01 171 | 0 172 | 173 | 174 | 1 175 | -0.01 176 | 0 177 | 0.2 178 | 1e+13 179 | 1 180 | 181 | 182 | 183 | 184 | 185 | 1 186 | 1 187 | 188 | 189 | -------------------------------------------------------------------------------- /testbed/worlds/models/office_simple/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | office_simple 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /testbed/worlds/models/office_simple/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -1.95575 1.34148 3.39662 0 -0 0 6 | 0 7 | 0 8 | 0 9 | 10 | 11 | nan 12 | 0 13 | 0 14 | nan 15 | 0 16 | nan 17 | 18 | 0 19 | 0 0 0 0 -0 0 20 | 21 | 22 | 0 23 | 24 | 25 | 11.6509 0.2 5.63018 26 | 27 | 28 | 29 | 34 | 35 | 0 0 0 0 -0 0 36 | 0 37 | 38 | 39 | 0 40 | 10 41 | 0 0 0 0 -0 0 42 | 43 | 44 | 11.6509 0.2 5.63018 45 | 46 | 47 | 48 | 49 | 50 | 1 51 | 1 52 | 0 0 0 53 | 0 54 | 0 55 | 56 | 57 | 1 58 | 0 59 | 0 60 | 1 61 | 62 | 0 63 | 64 | 65 | 66 | 67 | 0 68 | 1e+06 69 | 70 | 71 | 0 72 | 1 73 | 1 74 | 75 | 0 76 | 0.2 77 | 1e+13 78 | 1 79 | 0.01 80 | 0 81 | 82 | 83 | 1 84 | -0.01 85 | 0 86 | 0.2 87 | 1e+13 88 | 1 89 | 90 | 91 | 92 | 93 | 94 | 95 | 3.71982 7.10283 3.39662 0 0 -1.56276 96 | 0 97 | 0 98 | 0 99 | 100 | 101 | nan 102 | 0 103 | 0 104 | nan 105 | 0 106 | nan 107 | 108 | 0 109 | 0 0 0 0 -0 0 110 | 111 | 112 | 0 113 | 114 | 115 | 11.6509 0.2 5.63018 116 | 117 | 118 | 119 | 124 | 125 | 126 | 0 0 0 0 -0 0 127 | 0 128 | 129 | 130 | 0 131 | 10 132 | 0 0 0 0 -0 0 133 | 134 | 135 | 11.6509 0.2 5.63018 136 | 137 | 138 | 139 | 140 | 141 | 1 142 | 1 143 | 0 0 0 144 | 0 145 | 0 146 | 147 | 148 | 1 149 | 0 150 | 0 151 | 1 152 | 153 | 0 154 | 155 | 156 | 157 | 158 | 0 159 | 1e+06 160 | 161 | 162 | 0 163 | 1 164 | 1 165 | 166 | 0 167 | 0.2 168 | 1e+13 169 | 1 170 | 0.01 171 | 0 172 | 173 | 174 | 1 175 | -0.01 176 | 0 177 | 0.2 178 | 1e+13 179 | 1 180 | 181 | 182 | 183 | 184 | 185 | 186 | -7.7194 7.10341 3.39662 0 0 -1.56276 187 | 0 188 | 0 189 | 0 190 | 191 | 192 | nan 193 | 0 194 | 0 195 | nan 196 | 0 197 | nan 198 | 199 | 0 200 | 0 0 0 0 -0 0 201 | 202 | 203 | 0 204 | 205 | 206 | 11.6509 0.2 5.63018 207 | 208 | 209 | 210 | 215 | 216 | 217 | 0 0 0 0 -0 0 218 | 0 219 | 220 | 221 | 0 222 | 10 223 | 0 0 0 0 -0 0 224 | 225 | 226 | 11.6509 0.2 5.63018 227 | 228 | 229 | 230 | 231 | 232 | 1 233 | 1 234 | 0 0 0 235 | 0 236 | 0 237 | 238 | 239 | 1 240 | 0 241 | 0 242 | 1 243 | 244 | 0 245 | 246 | 247 | 248 | 249 | 0 250 | 1e+06 251 | 252 | 253 | 0 254 | 1 255 | 1 256 | 257 | 0 258 | 0.2 259 | 1e+13 260 | 1 261 | 0.01 262 | 0 263 | 264 | 265 | 1 266 | -0.01 267 | 0 268 | 0.2 269 | 1e+13 270 | 1 271 | 272 | 273 | 274 | 275 | 276 | 277 | -2.09862 7.04327 5.73647 1.5726 -0 -1.56276 278 | 0 279 | 0 280 | 0 281 | 282 | 283 | nan 284 | 0 285 | 0 286 | nan 287 | 0 288 | nan 289 | 290 | 0 291 | 0 0 0 0 -0 0 292 | 293 | 294 | 0 295 | 296 | 297 | 11.4489 0.212066 11.2025 298 | 299 | 300 | 301 | 306 | 307 | 308 | 0 0 0 0 -0 0 309 | 0 310 | 311 | 312 | 0 313 | 10 314 | 0 0 0 0 -0 0 315 | 316 | 317 | 11.4489 0.212066 11.2025 318 | 319 | 320 | 321 | 322 | 323 | 1 324 | 1 325 | 0 0 0 326 | 0 327 | 0 328 | 329 | 330 | 1 331 | 0 332 | 0 333 | 1 334 | 335 | 0 336 | 337 | 338 | 339 | 340 | 0 341 | 1e+06 342 | 343 | 344 | 0 345 | 1 346 | 1 347 | 348 | 0 349 | 0.2 350 | 1e+13 351 | 1 352 | 0.01 353 | 0 354 | 355 | 356 | 1 357 | -0.01 358 | 0 359 | 0.2 360 | 1e+13 361 | 1 362 | 363 | 364 | 365 | 366 | 367 | 368 | -2.05377 12.8226 3.39662 0 -0 0 369 | 0 370 | 0 371 | 0 372 | 373 | 374 | nan 375 | 0 376 | 0 377 | nan 378 | 0 379 | nan 380 | 381 | 0 382 | 0 0 0 0 -0 0 383 | 384 | 385 | 0 386 | 387 | 388 | 11.6509 0.2 5.63018 389 | 390 | 391 | 392 | 397 | 398 | 399 | 0 0 0 0 -0 0 400 | 0 401 | 402 | 403 | 0 404 | 10 405 | 0 0 0 0 -0 0 406 | 407 | 408 | 11.6509 0.2 5.63018 409 | 410 | 411 | 412 | 413 | 414 | 1 415 | 1 416 | 0 0 0 417 | 0 418 | 0 419 | 420 | 421 | 1 422 | 0 423 | 0 424 | 1 425 | 426 | 0 427 | 428 | 429 | 430 | 431 | 0 432 | 1e+06 433 | 434 | 435 | 0 436 | 1 437 | 1 438 | 439 | 0 440 | 0.2 441 | 1e+13 442 | 1 443 | 0.01 444 | 0 445 | 446 | 447 | 1 448 | -0.01 449 | 0 450 | 0.2 451 | 1e+13 452 | 1 453 | 454 | 455 | 456 | 457 | 458 | 1 459 | 1 460 | 461 | 462 | -------------------------------------------------------------------------------- /testbed/worlds/simple.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.1 0.1 0.1 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.5 -1 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 | 0 61 | 1 62 | 63 | 64 | 0 0 -9.8 65 | 6e-06 2.3e-05 -4.2e-05 66 | 67 | 68 | 0.001 69 | 1 70 | 1000 71 | 72 | 73 | 0.4 0.4 0.4 1 74 | 0.7 0.7 0.7 1 75 | 1 76 | 77 | 80 | 81 | 82 | EARTH_WGS84 83 | 0 84 | 0 85 | 0 86 | 0 87 | 88 | 89 | 90 | 0 0 0.182466 0 -0 1.5708 91 | 92 | 1.316 93 | 94 | 0.0128 95 | 0 96 | 0 97 | 0.0128 98 | 0 99 | 0.0218 100 | 101 | 102 | 103 | 104 | 105 | model://quadrotor/meshes/quadrotor_base.dae 106 | 1 1 1 107 | 108 | 109 | 10 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | model://quadrotor/meshes/quadrotor_base.dae 127 | 1 1 1 128 | 129 | 130 | 131 | 0 132 | 0 133 | 0 134 | 1 135 | 136 | 137 | gzsitl_perm_targ_ctrl 138 | gzsitl_perm_targ_vis 139 | target_pose 140 | gzsitl_subs_targ_ctrl 141 | gzsitl_subs_targ_vis 142 | coav/coav_target_pose 143 | vehicle_pose 144 | 145 | 0 146 | 147 | 148 | 1 149 | 150 | 151 | 0.65 0.65 0.23 152 | 153 | 154 | 0 0 0.115 0 -0 0.785 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 5 170 | 171 | contact_geometry 172 | __default_topic__ 173 | 174 | 175 | 0 0 0 0 -0 0 176 | 0 177 | 178 | 179 | 1 180 | 0 181 | 0 182 | 1 183 | 0 184 | 1 185 | 186 | 1 187 | 188 | 0 189 | 0 190 | 1 191 | 192 | 193 | gzsitl_quadcopter::sensors 194 | gzsitl_quadcopter::quadrotor::link 195 | 196 | 197 | 198 | 199 | 0.0615752 200 | 201 | 9.108e-05 202 | 0 203 | 0 204 | 2.51e-06 205 | 0 206 | 8.931e-05 207 | 208 | 0 0 0 0 -0 0 209 | 210 | 0 211 | 0 212 | 0 213 | 1 214 | 215 | 216 | 217 | model://realsense_camera/meshes/realsense.dae 218 | 1 1 1 219 | 220 | 221 | 222 | 223 | 0 224 | 10 225 | 0 0 0 0 -0 0 226 | 227 | 228 | 0.0078 0.13 0.0192 229 | 230 | 231 | 232 | 233 | 234 | 1 235 | 1 236 | 0 0 0 237 | 0 238 | 0 239 | 240 | 241 | 1 242 | 0 243 | 0 244 | 1 245 | 246 | 0 247 | 248 | 249 | 250 | 251 | 0 252 | 1e+06 253 | 254 | 255 | 0 256 | 1 257 | 1 258 | 259 | 0 260 | 0.2 261 | 1e+13 262 | 1 263 | 0.01 264 | 0 265 | 266 | 267 | 1 268 | -0.01 269 | 0 270 | 0.2 271 | 1e+13 272 | 1 273 | 274 | 275 | 276 | 277 | 278 | 0 -0.046 0.004 0 -0 0 279 | 280 | 1.047 281 | 282 | 640 283 | 480 284 | RGB_INT8 285 | 286 | 287 | 0.1 288 | 100 289 | 290 | 291 | gaussian 292 | 0 293 | 0.007 294 | 295 | 296 | 1 297 | 60 298 | 1 299 | 300 | 301 | 0 -0.06 0.004 0 -0 0 302 | 303 | 1.047 304 | 305 | 640 306 | 480 307 | L_INT8 308 | 309 | 310 | 0.1 311 | 100 312 | 313 | 314 | gaussian 315 | 0 316 | 0.007 317 | 318 | 319 | 1 320 | 60 321 | 0 322 | 323 | 324 | 0 0.01 0.004 0 -0 0 325 | 326 | 1.047 327 | 328 | 640 329 | 480 330 | L_INT8 331 | 332 | 333 | 0.1 334 | 100 335 | 336 | 337 | gaussian 338 | 0 339 | 0.007 340 | 341 | 342 | 1 343 | 60 344 | 0 345 | 346 | 347 | 0 -0.03 0.004 0 -0 0 348 | 349 | 1.047 350 | 351 | 640 352 | 480 353 | 354 | 355 | 0.1 356 | 100 357 | 358 | 359 | gaussian 360 | 0 361 | 0.007 362 | 363 | 364 | 1 365 | 60 366 | 0 367 | 368 | 0 0.07 0.05 0 -0 1.5708 369 | 370 | 371 | realsense_camera::link 372 | gzsitl_quadcopter::quadrotor::link 373 | 374 | 375 | 376 | 1 377 | 378 | 379 | 0.65 0.65 0.23 380 | 381 | 382 | 0 0 0.115 0 -0 0.785 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 5 398 | 399 | contact_geometry 400 | __default_topic__ 401 | 402 | 403 | 0 404 | 405 | 406 | 1 407 | 0 408 | 0 409 | 1 410 | 0 411 | 1 412 | 413 | 1 414 | 415 | 0 416 | 0 417 | 1 418 | 419 | 420 | sensors 421 | gzsitl_quadcopter::quadrotor::link 422 | 423 | -0.009662 -0.52337 0 0 -0 0 424 | 425 | 426 | 427 | 0 0 0 0 -0 0 428 | 429 | 0.12017 430 | 431 | 0.00292634 432 | 0 433 | 0 434 | 0.00292634 435 | 0 436 | 0.00292634 437 | 438 | 0 0 0 0 -0 0 439 | 440 | 0 441 | 0 442 | 1 443 | 444 | 0 0 0 0 -0 0 445 | 446 | 447 | 0.2 448 | 449 | 450 | 451 | 1 452 | 456 | 0.3 0.3 0.3 1 457 | 0.7 0.7 0.7 1 458 | 0.01 0.01 0.01 1 459 | 0 0 0 1 460 | 461 | __default__ 462 | 463 | 464 | 1 465 | 0.5 466 | 467 | false 468 | true 469 | true 470 | 471 | 472 | 0 473 | 474 | 1 475 | 1 476 | -0.08543 5.25418 0 0 -0 0 477 | 478 | 479 | 75 945000000 480 | 28 877265016 481 | 1473878638 582888827 482 | 27521 483 | 484 | 0 0 0 0 -0 0 485 | 1 1 1 486 | 487 | 0 0 0 0 -0 0 488 | 0 0 0 0 -0 0 489 | 0 0 0 0 -0 0 490 | 0 0 0 0 -0 0 491 | 492 | 493 | 494 | -0.017755 10.9351 4.5515 0 -0 0 495 | 1 1 1 496 | 497 | -0.017755 10.9351 4.5515 0 -0 0 498 | 0 0 0 0 -0 0 499 | 0 0 0 0 -0 0 500 | 0 0 0 0 -0 0 501 | 502 | 503 | 504 | -0.00019 -0.00019 0.151292 -0 1e-06 -4e-05 505 | 1 1 1 506 | 507 | -0.00019 -0.00019 0.333758 1e-06 -0 1.57076 508 | 0 0 -0.0098 0 -0 0 509 | 0 0 -9.8 0 -0 0 510 | 0 0 -12.8968 0 -0 0 511 | 512 | 513 | -0.00019 -0.00019 0.151292 -0 1e-06 -4e-05 514 | 0 0 -0.0098 0 -0 0 515 | 0 0 -9.8 0 -0 0 516 | 0 0 -9.8 0 -0 0 517 | 518 | 519 | -0.000188 0.06981 0.201292 1e-06 -0 1.57076 520 | 0 0 -0.0098 0 -0 0 521 | 0 0 -9.8 0 -0 0 522 | 0 0 -0.603437 0 -0 0 523 | 524 | 525 | -0.00019 -0.00019 0.151292 -0 1e-06 -4e-05 526 | 0 0 -0.0098 0 -0 0 527 | 0 0 -9.8 0 -0 0 528 | 0 0 -9.8 0 -0 0 529 | 530 | 531 | 532 | 0 0 10 0 -0 0 533 | 534 | 535 | 536 | 537 | 17.0106 -0.18938 4.50854 0 0.193682 2.93119 538 | orbit 539 | perspective 540 | 541 | 542 | 543 | 544 | -------------------------------------------------------------------------------- /tools/coav-control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1 FATAL_ERROR) 2 | 3 | configure_file(coav-control.sh.in coav-control.sh @ONLY) 4 | configure_file(coav-control.service.in coav-control.service @ONLY) 5 | 6 | if(DEFINED ENV{SDKTARGETSYSROOT}) 7 | install(PROGRAMS ${CMAKE_CURRENT_BINARY_DIR}/coav-control.sh 8 | DESTINATION ${CMAKE_INSTALL_SYSCONFDIR}/init.d) 9 | endif() 10 | 11 | set(LIBRARIES coav) 12 | 13 | if (${WITH_VDEBUG}) 14 | set(LIBRARIES ${LIBRARIES} ${OPENGL_LIBRARIES} ${GLUT_LIBRARIES}) 15 | endif() 16 | 17 | add_executable(coav-control coav-control.cc parser.cc visual.cc visual_depth.cc visual_env.cc) 18 | target_link_libraries(coav-control ${LIBRARIES}) 19 | install(TARGETS coav-control 20 | DESTINATION ${CMAKE_INSTALL_BINDIR}) 21 | -------------------------------------------------------------------------------- /tools/coav-control/coav-control.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2017 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include "coav-control.hh" 24 | 25 | #ifdef WITH_VDEBUG 26 | #include "visual.hh" 27 | #endif 28 | 29 | using namespace std; 30 | 31 | int main (int argc, char* argv[]) 32 | { 33 | control_options opts = parse_cmdline(argc, argv); 34 | 35 | if (!opts.quiet) { 36 | cout << "Using Detect Algorithm: " << detect_to_name(opts.detect) << endl; 37 | cout << "Using Avoidance Algorithm: " << avoidance_to_name(opts.avoidance) << endl; 38 | cout << "Using Sensor: " << sensor_to_name(opts.sensor) << endl; 39 | } 40 | 41 | shared_ptr vehicle = opts.port ? 42 | std::make_shared(opts.port) : std::make_shared(); 43 | 44 | shared_ptr sensor; 45 | 46 | if (opts.sensor == ST_REALSENSE) { 47 | #ifdef HAVE_REALSENSE 48 | sensor = make_shared(640, 480, 30); 49 | #endif 50 | } else if (opts.sensor == ST_GAZEBO_REALSENSE) { 51 | #ifdef HAVE_GAZEBO 52 | sensor = make_shared(); 53 | #endif 54 | } 55 | 56 | if (opts.sensor == ST_UNDEFINED || sensor == nullptr) { 57 | cerr << "ERROR: Invalid Sensor" << endl; 58 | exit(-EINVAL); 59 | } 60 | 61 | shared_ptr detector; 62 | switch (opts.detect) { 63 | case DI_OBSTACLE: 64 | detector = make_shared(5.0); 65 | break; 66 | case DI_POLAR_HIST: 67 | detector = make_shared(5); 68 | break; 69 | default: 70 | cerr << "ERROR: Invalid Detector" << endl; 71 | exit(-EINVAL); 72 | } 73 | 74 | shared_ptr> avoidance; 75 | switch(opts.avoidance) { 76 | case QC_SHIFT_AVOIDANCE: 77 | avoidance = make_shared(vehicle); 78 | break; 79 | case QC_STOP: 80 | avoidance = make_shared(vehicle); 81 | break; 82 | case QC_VFF: 83 | avoidance = make_shared(vehicle); 84 | break; 85 | default: 86 | cerr << "ERROR: Invalid Avoidance" << endl; 87 | exit(-EINVAL); 88 | } 89 | 90 | if (opts.vdebug) { 91 | #ifdef WITH_VDEBUG 92 | visual_mainlopp(argc, argv, vehicle, sensor, detector, avoidance); 93 | #endif 94 | } else { 95 | while (true) { 96 | avoidance->avoid(detector->detect(sensor->read())); 97 | } 98 | } 99 | 100 | return 0; 101 | } 102 | -------------------------------------------------------------------------------- /tools/coav-control/coav-control.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2017 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | 21 | enum detect_algorithm { 22 | DA_UNDEFINED = 0, 23 | DI_OBSTACLE, 24 | DI_POLAR_HIST, 25 | }; 26 | 27 | enum avoidance_algorithm { 28 | AA_UNDEFINED = 0, 29 | QC_SHIFT_AVOIDANCE, 30 | QC_STOP, 31 | QC_VFF, 32 | }; 33 | 34 | enum sensor_type { 35 | ST_UNDEFINED, 36 | ST_REALSENSE, 37 | ST_GAZEBO_REALSENSE, 38 | }; 39 | 40 | struct control_options { 41 | enum detect_algorithm detect; 42 | enum avoidance_algorithm avoidance; 43 | enum sensor_type sensor; 44 | unsigned int port; 45 | bool quiet; 46 | bool vdebug; 47 | }; 48 | 49 | control_options parse_cmdline(int argc, char *argv[]); 50 | 51 | std::string detect_to_name(detect_algorithm d); 52 | detect_algorithm name_to_detect(std::string name); 53 | 54 | std::string avoidance_to_name(avoidance_algorithm a); 55 | avoidance_algorithm name_to_avoidance(std::string name); 56 | 57 | std::string sensor_to_name(sensor_type s); 58 | sensor_type name_to_sensor(std::string name); 59 | -------------------------------------------------------------------------------- /tools/coav-control/coav-control.service.in: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=Collision Avoidance Daemon 3 | 4 | [Service] 5 | Type=simple 6 | ExecStart=@CMAKE_INSTALL_FULL_BINDIR@/coav-control -d DI_OBSTACLE -a QC_STOP -s ST_REALSENSE 7 | Restart=on-failure 8 | 9 | [Install] 10 | WantedBy=multi-user.target 11 | -------------------------------------------------------------------------------- /tools/coav-control/coav-control.sh.in: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | ### BEGIN INIT INFO 4 | # Provides: Collision Avoidance Daemon 5 | # Required-Start: $network #FIXME 6 | # Required-Stop: $network #FIXME 7 | # Default-Start: 2 3 4 5 8 | # Default-Stop: 9 | # Description: Enable collision avoidance capabilities. 10 | ### END INIT INFO 11 | 12 | . /etc/init.d/functions 13 | 14 | get_pid() { 15 | pidof "coav-control" 16 | } 17 | 18 | start() { 19 | nohup @CMAKE_INSTALL_FULL_BINDIR@/coav-control \ 20 | -d DI_OBSTACLE \ 21 | -a QC_STOP \ 22 | -s ST_REALSENSE \ 23 | 2> /tmp/coav-control.log \ 24 | >/dev/null & 25 | 26 | get_pid > /dev/null 2>&1 27 | } 28 | 29 | stop() { 30 | pid=`get_pid` 31 | [ -n "$pid" ] && kill -KILL $pid > /dev/null 2>&1 && wait $pid 2> /dev/null || true 32 | } 33 | 34 | status() { 35 | get_pid > /dev/null 2>&1 36 | echo $? 37 | } 38 | 39 | case "$1" in 40 | start) 41 | start 42 | ;; 43 | stop) 44 | stop 45 | ;; 46 | restart) 47 | stop 48 | start 49 | ;; 50 | status) 51 | status 52 | ;; 53 | *) 54 | echo "Usage: $0 { start | stop | restart | status }" >&2 55 | exit 1 56 | ;; 57 | esac 58 | 59 | exit $? 60 | -------------------------------------------------------------------------------- /tools/coav-control/parser.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2017 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "coav-control.hh" 23 | 24 | using namespace std; 25 | 26 | struct { 27 | bool realsense; 28 | bool gazebo; 29 | } control_features { 30 | #ifdef HAVE_REALSENSE 31 | .realsense = true, 32 | #else 33 | .realsense = false, 34 | #endif 35 | 36 | #ifdef HAVE_GAZEBO 37 | .gazebo = true, 38 | #else 39 | .gazebo = false, 40 | #endif 41 | }; 42 | 43 | struct v_pair { 44 | string option; 45 | string val; 46 | }; 47 | 48 | void print_help(void) 49 | { 50 | cout << "Usage: coav-control [options]\n\n" 51 | "Options:\n" 52 | " -d, --detect\n" 53 | " Detection Algorithms. Can be one of the following:\n" 54 | " DI_OBSTACLE\n" 55 | " DI_POLAR_HIST\n" 56 | " -a, --avoidance\n" 57 | " Avoidance Algorithm. Can be one of the following:\n" 58 | " QC_SHIFT_AVOIDANCE\n" 59 | " QC_STOP\n" 60 | " QC_VFF\n" 61 | " -s, --sensor\n" 62 | " Vehicle Sensor. Can be one of the following:\n" 63 | " ST_REALSENSE\n" 64 | " ST_GAZEBO_REALSENSE\n" 65 | " -p, --port\n" 66 | " UDP port to use \n" 67 | " -q, --quiet\n" 68 | " Supress info messages \n" 69 | " -x, --visual\n" 70 | " Run visual debugger\n\n" 71 | " -h, --help\n" 72 | " Display this help and exit\n\n" 73 | "Example:\n" 74 | " $ coav-control -d DI_POLAR_HIST -a QC_SHIFT_AVOIDANCE -s ST_REALSENSE" 75 | << endl; 76 | } 77 | 78 | string detect_to_name(detect_algorithm d) 79 | { 80 | switch (d) { 81 | case DA_UNDEFINED: 82 | return string("DA_UNDEFINED"); 83 | case DI_OBSTACLE: 84 | return string("DI_OBSTACLE"); 85 | case DI_POLAR_HIST: 86 | return string("DI_POLAR_HIST"); 87 | } 88 | 89 | return string("UNKOWN_VALUE"); 90 | } 91 | 92 | detect_algorithm name_to_detect(string name) 93 | { 94 | transform(name.begin(), name.end(), name.begin(), ::toupper); 95 | 96 | if (name == "DI_OBSTACLE") { 97 | return DI_OBSTACLE; 98 | } else if (name == "DI_POLAR_HIST") { 99 | return DI_POLAR_HIST; 100 | } 101 | 102 | return DA_UNDEFINED; 103 | } 104 | 105 | string avoidance_to_name(avoidance_algorithm a) 106 | { 107 | switch (a) { 108 | case AA_UNDEFINED: 109 | return string("AA_UNDEFINED"); 110 | case QC_SHIFT_AVOIDANCE: 111 | return string("QC_SHIFT_AVOIDANCE"); 112 | case QC_STOP: 113 | return string("QC_STOP"); 114 | case QC_VFF: 115 | return string("QC_VFF"); 116 | } 117 | 118 | return string("UNKOWN_VALUE"); 119 | } 120 | 121 | avoidance_algorithm name_to_avoidance(string name) 122 | { 123 | transform(name.begin(), name.end(), name.begin(), ::toupper); 124 | 125 | if (name == "QC_SHIFT_AVOIDANCE") { 126 | return QC_SHIFT_AVOIDANCE; 127 | } else if (name == "QC_STOP") { 128 | return QC_STOP; 129 | } else if (name == "QC_VFF") { 130 | return QC_VFF; 131 | } 132 | 133 | return AA_UNDEFINED; 134 | } 135 | 136 | string sensor_to_name(sensor_type s) 137 | { 138 | switch (s) { 139 | case ST_UNDEFINED: 140 | return string("ST_UNDEFINED"); 141 | case ST_REALSENSE: 142 | return string("ST_REALSENSE"); 143 | case ST_GAZEBO_REALSENSE: 144 | return string("ST_GAZEBO_REALSENSE"); 145 | } 146 | 147 | return string("UNKOWN_VALUE"); 148 | } 149 | 150 | sensor_type name_to_sensor(string name) 151 | { 152 | transform(name.begin(), name.end(), name.begin(), ::toupper); 153 | 154 | if (name == "ST_REALSENSE") { 155 | return ST_REALSENSE; 156 | } else if (name == "ST_GAZEBO_REALSENSE") { 157 | return ST_GAZEBO_REALSENSE; 158 | } 159 | 160 | return ST_UNDEFINED; 161 | } 162 | 163 | 164 | vector get_arg_list(int argc, char* argv[]) 165 | { 166 | vector list; 167 | 168 | for (int i = 1; i < argc; i++) { 169 | if (argv[i][0] == '-' && argv[i][1] != '\0') { 170 | list.push_back((v_pair) {argv[i], ""}); 171 | } else if (argv[i][0] != '-' && list.size() != 0) { 172 | v_pair& cur = list.back(); 173 | string val = argv[i]; 174 | cur.val += cur.val.size() != 0 ? " " + val : val; 175 | } else { 176 | cerr << "Invalid argument : " << argv[i] << endl; 177 | exit(-EINVAL); 178 | } 179 | } 180 | 181 | return list; 182 | } 183 | 184 | control_options parse_cmdline(int argc, char *argv[]) 185 | { 186 | vector list = get_arg_list(argc, argv); 187 | 188 | control_options opts = { 189 | .detect = DA_UNDEFINED, 190 | .avoidance = AA_UNDEFINED, 191 | .sensor = ST_UNDEFINED, 192 | .port = 0, 193 | .quiet = false, 194 | .vdebug = false, 195 | }; 196 | 197 | for (v_pair p : list) { 198 | // Detect Algorithms 199 | if (p.option == "-d" || p.option == "--detect") { 200 | if (opts.detect != DA_UNDEFINED) { 201 | cerr << "ERROR: Multiple detect algorithms provided" << endl; 202 | exit(-EINVAL); 203 | } 204 | 205 | opts.detect = name_to_detect(p.val); 206 | 207 | if (opts.detect == DA_UNDEFINED) { 208 | cerr << "ERROR: Unkown detect algorithm value '" << p.val << "'" << endl; 209 | exit(-EINVAL); 210 | } 211 | 212 | // Avoid Algorithms 213 | } else if (p.option == "-a" || p.option == "--avoidance") { 214 | if (opts.avoidance != AA_UNDEFINED) { 215 | cerr << "ERROR: Multiple avoidance algorithms provided" << endl; 216 | exit(-EINVAL); 217 | } 218 | 219 | opts.avoidance = name_to_avoidance(p.val); 220 | 221 | if (opts.avoidance == AA_UNDEFINED) { 222 | cerr << "ERROR: Unkown avoidance algorithm value '" << p.val << "'" << endl; 223 | exit(-EINVAL); 224 | } 225 | 226 | // Sensor Type 227 | } else if (p.option == "-s" || p.option == "--sensor") { 228 | if (opts.sensor != ST_UNDEFINED) { 229 | cerr << "ERROR: Multiple sensors provided" << endl; 230 | exit(-EINVAL); 231 | } 232 | 233 | opts.sensor = name_to_sensor(p.val); 234 | 235 | if (opts.sensor == ST_UNDEFINED) { 236 | cerr << "ERROR: Unkown sensor value '" << p.val << "'" << endl; 237 | exit(-EINVAL); 238 | } else if ((opts.sensor == ST_REALSENSE && !control_features.realsense) || 239 | (opts.sensor == ST_GAZEBO_REALSENSE && !control_features.gazebo)) { 240 | cerr << "ERROR: Feature not available '" << opts.sensor << endl; 241 | exit(-EINVAL); 242 | } 243 | 244 | // Port 245 | } else if (p.option == "-p" || p.option == "--port") { 246 | opts.port = (unsigned int) stoul(p.val); 247 | 248 | // Quiet 249 | } else if (p.option == "-q" || p.option == "--quiet") { 250 | opts.quiet = true; 251 | 252 | // VDebug 253 | } else if (p.option == "-x" || p.option == "--visual") { 254 | #ifdef WITH_VDEBUG 255 | opts.vdebug = true; 256 | #else 257 | cerr << "ERROR: Feature not available 'visual debugger'" << endl; 258 | #endif 259 | 260 | //Help 261 | } else if (p.option == "-h" || p.option == "--help") { 262 | print_help(); 263 | exit(0); 264 | } else { 265 | cerr << "ERROR: Invalid Option '" << p.option << "'" << endl; 266 | print_help(); 267 | exit(-EINVAL); 268 | } 269 | } 270 | 271 | return opts; 272 | } 273 | 274 | -------------------------------------------------------------------------------- /tools/coav-control/visual.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifdef WITH_VDEBUG 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include "visual.hh" 28 | 29 | using namespace std; 30 | 31 | struct VisualData vdata; 32 | 33 | void reshape_handler(GLint newWidth, GLint newHeight) 34 | { 35 | vdata.window.width = newWidth; 36 | vdata.window.height = newHeight; 37 | 38 | vdata.env->set_viewport(0, 0, vdata.window.width, vdata.window.height); 39 | vdata.depth->set_viewport(0, 0, vdata.window.width * 0.3, vdata.window.height * 0.3); 40 | } 41 | 42 | void keyboard_handler( unsigned char key, int x, int y ) 43 | { 44 | switch ( key ) 45 | { 46 | case 27: // Escape key 47 | glutDestroyWindow(vdata.window.ref); 48 | exit (0); 49 | break; 50 | } 51 | } 52 | 53 | void mouse_move_handler(int x, int y) 54 | { 55 | vdata.env->on_mouse_move(x, y); 56 | } 57 | 58 | void mouse_button_handler(int button, int state, int x, int y) 59 | { 60 | vdata.env->on_mouse_button(button, state, x, y); 61 | } 62 | 63 | void coav_loop() 64 | { 65 | vdata.coav.depth_data = vdata.coav.sensor->read(); 66 | vdata.coav.obstacles = vdata.coav.detector->detect(vdata.coav.depth_data); 67 | vdata.coav.avoidance->avoid(vdata.coav.obstacles); 68 | 69 | glutPostRedisplay(); 70 | } 71 | 72 | void update_display() 73 | { 74 | glClearColor(0.0, 0.0, 0.0, 1.0); 75 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 76 | 77 | vdata.env->visualize(vdata.coav.vehicle, vdata.coav.obstacles); 78 | vdata.depth->visualize(vdata.coav.depth_data, vdata.coav.obstacles); 79 | 80 | glutSwapBuffers(); 81 | } 82 | 83 | void visual_mainlopp(int argc, char* argv[], shared_ptr vehicle, 84 | shared_ptr sensor, shared_ptr detector, 85 | shared_ptr> avoidance) 86 | { 87 | vdata.coav.vehicle = vehicle; 88 | vdata.coav.sensor = sensor; 89 | vdata.coav.detector = detector; 90 | vdata.coav.avoidance = avoidance; 91 | 92 | vdata.window.width = 1280; 93 | vdata.window.height = 960; 94 | 95 | vdata.env = make_shared(0, 0, vdata.window.width, vdata.window.height); 96 | vdata.depth = make_shared(0, 0, vdata.window.width * 0.3, vdata.window.height * 0.3); 97 | 98 | glutInit(&argc, argv); 99 | glutInitDisplayMode(GLUT_SINGLE); 100 | glutInitWindowSize(vdata.window.width, vdata.window.height); 101 | glutInitWindowPosition(100, 100); 102 | 103 | vdata.window.ref = glutCreateWindow("Coav Visual Debugger"); 104 | 105 | glutReshapeFunc(reshape_handler); 106 | glutKeyboardFunc(keyboard_handler); 107 | glutMouseFunc(mouse_button_handler); 108 | glutMotionFunc(mouse_move_handler); 109 | 110 | glutIdleFunc(coav_loop); 111 | 112 | glutDisplayFunc(update_display); 113 | 114 | glutMainLoop(); 115 | } 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /tools/coav-control/visual.hh: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2017 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | 25 | using namespace std; 26 | 27 | class VisualDepth 28 | { 29 | public: 30 | VisualDepth(int x, int y, unsigned int width, unsigned int height); 31 | ~VisualDepth(); 32 | void info(); 33 | void rainbow_scale(double value, uint8_t rgb[]); 34 | void visualize(shared_ptr depth_data, vector obstacles = vector()); 35 | void set_viewport(int x, int y, unsigned int width, unsigned int height); 36 | 37 | private: 38 | int x; 39 | int y; 40 | 41 | unsigned int width; 42 | unsigned int height; 43 | 44 | GLuint texture; 45 | uint8_t *frame_buffer; 46 | size_t frame_buffer_size; 47 | }; 48 | 49 | class VisualEnvironment 50 | { 51 | public: 52 | VisualEnvironment(int x, int y, unsigned int width, unsigned int height); 53 | void visualize(shared_ptr vehicle, vector obstacles); 54 | void set_viewport(int x, int y, unsigned int width, unsigned int height); 55 | 56 | void on_mouse_move(int mouseX, int mouseY); 57 | void on_mouse_button(int button, int state, int mouseX, int mouseY); 58 | 59 | private: 60 | int x; 61 | int y; 62 | 63 | unsigned int width; 64 | unsigned int height; 65 | 66 | double x_eye = 40; 67 | double y_eye = -30; 68 | double z_eye = 15; 69 | double zoom = 20; 70 | 71 | int x_enter = 0; 72 | int y_enter = 0; 73 | }; 74 | 75 | struct VisualData 76 | { 77 | struct { 78 | unsigned int width; 79 | unsigned int height; 80 | 81 | int ref; 82 | } window; 83 | 84 | struct { 85 | shared_ptr vehicle; 86 | shared_ptr sensor; 87 | shared_ptr detector; 88 | shared_ptr> avoidance; 89 | 90 | shared_ptr depth_data; 91 | vector obstacles; 92 | } coav; 93 | 94 | shared_ptr depth; 95 | shared_ptr env; 96 | }; 97 | 98 | void visual_mainlopp(int argc, char* argv[], shared_ptr vehicle, 99 | shared_ptr sensor, shared_ptr detector, 100 | shared_ptr> avoidance); 101 | -------------------------------------------------------------------------------- /tools/coav-control/visual_depth.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2017 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifdef WITH_VDEBUG 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include "visual.hh" 26 | 27 | VisualDepth::VisualDepth(int x, int y, unsigned int width, unsigned int height) 28 | { 29 | this->set_viewport(x, y, width, height); 30 | 31 | this->frame_buffer = nullptr; 32 | this->frame_buffer_size = 0; 33 | 34 | glGenTextures(1, &this->texture); 35 | } 36 | 37 | void VisualDepth::set_viewport(int x, int y, unsigned int width, unsigned int height) 38 | { 39 | this->x = x; 40 | this->y = y; 41 | this->width = width; 42 | this->height = height; 43 | } 44 | 45 | VisualDepth::~VisualDepth() 46 | { 47 | delete this->frame_buffer; 48 | } 49 | 50 | void VisualDepth::info() 51 | { 52 | cout << "Scale:" << endl; 53 | cout << "null : BLACK" << endl; 54 | cout << "0.01 m: RED" << endl; 55 | cout << "1.25 m: YELLOW" << endl; 56 | cout << "2.50 m: GREEN" << endl; 57 | cout << "3.75 m: CYAN" << endl; 58 | cout << "5.0+ m: BLUE" << endl; 59 | } 60 | 61 | void VisualDepth::rainbow_scale(double value, uint8_t rgb[]) 62 | { 63 | rgb[0] = rgb[1] = rgb[2] = 0; 64 | 65 | if (value <= 0.0) 66 | return; 67 | 68 | if (value < 0.25) { // RED to YELLOW 69 | rgb[0] = 255; 70 | rgb[1] = (uint8_t)255 * (value / 0.25); 71 | } else if (value < 0.5) { // YELLOW to GREEN 72 | rgb[0] = (uint8_t)255 * (1 - ((value - 0.25) / 0.25)); 73 | rgb[1] = 255; 74 | } else if (value < 0.75) { // GREEN to CYAN 75 | rgb[1] = 255; 76 | rgb[2] = (uint8_t)255 * (value - 0.5 / 0.25); 77 | } else if (value < 1.0) { // CYAN to BLUE 78 | rgb[1] = (uint8_t)255 * (1 - ((value - 0.75) / 0.25)); 79 | rgb[2] = 255; 80 | } else { // BLUE 81 | rgb[2] = 255; 82 | } 83 | } 84 | 85 | void VisualDepth::visualize(shared_ptr depth_data, vector obstacles) 86 | { 87 | if (depth_data == nullptr) { 88 | return; 89 | } 90 | 91 | if (this->frame_buffer_size < depth_data->width * depth_data->height * 3) { 92 | delete this->frame_buffer; 93 | this->frame_buffer = new uint8_t[depth_data->width * depth_data->height * 3]; 94 | } 95 | 96 | glViewport(this->x, this->y, this->width, this->height); 97 | 98 | glMatrixMode(GL_PROJECTION); 99 | glLoadIdentity(); 100 | gluOrtho2D(0, 1, 0, 1); 101 | 102 | glMatrixMode(GL_MODELVIEW); 103 | glLoadIdentity(); 104 | gluLookAt(0.0f, 0.0f, 1.0f, 105 | 0.0f, 0.0f, 0.0f, 106 | 0.0f, 1.0f, 0.0f); 107 | 108 | glColor3f(1.0f, 1.0f, 1.0f); 109 | 110 | glBindTexture(GL_TEXTURE_2D, this->texture); 111 | glPixelStorei(GL_UNPACK_ROW_LENGTH, depth_data->width); 112 | 113 | for (unsigned int i = 0; i < depth_data->depth_buffer.size(); i++) { 114 | uint8_t *rgb = this->frame_buffer + (3 * i); 115 | this->rainbow_scale(((double)depth_data->depth_buffer[i] * depth_data->scale) / 5.0, rgb); 116 | } 117 | 118 | if (obstacles.size() != 0) { 119 | double base_phi = (M_PI - depth_data->hfov) / 2; 120 | double base_theta = (M_PI - depth_data->vfov) / 2; 121 | 122 | for (Obstacle o : obstacles) { 123 | unsigned int x = (1 - ((o.center.z - base_phi) / depth_data->hfov)) 124 | * depth_data->width; 125 | unsigned int y = (o.center.y - base_theta) * depth_data->height 126 | / depth_data->vfov; 127 | 128 | x = x < 5 ? 5 : (x > depth_data->width - 5 ? depth_data->width - 5 : x); 129 | y = y < 5 ? 5 : (y > depth_data->height - 5 ? depth_data->height - 5 : y); 130 | 131 | uint8_t *p = this->frame_buffer; 132 | for (unsigned int j = y - 5; j < y + 5; j++) { 133 | for (unsigned int i = x - 5; i < x + 5; i++) { 134 | p[((j * depth_data->width) + i) * 3] = 255; 135 | p[(((j * depth_data->width) + i) * 3) + 1] = 0; 136 | p[(((j * depth_data->width) + i) * 3) + 2] = 255; 137 | } 138 | } 139 | } 140 | } 141 | 142 | glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, depth_data->width, depth_data->height, 0, GL_RGB, 143 | GL_UNSIGNED_BYTE, reinterpret_cast(this->frame_buffer)); 144 | 145 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); 146 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); 147 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP); 148 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP); 149 | glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); 150 | glBindTexture(GL_TEXTURE_2D, 0); 151 | 152 | glBindTexture(GL_TEXTURE_2D, this->texture); 153 | glEnable(GL_TEXTURE_2D); 154 | 155 | glBegin(GL_QUADS); 156 | // Order matters :/ 157 | glTexCoord2f(0, 0); 158 | glVertex2f(0, 1); 159 | 160 | glTexCoord2f(1, 0); 161 | glVertex2f(1, 1); 162 | 163 | glTexCoord2f(1, 1); 164 | glVertex2f(1, 0); 165 | 166 | glTexCoord2f(0, 1); 167 | glVertex2f(0, 0); 168 | glEnd(); 169 | 170 | glDisable(GL_TEXTURE_2D); 171 | glBindTexture(GL_TEXTURE_2D, 0); 172 | } 173 | 174 | #endif 175 | -------------------------------------------------------------------------------- /tools/coav-control/visual_env.cc: -------------------------------------------------------------------------------- 1 | /* 2 | // Copyright (c) 2016 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | */ 16 | 17 | #ifdef WITH_VDEBUG 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #include "visual.hh" 29 | 30 | void draw_blade(float x, float y, float z) 31 | { 32 | glPushMatrix(); 33 | 34 | glColor3f(0.4f, 0.4f, 0.4f); 35 | 36 | GLUquadricObj *cylinder; 37 | cylinder = gluNewQuadric(); 38 | 39 | GLUquadricObj *circle; 40 | circle = gluNewQuadric(); 41 | 42 | glTranslatef(x, y, z); 43 | gluCylinder(cylinder, 0.3f, 0.3f, 0.05f, 32, 32); 44 | gluDisk(circle, 0.0, 0.3f, 32, 1); 45 | glTranslatef(0.0f, 0.0f, 0.05f); 46 | gluDisk(circle, 0.0, 0.3f, 32, 1); 47 | 48 | glPopMatrix(); 49 | } 50 | 51 | void draw_vehicle() 52 | { 53 | glPushMatrix(); 54 | 55 | glColor3f(0.1f, 0.1f, 0.1f); 56 | 57 | // top 58 | glBegin(GL_QUADS); 59 | glNormal3f(0.0f, 0.0f, 1.0f); 60 | glVertex3f(-0.2f, 0.3f, 0.2f); 61 | glVertex3f(0.2f, 0.3f, 0.2f); 62 | glVertex3f(0.2f, -0.3f, 0.2f); 63 | glVertex3f(-0.2f, -0.3f, 0.2f); 64 | 65 | glEnd(); 66 | 67 | // front 68 | glBegin(GL_QUADS); 69 | glNormal3f(0.0f, 1.0f, 0.0f); 70 | glVertex3f(0.2f, 0.3f, 0.2f); 71 | glVertex3f(0.2f, 0.3f, 0.0f); 72 | glVertex3f(-0.2f, 0.3f, 0.0f); 73 | glVertex3f(-0.2f, 0.3f, 0.2f); 74 | 75 | glEnd(); 76 | 77 | // right 78 | glBegin(GL_QUADS); 79 | glNormal3f(1.0f, 0.0f, 0.0f); 80 | glVertex3f(0.2f, 0.3f, 0.0f); 81 | glVertex3f(0.2f, 0.3f, 0.2f); 82 | glVertex3f(0.2f, -0.3f, 0.2f); 83 | glVertex3f(0.2f, -0.3f, 0.0f); 84 | 85 | glEnd(); 86 | 87 | // left 88 | glBegin(GL_QUADS); 89 | glNormal3f(-1.0f, 0.0f, 0.0f); 90 | glVertex3f(-0.2f, -0.3f, 0.2f); 91 | glVertex3f(-0.2f, 0.3f, 0.2f); 92 | glVertex3f(-0.2f, 0.3f, 0.0f); 93 | glVertex3f(-0.2f, -0.3f, 0.0f); 94 | 95 | glEnd(); 96 | 97 | // bottom 98 | glBegin(GL_QUADS); 99 | glNormal3f(0.0f, 0.0f, -1.0f); 100 | glVertex3f(0.2f, 0.3f, 0.0f); 101 | glVertex3f(0.2f, -0.3f, 0.0f); 102 | glVertex3f(-0.2f, -0.3f, 0.0f); 103 | glVertex3f(-0.2f, 0.3f, 0.0f); 104 | 105 | glEnd(); 106 | 107 | // back 108 | glBegin(GL_QUADS); 109 | glNormal3f(0.0f, -1.0f, 0.0f); 110 | glVertex3f(0.2f, -0.3f, 0.2f); 111 | glVertex3f(0.2f, -0.3f, 0.0f); 112 | glVertex3f(-0.2f, -0.3f, 0.0f); 113 | glVertex3f(-0.2f, -0.3f, 0.2f); 114 | 115 | glEnd(); 116 | 117 | draw_blade(0.4, 0.4, 0.2); 118 | draw_blade(0.4, -0.4, 0.2); 119 | draw_blade(-0.4, -0.4, 0.2); 120 | draw_blade(-0.4, 0.4, 0.2); 121 | 122 | } 123 | 124 | void draw_obstacle(float x, float y, float z) 125 | { 126 | glPushMatrix(); 127 | 128 | glTranslatef(x, y, z); 129 | glColor3f(0.7f, 0.0f, 0.0f); 130 | glutSolidSphere(0.3, 20.0, 20.0); 131 | 132 | glPopMatrix(); 133 | } 134 | 135 | void draw_grid() 136 | { 137 | glPushMatrix(); 138 | 139 | glBegin(GL_LINES); 140 | glColor3f(0.6f, 0.6f, 0.6f); 141 | for (int i = -10; i <= 10; i++) { 142 | if (i == 0) continue; 143 | 144 | glNormal3f(0.0f, 0.0f, 1.0f); 145 | glVertex3f(100, i * 10, 0); 146 | glVertex3f(-100, i * 10, 0); 147 | 148 | glNormal3f(0.0f, 0.0f, 1.0f); 149 | glVertex3f(i * 10, 100, 0); 150 | glVertex3f(i * 10, -100, 0); 151 | } 152 | 153 | // X axis 154 | glColor3f(0.8, 0.0, 0.0); 155 | glVertex3f(-100, 0, 0); 156 | glVertex3f( 100, 0, 0); 157 | // Y axis 158 | glColor3f(0.0, 0.8, 0.0); 159 | glVertex3f(0, -100, 0); 160 | glVertex3f(0, 100, 0); 161 | // Z axis 162 | glColor3f(0.0, 0.0, 0.8); 163 | glVertex3f(0, 0, 0); 164 | glVertex3f(0, 0, 100); 165 | glEnd(); 166 | 167 | glTranslatef(0.0, 0.0, -0.01); 168 | 169 | glBegin(GL_QUADS); 170 | glNormal3f(0.0f, 0.0f, 1.0f); 171 | glColor3f(0.5f, 0.5f, 0.5f); 172 | glVertex3f(100.0f, 100.0f, 0.0f); 173 | glVertex3f(100.0f, -100.0f, 0.0f); 174 | glVertex3f(-100.0f, -100.0f, 0.0f); 175 | glVertex3f(-100.0f, 100.0f, 0.0f); 176 | glEnd(); 177 | 178 | glPopMatrix(); 179 | } 180 | 181 | VisualEnvironment::VisualEnvironment(int x, int y, unsigned int width, unsigned int height) 182 | { 183 | this->set_viewport(x, y, width, height); 184 | } 185 | 186 | void VisualEnvironment::set_viewport(int x, int y, unsigned int width, unsigned int height) 187 | { 188 | this->x = x; 189 | this->y = y; 190 | this->width = width; 191 | this->height = height; 192 | } 193 | 194 | void VisualEnvironment::visualize(shared_ptr vehicle, vector obstacles) 195 | { 196 | glEnable(GL_DEPTH_TEST); 197 | glDepthFunc(GL_LESS); 198 | 199 | glEnable(GL_LIGHTING); 200 | glEnable(GL_LIGHT0); 201 | 202 | GLfloat ambient[] = { 0.0f, 0.0f, 0.0f, 1.0f }; 203 | GLfloat diffuse[] = { 1.0f, 1.0f, 1.0f, 1.0f }; 204 | GLfloat specular[] = { 1.0f, 1.0f, 1.0f, 1.0f }; 205 | GLfloat position[] = { 1.0f, 1.0f, 0.0f, 0.0f }; 206 | 207 | glLightfv(GL_LIGHT0, GL_AMBIENT, ambient); 208 | glLightfv(GL_LIGHT0, GL_SPECULAR, specular); 209 | glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse); 210 | glLightfv(GL_LIGHT0, GL_POSITION, position); 211 | 212 | glEnable(GL_COLOR_MATERIAL); 213 | glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE); 214 | 215 | glViewport(this->x, this->y, this->width, this->height); 216 | 217 | glMatrixMode(GL_PROJECTION); 218 | glLoadIdentity(); 219 | gluPerspective(45, float(this->width) / float(this->height), 1, 1000); 220 | 221 | glMatrixMode(GL_MODELVIEW); 222 | glLoadIdentity(); 223 | 224 | gluLookAt(this->x_eye, this->y_eye, this->z_eye, 0, 0, 0, 0, 0, 1); 225 | 226 | draw_grid(); 227 | 228 | glm::dvec3 pos = vehicle->vehicle_pose().pos; 229 | glm::dquat rot = vehicle->vehicle_pose().rot; 230 | 231 | glPushMatrix(); 232 | 233 | glTranslatef(pos.x, pos.y, pos.z); 234 | auto angleAxis = glm::axis(rot); 235 | glRotatef(glm::angle(rot), angleAxis.x, angleAxis.y, angleAxis.z); 236 | 237 | draw_vehicle(); 238 | 239 | for (Obstacle o : obstacles) { 240 | float r = o.center.x; 241 | float theta = o.center.y; 242 | float phi = o.center.z; 243 | 244 | float ox = r * sin(theta) * cos(phi); 245 | float oy = r * sin(theta) * sin(phi); 246 | float oz = r * cos(theta); 247 | 248 | draw_obstacle(ox, oy, oz); 249 | } 250 | 251 | glPopMatrix(); 252 | 253 | glDisable(GL_LIGHT0); 254 | glDisable(GL_LIGHTING); 255 | glDisable(GL_COLOR_MATERIAL); 256 | glDisable(GL_DEPTH_TEST); 257 | } 258 | 259 | void VisualEnvironment::on_mouse_move(int x, int y) 260 | { 261 | double angle = 3.14 * float(this->x_enter + x) / this->width; 262 | this->x_eye = this->zoom * sin(angle); 263 | this->y_eye = this->zoom * cos(angle); 264 | this->z_eye = (this->y_enter + y) / 10 ; 265 | 266 | glutPostRedisplay(); 267 | } 268 | 269 | void VisualEnvironment::on_mouse_button(int button, int state, int x, int y) 270 | { 271 | if (button == GLUT_LEFT_BUTTON) { 272 | if (state == GLUT_DOWN) { 273 | this->x_enter = this->x_enter - x; 274 | this->y_enter = this->y_enter - y; 275 | } else if (state == GLUT_UP) { 276 | this->x_enter = this->x_enter + x; 277 | this->y_enter = this->y_enter + y; 278 | } 279 | } else if (button == 3) { 280 | if (state == GLUT_UP) return; 281 | this->zoom -= 5; 282 | this->x_eye = this->zoom / (this->zoom + 5) * this->x_eye; 283 | this->y_eye = this->zoom / (this->zoom + 5) * this->y_eye; 284 | } else if (button == 4) { 285 | if (state == GLUT_UP) return; 286 | this->zoom += 5; 287 | this->x_eye = this->zoom / (this->zoom - 5) * this->x_eye; 288 | this->y_eye = this->zoom / (this->zoom - 5) * this->y_eye; 289 | } 290 | 291 | glutPostRedisplay(); 292 | } 293 | 294 | #endif 295 | --------------------------------------------------------------------------------