├── .gitignore ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── cmake └── cmake_uninstall.cmake.in ├── include ├── ArduCopterIRLockPlugin.hh ├── ArduPilotPlugin.hh ├── GimbalSmall2dPlugin.hh └── SelectionBuffer.hh ├── models ├── iris_with_ardupilot │ ├── model.config │ └── model.sdf └── iris_with_standoffs │ ├── meshes │ ├── iris.dae │ ├── iris_prop_ccw.dae │ └── iris_prop_cw.dae │ ├── model.config │ └── model.sdf ├── src ├── ArduCopterIRLockPlugin.cc ├── ArduPilotPlugin.cc └── GimbalSmall2dPlugin.cc └── worlds ├── iris_arducopter_cmac.world └── iris_arducopter_runway.world /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | Build/ 3 | bin/ 4 | lib/ 5 | msg_gen/ 6 | srv_gen/ 7 | msg/*Action.msg 8 | msg/*ActionFeedback.msg 9 | msg/*ActionGoal.msg 10 | msg/*ActionResult.msg 11 | msg/*Feedback.msg 12 | msg/*Goal.msg 13 | msg/*Result.msg 14 | msg/_*.py 15 | 16 | # Generated by dynamic reconfigure 17 | *.cfgc 18 | /cfg/cpp/ 19 | /cfg/*.py 20 | 21 | # Ignore generated docs 22 | *.dox 23 | *.wikidoc 24 | 25 | # eclipse stuff 26 | .project 27 | .cproject 28 | 29 | # qcreator stuff 30 | CMakeLists.txt.user 31 | 32 | srv/_*.py 33 | *.pcd 34 | *.pyc 35 | qtcreator-* 36 | *.user 37 | 38 | /planning/cfg 39 | /planning/docs 40 | /planning/src 41 | 42 | *~ 43 | 44 | # Emacs 45 | .#* 46 | 47 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm 48 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 49 | 50 | # User-specific stuff: 51 | .idea/**/workspace.xml 52 | .idea/**/tasks.xml 53 | .idea/dictionaries 54 | 55 | # Sensitive or high-churn files: 56 | .idea/**/dataSources/ 57 | .idea/**/dataSources.ids 58 | .idea/**/dataSources.xml 59 | .idea/**/dataSources.local.xml 60 | .idea/**/sqlDataSources.xml 61 | .idea/**/dynamic.xml 62 | .idea/**/uiDesigner.xml 63 | 64 | # Gradle: 65 | .idea/**/gradle.xml 66 | .idea/**/libraries 67 | 68 | # Mongo Explorer plugin: 69 | .idea/**/mongoSettings.xml 70 | 71 | ## File-based project format: 72 | *.iws 73 | 74 | ## Plugin-specific files: 75 | 76 | # IntelliJ 77 | /out/ 78 | 79 | # mpeltonen/sbt-idea plugin 80 | .idea_modules/ 81 | 82 | # JIRA plugin 83 | atlassian-ide-plugin.xml 84 | 85 | # Cursive Clojure plugin 86 | .idea/replstate.xml 87 | 88 | # Crashlytics plugin (for Android Studio and IntelliJ) 89 | com_crashlytics_export_strings.xml 90 | crashlytics.properties 91 | crashlytics-build.properties 92 | fabric.properties 93 | 94 | cmake-build-debug 95 | 96 | # Catkin custom files 97 | CATKIN_IGNORE 98 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | 3 | 4 | project(ardupilot_sitl_gazebo) 5 | 6 | 7 | ####################### 8 | ## Find Dependencies ## 9 | ####################### 10 | 11 | find_package(gazebo REQUIRED) 12 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 13 | 14 | if("${GAZEBO_VERSION}" VERSION_LESS "8.0") 15 | message(FATAL_ERROR "You need at least Gazebo 8.0. Your version: ${GAZEBO_VERSION}") 16 | else() 17 | message("Gazebo version: ${GAZEBO_VERSION}") 18 | endif() 19 | 20 | ###### COMPILE RULES ###### 21 | include(CheckCXXCompilerFlag) 22 | 23 | macro(filter_valid_compiler_flags) 24 | foreach(flag ${ARGN}) 25 | CHECK_CXX_COMPILER_FLAG(${flag} R${flag}) 26 | if(${R${flag}}) 27 | set(VALID_CXX_FLAGS "${VALID_CXX_FLAGS} ${flag}") 28 | endif() 29 | endforeach() 30 | endmacro() 31 | 32 | set(CMAKE_CXX_EXTENSIONS off) # see gazebo CMakeList 33 | 34 | if (NOT CMAKE_BUILD_TYPE) 35 | set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING 36 | "Choose the type of build, options are: Debug Release RelWithDebInfo Profile Check" FORCE) 37 | endif (NOT CMAKE_BUILD_TYPE) 38 | 39 | # Build type link flags 40 | set (CMAKE_LINK_FLAGS_RELEASE " " CACHE INTERNAL "Link flags for release" FORCE) 41 | set (CMAKE_LINK_FLAGS_RELWITHDEBINFO " " CACHE INTERNAL "Link flags for release with debug support" FORCE) 42 | set (CMAKE_LINK_FLAGS_DEBUG " " CACHE INTERNAL "Link flags for debug" FORCE) 43 | set (CMAKE_LINK_FLAGS_PROFILE " -pg" CACHE INTERNAL "Link flags for profile" FORCE) 44 | set (CMAKE_LINK_FLAGS_COVERAGE " --coverage" CACHE INTERNAL "Link flags for static code coverage" FORCE) 45 | 46 | set (CMAKE_C_FLAGS_RELEASE "") 47 | if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang" AND NOT MSVC) 48 | # -s doesn't work with clang or Visual Studio, see alternative in link below: 49 | # http://stackoverflow.com/questions/6085491/gcc-vs-clang-symbol-strippingu 50 | set (CMAKE_C_FLAGS_RELEASE "-s") 51 | endif() 52 | 53 | if (NOT MSVC) 54 | set (CMAKE_C_FLAGS_RELEASE " ${CMAKE_C_FLAGS_RELEASE} -O3 -DNDEBUG ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release" FORCE) 55 | set (CMAKE_CXX_FLAGS_RELEASE ${CMAKE_C_FLAGS_RELEASE}) 56 | 57 | set (CMAKE_C_FLAGS_RELWITHDEBINFO " -g -O2 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release with debug support" FORCE) 58 | set (CMAKE_CXX_FLAGS_RELWITHDEBINFO ${CMAKE_C_FLAGS_RELWITHDEBINFO}) 59 | 60 | set (CMAKE_C_FLAGS_DEBUG " -ggdb3 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for debug" FORCE) 61 | set (CMAKE_CXX_FLAGS_DEBUG ${CMAKE_C_FLAGS_DEBUG}) 62 | 63 | set (CMAKE_C_FLAGS_PROFILE " -fno-omit-frame-pointer -g -pg ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for profile" FORCE) 64 | set (CMAKE_CXX_FLAGS_PROFILE ${CMAKE_C_FLAGS_PROFILE}) 65 | 66 | set (CMAKE_C_FLAGS_COVERAGE " -g -O0 -Wformat=2 --coverage -fno-inline ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for static code coverage" FORCE) 67 | set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_C_FLAGS_COVERAGE}") 68 | if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 69 | # -fno-default-inline -fno-implicit-inline-templates are unimplemented, cause errors in clang 70 | # -fno-elide-constructors can cause seg-faults in clang 3.4 and earlier 71 | # http://llvm.org/bugs/show_bug.cgi?id=12208 72 | set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_COVERAGE} -fno-default-inline -fno-implicit-inline-templates -fno-elide-constructors") 73 | endif() 74 | endif() 75 | 76 | ##################################### 77 | # Set all the global build flags 78 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") 79 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") 80 | set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") 81 | set (CMAKE_MODULE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") 82 | 83 | # Compiler-specific C++11 activation. 84 | if ("${CMAKE_CXX_COMPILER_ID}" MATCHES "GNU") 85 | execute_process( 86 | COMMAND ${CMAKE_CXX_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) 87 | if (NOT (GCC_VERSION VERSION_GREATER 4.7)) 88 | message(FATAL_ERROR "${PROJECT_NAME} requires g++ 4.8 or greater.") 89 | endif () 90 | elseif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") 91 | execute_process( 92 | COMMAND ${CMAKE_CXX_COMPILER} -dumpversion OUTPUT_VARIABLE CLANG_VERSION) 93 | if (NOT (CLANG_VERSION VERSION_GREATER 3.2)) 94 | message(FATAL_ERROR "${PROJECT_NAME} requires clang 3.3 or greater.") 95 | endif () 96 | 97 | if ("${CMAKE_SYSTEM_NAME}" MATCHES "Darwin") 98 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++") 99 | endif () 100 | elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") 101 | if (MSVC_VERSION LESS 1800) 102 | message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.") 103 | endif () 104 | else () 105 | message(FATAL_ERROR "Your C++ compiler does not support C++11.") 106 | endif () 107 | 108 | set(WARN_LEVEL "-Wall") 109 | filter_valid_compiler_flags(${WARN_LEVEL} 110 | -Wextra -Wno-long-long -Wno-unused-value -Wfloat-equal -Wshadow 111 | -Wswitch-default -Wmissing-include-dirs -pedantic) 112 | if (UNIX AND NOT APPLE) 113 | filter_valid_compiler_flags(-fvisibility=hidden -fvisibility-inlines-hidden) 114 | endif() 115 | 116 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${VALID_CXX_FLAGS}") 117 | 118 | ########### 119 | ## Build ## 120 | ########### 121 | 122 | include_directories( 123 | ${PROJECT_SOURCE_DIR} 124 | include 125 | ${GAZEBO_INCLUDE_DIRS} 126 | ) 127 | 128 | link_libraries( 129 | ${GAZEBO_LIBRARIES} 130 | ) 131 | 132 | link_directories( 133 | ${GAZEBO_LIBRARY_DIRS} 134 | ) 135 | 136 | set (plugins_single_header 137 | ArduPilotPlugin 138 | ArduCopterIRLockPlugin 139 | GimbalSmall2dPlugin 140 | ) 141 | 142 | add_library(ArduCopterIRLockPlugin SHARED src/ArduCopterIRLockPlugin.cc) 143 | target_link_libraries(ArduCopterIRLockPlugin ${GAZEBO_LIBRARIES}) 144 | 145 | add_library(ArduPilotPlugin SHARED src/ArduPilotPlugin.cc) 146 | target_link_libraries(ArduPilotPlugin ${GAZEBO_LIBRARIES}) 147 | 148 | if("${GAZEBO_VERSION}" VERSION_LESS "8.0") 149 | add_library(GimbalSmall2dPlugin SHARED src/GimbalSmall2dPlugin.cc) 150 | target_link_libraries(GimbalSmall2dPlugin ${GAZEBO_LIBRARIES}) 151 | install(TARGETS GimbalSmall2dPlugin DESTINATION ${GAZEBO_PLUGIN_PATH}) 152 | endif() 153 | 154 | install(TARGETS ArduCopterIRLockPlugin DESTINATION ${GAZEBO_PLUGIN_PATH}) 155 | install(TARGETS ArduPilotPlugin DESTINATION ${GAZEBO_PLUGIN_PATH}) 156 | 157 | install(DIRECTORY models DESTINATION ${GAZEBO_MODEL_PATH}/..) 158 | install(DIRECTORY worlds DESTINATION ${GAZEBO_MODEL_PATH}/..) 159 | 160 | # uninstall target 161 | if(NOT TARGET uninstall) 162 | configure_file( 163 | "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in" 164 | "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" 165 | IMMEDIATE @ONLY) 166 | 167 | add_custom_target(uninstall 168 | COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) 169 | endif() 170 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Deprecated : please use https://github.com/ArduPilot/ardupilot_gazebo 2 | 3 | # Ardupilot Gazebo plugin 4 | 5 | ## Requirements : 6 | Native Ubuntu able to run full 3D graphics. 7 | Gazebo version 8.x or greater 8 | The dev branch will works on gazebo >= 8.x 9 | For Gazebo 7 use branch gazebo7 10 | 11 | ## Disclamer : 12 | This is a playground until I get some time to push the correct patch to gazebo master (I got hard time to work with mercurial..)! 13 | So you can expect things to not be up-to-date. 14 | This assume that your are using Ubuntu 16.04 or Ubuntu 18.04 15 | 16 | ## Usage : 17 | I assume you already have Gazebo installed with ROS (or without). 18 | If you don't have it yet, install ROS with `sudo apt install ros-melodic-desktop-full` 19 | (follow instruction here http://wiki.ros.org/melodic/Installation/Ubuntu). 20 | Due to a bug in current gazebo release from ROS, please update gazebo with OSRF version from http://gazebosim.org/tutorials?tut=install_ubuntu 21 | 22 | libgazeboX-dev must be installed, X be your gazebo version (9 on ROS melodic). 23 | 24 | For Gazebo X 25 | ```` 26 | sudo apt-get install libgazeboX-dev 27 | ```` 28 | 29 | ```` 30 | git clone https://github.com/khancyr/ardupilot_gazebo 31 | cd ardupilot_gazebo 32 | mkdir build 33 | cd build 34 | cmake .. 35 | make -j4 36 | sudo make install 37 | ```` 38 | 39 | ```` 40 | echo 'source /usr/share/gazebo/setup.sh' >> ~/.bashrc 41 | ```` 42 | 43 | Set Path of Gazebo Models (Adapt the path to where to clone the repo) 44 | ```` 45 | echo 'export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models' >> ~/.bashrc 46 | ```` 47 | 48 | Set Path of Gazebo Worlds (Adapt the path to where to clone the repo) 49 | ```` 50 | echo 'export GAZEBO_RESOURCE_PATH=~/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}' >> ~/.bashrc 51 | ```` 52 | 53 | ```` 54 | source ~/.bashrc 55 | ```` 56 | 57 | DONE ! 58 | 59 | Now launch a world file with a copter/rover/plane and ardupilot plugin, and it should work! 60 | (I will try to add some world file and model later) 61 | 62 | ## HELP 63 | 64 | How to Launch : 65 | Launch Ardupilot Software In the Loop Simulation for each vehicle. 66 | On new terminal, launch Gazebo with basic demo world. 67 | 68 | #####ROVER (no model provided for now) 69 | 70 | On 1st Terminal (Launch Ardupilot SITL) 71 | ```` 72 | sim_vehicle.py -v APMrover2 -f gazebo-rover --map --console 73 | ```` 74 | 75 | On 2nd Terminal (Launch Gazebo with demo Rover model) 76 | ```` 77 | gazebo --verbose worlds/ (Please Add if there is one.) 78 | ```` 79 | 80 | ##### COPTER 81 | 82 | On 1st Terminal (Launch Ardupilot SITL) 83 | ```` 84 | sim_vehicle.py -v ArduCopter -f gazebo-iris --map --console 85 | ```` 86 | 87 | On 2nd Terminal (Launch Gazebo with demo 3DR Iris model) 88 | ```` 89 | gazebo --verbose worlds/iris_arducopter_runway.world 90 | ```` 91 | 92 | ##### PLANE 93 | 94 | On 1st Terminal (Launch Ardupilot SITL) 95 | ```` 96 | sim_vehicle.py -v ArduPlane -f gazebo-zephyr --map --console 97 | ```` 98 | 99 | On 2nd Terminal (Launch Gazebo with demo Zephyr flying wing model) 100 | ```` 101 | gazebo --verbose worlds/zephyr_ardupilot_demo.world 102 | ```` 103 | 104 | In addition, you can use any GCS of Ardupilot locally or remotely (will require connection setup). 105 | If MAVProxy Developer GCS is uncomportable. Omit --map --console arguments out of SITL launch and use APMPlanner 2 or QGroundControl instead. 106 | Local connection with APMPlanner2/QGroundControl is automatic, and recommended. 107 | 108 | ## Troubleshooting 109 | 110 | ### Missing libArduPilotPlugin.so... etc 111 | 112 | In case you see this message when you launch gazebo with demo worlds, check you have no error after sudo make install. 113 | If no error use "ls" on the install path given to see if the plugin is really here. 114 | If this is correct, check with `cat /usr/share/gazebo/setup.sh` the variable `GAZEBO_PLUGIN_PATH`. It should be the same as the install path. If not use `cp` to copy the lib to right path. 115 | 116 | For Example 117 | 118 | ```` 119 | sudo cp -a /usr/lib/x86_64-linux-gnu/gazebo-7.0/plugins/ /usr/lib/x86_64-linux-gnu/gazebo-7/ 120 | ```` 121 | 122 | path mismatch is confirmed as ROS's glitch. It will be fixed. 123 | 124 | ### Future(not activated yet) 125 | To use Gazebo gps, you must offset the heading of +90° as gazebo gps is NWU and ardupilot is NED 126 | (I don't use GPS altitude for now) 127 | example : for SITL default location 128 | ```` 129 | 130 | EARTH_WGS84 131 | -35.363261 132 | 149.165230 133 | 584 134 | 87 135 | 136 | ```` 137 | Rangefinder 138 | -------------------------------------------------------------------------------- /cmake/cmake_uninstall.cmake.in: -------------------------------------------------------------------------------- 1 | # Source: https://gitlab.kitware.com/cmake/community/-/wikis/FAQ#can-i-do-make-uninstall-with-cmake 2 | 3 | if(NOT EXISTS "@CMAKE_BINARY_DIR@/install_manifest.txt") 4 | message(FATAL_ERROR "Cannot find install manifest: @CMAKE_BINARY_DIR@/install_manifest.txt") 5 | endif() 6 | 7 | file(READ "@CMAKE_BINARY_DIR@/install_manifest.txt" files) 8 | string(REGEX REPLACE "\n" ";" files "${files}") 9 | foreach(file ${files}) 10 | message(STATUS "Uninstalling $ENV{DESTDIR}${file}") 11 | if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 12 | exec_program( 13 | "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" 14 | OUTPUT_VARIABLE rm_out 15 | RETURN_VALUE rm_retval 16 | ) 17 | if(NOT "${rm_retval}" STREQUAL 0) 18 | message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") 19 | endif() 20 | else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 21 | message(STATUS "File $ENV{DESTDIR}${file} does not exist.") 22 | endif() 23 | endforeach() 24 | -------------------------------------------------------------------------------- /include/ArduCopterIRLockPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 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 | 18 | #ifndef _GAZEBO_ARDUCOPTERIRLOCK_PLUGIN_HH_ 19 | #define _GAZEBO_ARDUCOPTERIRLOCK_PLUGIN_HH_ 20 | 21 | #include 22 | #include 23 | 24 | #include "gazebo/common/Plugin.hh" 25 | #include "gazebo/util/system.hh" 26 | 27 | namespace gazebo 28 | { 29 | // Forward declare private class. 30 | class ArduCopterIRLockPluginPrivate; 31 | 32 | /// \brief A camera sensor plugin for fiducial detection 33 | class GAZEBO_VISIBLE ArduCopterIRLockPlugin : public SensorPlugin 34 | { 35 | /// \brief Constructor 36 | public: ArduCopterIRLockPlugin(); 37 | 38 | /// \brief Destructor 39 | public: virtual ~ArduCopterIRLockPlugin(); 40 | 41 | // Documentation Inherited. 42 | public: void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); 43 | 44 | /// \brief Callback when a new camera frame is available 45 | /// \param[in] _image image data 46 | /// \param[in] _width image width 47 | /// \param[in] _height image height 48 | /// \param[in] _depth image depth 49 | /// \param[in] _format image format 50 | public: virtual void OnNewFrame(const unsigned char *_image, 51 | unsigned int _width, unsigned int _height, unsigned int _depth, 52 | const std::string &_format); 53 | 54 | /// \brief Publish the result 55 | /// \param[in] _name Name of fiducial 56 | /// \param[in] _x x position in image 57 | /// \param[in] _y y position in image 58 | public: virtual void Publish(const std::string &_fiducial, unsigned int _x, 59 | unsigned int _y); 60 | 61 | /// \internal 62 | /// \brief Pointer to private data. 63 | private: std::unique_ptr dataPtr; 64 | }; 65 | } 66 | #endif 67 | -------------------------------------------------------------------------------- /include/ArduPilotPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 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 | #ifndef GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ 18 | #define GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace gazebo 25 | { 26 | // Forward declare private data class 27 | class ArduPilotSocketPrivate; 28 | class ArduPilotPluginPrivate; 29 | 30 | /// \brief Interface ArduPilot from ardupilot stack 31 | /// modeled after SITL/SIM_* 32 | /// 33 | /// The plugin requires the following parameters: 34 | /// control description block 35 | /// 36 | /// channel attribute, ardupilot control channel 37 | /// multiplier command multiplier 38 | /// 39 | /// type type of control, VELOCITY, POSITION or EFFORT 40 | /// velocity pid p gain 41 | /// velocity pid i gain 42 | /// velocity pid d gain 43 | /// velocity pid max integral correction 44 | /// velocity pid min integral correction 45 | /// velocity pid max command torque 46 | /// velocity pid min command torque 47 | /// motor joint, torque applied here 48 | /// rotor turning direction, 'cw' or 'ccw' 49 | /// frequencyCutoff filter incoming joint state 50 | /// samplingRate sampling rate for filtering incoming joint state 51 | /// for rotor aliasing problem, experimental 52 | /// scoped name for the imu sensor 53 | /// timeout before giving up on 54 | /// controller synchronization 55 | class GAZEBO_VISIBLE ArduPilotPlugin : public ModelPlugin 56 | { 57 | /// \brief Constructor. 58 | public: ArduPilotPlugin(); 59 | 60 | /// \brief Destructor. 61 | public: ~ArduPilotPlugin(); 62 | 63 | // Documentation Inherited. 64 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 65 | 66 | /// \brief Update the control surfaces controllers. 67 | /// \param[in] _info Update information provided by the server. 68 | private: void OnUpdate(); 69 | 70 | /// \brief Update PID Joint controllers. 71 | /// \param[in] _dt time step size since last update. 72 | private: void ApplyMotorForces(const double _dt); 73 | 74 | /// \brief Reset PID Joint controllers. 75 | private: void ResetPIDs(); 76 | 77 | /// \brief Receive motor commands from ArduPilot 78 | private: void ReceiveMotorCommand(); 79 | 80 | /// \brief Send state to ArduPilot 81 | private: void SendState() const; 82 | 83 | /// \brief Init ardupilot socket 84 | private: bool InitArduPilotSockets(sdf::ElementPtr _sdf) const; 85 | 86 | /// \brief Private data pointer. 87 | private: std::unique_ptr dataPtr; 88 | 89 | /// \brief transform from model orientation to x-forward and z-up 90 | private: ignition::math::Pose3d modelXYZToAirplaneXForwardZDown; 91 | 92 | /// \brief transform from world frame to NED frame 93 | private: ignition::math::Pose3d gazeboXYZToNED; 94 | }; 95 | } 96 | #endif 97 | -------------------------------------------------------------------------------- /include/GimbalSmall2dPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 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 | #ifndef GAZEBO_PLUGINS_GIMBALSMALL2DPLUGIN_HH_ 18 | #define GAZEBO_PLUGINS_GIMBALSMALL2DPLUGIN_HH_ 19 | 20 | #include "gazebo/common/Plugin.hh" 21 | #include "gazebo/physics/physics.hh" 22 | #include "gazebo/util/system.hh" 23 | 24 | namespace gazebo 25 | { 26 | // Forward declare private data class 27 | class GimbalSmall2dPluginPrivate; 28 | 29 | /// \brief A plugin for controlling the angle of a gimbal joint 30 | class GAZEBO_VISIBLE GimbalSmall2dPlugin : public ModelPlugin 31 | { 32 | /// \brief Constructor 33 | public: GimbalSmall2dPlugin(); 34 | 35 | // Documentation Inherited. 36 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 37 | 38 | // Documentation Inherited. 39 | public: virtual void Init(); 40 | 41 | /// \brief Callback on world update 42 | private: void OnUpdate(); 43 | 44 | /// \brief Private data pointer 45 | private: std::unique_ptr dataPtr; 46 | }; 47 | } 48 | #endif 49 | -------------------------------------------------------------------------------- /include/SelectionBuffer.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Open Source Robotics Foundation 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 | #ifndef _GAZEBO_RENDERING_SELECTION_BUFFER_SELECTIONBUFFER_HH_ 18 | #define _GAZEBO_RENDERING_SELECTION_BUFFER_SELECTIONBUFFER_HH_ 19 | 20 | #include 21 | #include 22 | #include "gazebo/util/system.hh" 23 | 24 | namespace Ogre 25 | { 26 | class Entity; 27 | class RenderTarget; 28 | class SceneManager; 29 | } 30 | 31 | namespace gazebo 32 | { 33 | namespace rendering 34 | { 35 | struct SelectionBufferPrivate; 36 | 37 | class GZ_RENDERING_VISIBLE SelectionBuffer 38 | { 39 | /// \brief Constructor 40 | /// \param[in] _camera Name of the camera to generate a selection 41 | /// buffer for. 42 | /// \param[in] _mgr Pointer to the scene manager. 43 | /// \param[in] _renderTarget Pointer to the render target. 44 | public: SelectionBuffer(const std::string &_cameraName, 45 | Ogre::SceneManager *_mgr, Ogre::RenderTarget *_renderTarget); 46 | 47 | /// \brief Destructor 48 | public: ~SelectionBuffer(); 49 | 50 | /// \brief Handle on mouse click 51 | /// \param[in] _x X coordinate in pixels. 52 | /// \param[in] _y Y coordinate in pixels. 53 | /// \return Returns the Ogre entity at the coordinate. 54 | public: Ogre::Entity *OnSelectionClick(int _x, int _y); 55 | 56 | /// \brief Debug show overlay 57 | /// \param[in] _show True to show the selection buffer in an overlay. 58 | public: void ShowOverlay(bool _show); 59 | 60 | /// \brief Call this to update the selection buffer contents 61 | public: void Update(); 62 | 63 | /// \brief Delete the render texture 64 | private: void DeleteRTTBuffer(); 65 | 66 | /// \brief Create the render texture 67 | private: void CreateRTTBuffer(); 68 | 69 | /// \brief Create the selection buffer offscreen render texture. 70 | private: void CreateRTTOverlays(); 71 | 72 | /// \internal 73 | /// \brief Pointer to private data. 74 | private: std::unique_ptr dataPtr; 75 | }; 76 | } 77 | } 78 | #endif 79 | -------------------------------------------------------------------------------- /models/iris_with_ardupilot/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Iris with Standoffs and Camera LiftDrag ArduCopter Plugins 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Fadri Furrer 10 | fadri.furrer@mavt.ethz.ch 11 | 12 | 13 | Michael Burri 14 | 15 | 16 | Mina Kamel 17 | 18 | 19 | Janosch Nikolic 20 | 21 | 22 | Markus Achtelik 23 | 24 | 25 | john hsu 26 | 27 | 28 | 29 | starting with iris_with_standoffs 30 | add LiftDragPlugin 31 | add ArduCopterPlugin 32 | attach gimbal_small_2d model with GimbalSmall2dPlugin 33 | 34 | 35 | 36 | model://gimbal_small_2d 37 | 1.0 38 | 39 | 40 | model://iris_with_standoffs 41 | 1.0 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /models/iris_with_ardupilot/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://iris_with_standoffs 6 | 7 | 8 | 9 | model://gimbal_small_2d 10 | 0 -0.01 0.070 1.57 0 1.57 11 | 12 | 13 | 14 | iris::base_link 15 | gimbal_small_2d::base_link 16 | 17 | 18 | 0 19 | 0 20 | 21 | 0 0 1 22 | true 23 | 24 | 25 | 382 | 383 | 384 | 385 | 0.3 386 | 1.4 387 | 4.2500 388 | 0.10 389 | 0.00 390 | -0.025 391 | 0.0 392 | 0.0 393 | 0.002 394 | 1.2041 395 | 0.084 0 0 396 | 0 1 0 397 | 0 0 1 398 | iris::rotor_0 399 | 400 | 401 | 0.3 402 | 1.4 403 | 4.2500 404 | 0.10 405 | 0.00 406 | -0.025 407 | 0.0 408 | 0.0 409 | 0.002 410 | 1.2041 411 | -0.084 0 0 412 | 0 -1 0 413 | 0 0 1 414 | iris::rotor_0 415 | 416 | 417 | 418 | 0.3 419 | 1.4 420 | 4.2500 421 | 0.10 422 | 0.00 423 | -0.025 424 | 0.0 425 | 0.0 426 | 0.002 427 | 1.2041 428 | 0.084 0 0 429 | 0 1 0 430 | 0 0 1 431 | iris::rotor_1 432 | 433 | 434 | 0.3 435 | 1.4 436 | 4.2500 437 | 0.10 438 | 0.00 439 | -0.025 440 | 0.0 441 | 0.0 442 | 0.002 443 | 1.2041 444 | -0.084 0 0 445 | 0 -1 0 446 | 0 0 1 447 | iris::rotor_1 448 | 449 | 450 | 451 | 0.3 452 | 1.4 453 | 4.2500 454 | 0.10 455 | 0.00 456 | -0.025 457 | 0.0 458 | 0.0 459 | 0.002 460 | 1.2041 461 | 0.084 0 0 462 | 0 -1 0 463 | 0 0 1 464 | iris::rotor_2 465 | 466 | 467 | 0.3 468 | 1.4 469 | 4.2500 470 | 0.10 471 | 0.00 472 | -0.025 473 | 0.0 474 | 0.0 475 | 0.002 476 | 1.2041 477 | -0.084 0 0 478 | 0 1 0 479 | 0 0 1 480 | iris::rotor_2 481 | 482 | 483 | 484 | 0.3 485 | 1.4 486 | 4.2500 487 | 0.10 488 | 0.00 489 | -0.025 490 | 0.0 491 | 0.0 492 | 0.002 493 | 1.2041 494 | 0.084 0 0 495 | 0 -1 0 496 | 0 0 1 497 | iris::rotor_3 498 | 499 | 500 | 0.3 501 | 1.4 502 | 4.2500 503 | 0.10 504 | 0.00 505 | -0.025 506 | 0.0 507 | 0.0 508 | 0.002 509 | 1.2041 510 | -0.084 0 0 511 | 0 1 0 512 | 0 0 1 513 | iris::rotor_3 514 | 515 | 516 | 127.0.0.1 517 | 9002 518 | 9003 519 | 523 | 0 0 0 3.141593 0 0 524 | 0 0 0 3.141593 0 0 525 | iris_demo::iris::iris/imu_link::imu_sensor 526 | 5 527 | 528 | 535 | VELOCITY 536 | 0 537 | 0.20 538 | 0 539 | 0 540 | 0 541 | 0 542 | 2.5 543 | -2.5 544 | iris::rotor_0_joint 545 | 838 546 | 1 547 | 548 | 549 | VELOCITY 550 | 0 551 | 0.20 552 | 0 553 | 0 554 | 0 555 | 0 556 | 2.5 557 | -2.5 558 | iris::rotor_1_joint 559 | 838 560 | 1 561 | 562 | 563 | VELOCITY 564 | 0 565 | 0.20 566 | 0 567 | 0 568 | 0 569 | 0 570 | 2.5 571 | -2.5 572 | iris::rotor_2_joint 573 | -838 574 | 1 575 | 576 | 577 | VELOCITY 578 | 0 579 | 0.20 580 | 0 581 | 0 582 | 0 583 | 0 584 | 2.5 585 | -2.5 586 | iris::rotor_3_joint 587 | -838 588 | 1 589 | 590 | 591 | 592 | 593 | 594 | -------------------------------------------------------------------------------- /models/iris_with_standoffs/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Iris with Standoffs 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Fadri Furrer 10 | fadri.furrer@mavt.ethz.ch 11 | 12 | 13 | Michael Burri 14 | 15 | 16 | Mina Kamel 17 | 18 | 19 | Janosch Nikolic 20 | 21 | 22 | Markus Achtelik 23 | 24 | 25 | john hsu 26 | 27 | 28 | 29 | A copy of the 3DR Iris model taken from 30 | https://github.com/PX4/sitl_gazebo/tree/master/models 31 | Local modifications include adding standoffs, 32 | inertia and collision updates.. 33 | 34 | 35 | -------------------------------------------------------------------------------- /models/iris_with_standoffs/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.194923 0 0 0 5 | 6 | 7 | 0.0 8 | 0.0 9 | 10 | 11 | 0 0 0 0 0 0 12 | 1.5 13 | 14 | 0.008 15 | 0 16 | 0 17 | 0.015 18 | 0 19 | 0.017 20 | 21 | 22 | 23 | 0 0 -0.08 0 0 0 24 | 25 | 26 | 0.47 0.47 0.23 27 | 28 | 29 | 30 | 31 | 32 | 100.0 33 | 0.001 34 | 35 | 36 | 37 | 38 | 100000.0 39 | 100000.0 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | model://iris_with_standoffs/meshes/iris.dae 48 | 49 | 50 | 51 | 54 | 55 | 56 | 57 | 58 | 0.123 0.22 -0.11 0 0 0 59 | 60 | 61 | 0.005 62 | 0.17 63 | 64 | 65 | 66 | 70 | 71 | 72 | 73 | 0.123 -0.22 -0.11 0 0 0 74 | 75 | 76 | 0.005 77 | 0.17 78 | 79 | 80 | 81 | 85 | 86 | 87 | 88 | -0.140 0.21 -0.11 0 0 0 89 | 90 | 91 | 0.005 92 | 0.17 93 | 94 | 95 | 96 | 100 | 101 | 102 | 103 | -0.140 -0.21 -0.11 0 0 0 104 | 105 | 106 | 0.005 107 | 0.17 108 | 109 | 110 | 111 | 115 | 116 | 117 | 118 | 119 | 0 0 0 0 -0 0 120 | 121 | 0 0 0 0 -0 0 122 | 0.15 123 | 124 | 0.0001 125 | 0 126 | 0 127 | 0.0002 128 | 0 129 | 0.0002 130 | 131 | 132 | 133 | 134 | iris/ground_truth/odometry_sensorgt_link 135 | base_link 136 | 137 | 0 0 1 138 | 139 | 0 140 | 0 141 | 0 142 | 0 143 | 144 | 145 | 1.0 146 | 147 | 1 148 | 149 | 150 | 151 | 1 152 | 153 | 154 | 155 | 156 | 157 | 0 0 0 0 0 0 158 | 0.15 159 | 160 | 0.00001 161 | 0 162 | 0 163 | 0.00002 164 | 0 165 | 0.00002 166 | 167 | 168 | 169 | 0 0 0 3.141593 0 0 170 | 1 171 | 1000.0 172 | 173 | 174 | 175 | iris/imu_link 176 | base_link 177 | 178 | 0 0 1 179 | 180 | 0 181 | 0 182 | 0 183 | 0 184 | 185 | 186 | 1.0 187 | 188 | 1 189 | 190 | 191 | 192 | 1 193 | 194 | 195 | 196 | 231 | 232 | 0.13 -0.22 0.023 0 -0 0 233 | 234 | 0 0 0 0 -0 0 235 | 0.025 236 | 237 | 9.75e-06 238 | 0 239 | 0 240 | 0.000166704 241 | 0 242 | 0.000167604 243 | 244 | 245 | 246 | 0 0 0 0 -0 0 247 | 248 | 249 | 0.005 250 | 0.1 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 0 0 0 0 -0 0 264 | 265 | 266 | 1 1 1 267 | model://iris_with_standoffs/meshes/iris_prop_ccw.dae 268 | 269 | 270 | 271 | 275 | 276 | 277 | 1 278 | 279 | 0 280 | 281 | 282 | rotor_0 283 | base_link 284 | 285 | 0 0 1 286 | 287 | -1e+16 288 | 1e+16 289 | 290 | 291 | 0.004 292 | 293 | 1 294 | 295 | 296 | 297 | 1 298 | 299 | 300 | 301 | 302 | -0.13 0.2 0.023 0 -0 0 303 | 304 | 0 0 0 0 -0 0 305 | 0.025 306 | 307 | 9.75e-06 308 | 0 309 | 0 310 | 0.000166704 311 | 0 312 | 0.000167604 313 | 314 | 315 | 316 | 0 0 0 0 -0 0 317 | 318 | 319 | 0.005 320 | 0.1 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 0 0 0 0 -0 0 334 | 335 | 336 | 1 1 1 337 | model://iris_with_standoffs/meshes/iris_prop_ccw.dae 338 | 339 | 340 | 341 | 345 | 346 | 347 | 1 348 | 349 | 0 350 | 351 | 352 | rotor_1 353 | base_link 354 | 355 | 0 0 1 356 | 357 | -1e+16 358 | 1e+16 359 | 360 | 361 | 0.004 362 | 363 | 1 364 | 365 | 366 | 367 | 1 368 | 369 | 370 | 371 | 372 | 0.13 0.22 0.023 0 -0 0 373 | 374 | 0 0 0 0 -0 0 375 | 0.025 376 | 377 | 9.75e-06 378 | 0 379 | 0 380 | 0.000166704 381 | 0 382 | 0.000167604 383 | 384 | 385 | 386 | 0 0 0 0 -0 0 387 | 388 | 389 | 0.005 390 | 0.1 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 0 0 0 0 -0 0 404 | 405 | 406 | 1 1 1 407 | model://iris_with_standoffs/meshes/iris_prop_cw.dae 408 | 409 | 410 | 411 | 415 | 416 | 417 | 1 418 | 419 | 0 420 | 421 | 422 | rotor_2 423 | base_link 424 | 425 | 0 0 1 426 | 427 | -1e+16 428 | 1e+16 429 | 430 | 431 | 0.004 432 | 433 | 1 434 | 435 | 436 | 437 | 1 438 | 439 | 440 | 441 | 442 | -0.13 -0.2 0.023 0 -0 0 443 | 444 | 0 0 0 0 -0 0 445 | 0.025 446 | 447 | 9.75e-06 448 | 0 449 | 0 450 | 0.000166704 451 | 0 452 | 0.000167604 453 | 454 | 455 | 456 | 0 0 0 0 -0 0 457 | 458 | 459 | 0.005 460 | 0.1 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 0 0 0 0 -0 0 474 | 475 | 476 | 1 1 1 477 | model://iris_with_standoffs/meshes/iris_prop_cw.dae 478 | 479 | 480 | 481 | 485 | 486 | 487 | 1 488 | 489 | 0 490 | 491 | 492 | rotor_3 493 | base_link 494 | 495 | 0 0 1 496 | 497 | -1e+16 498 | 1e+16 499 | 500 | 501 | 0.004 502 | 503 | 1 504 | 505 | 506 | 507 | 1 508 | 509 | 510 | 511 | 0 512 | 513 | 514 | -------------------------------------------------------------------------------- /src/ArduCopterIRLockPlugin.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 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 | 18 | #include 19 | #include 20 | 21 | #ifdef _WIN32 22 | #include 23 | #include 24 | #include 25 | #include 26 | using raw_type = char; 27 | #else 28 | #include 29 | #include 30 | #include 31 | #include 32 | using raw_type = void; 33 | #endif 34 | 35 | #if defined(_MSC_VER) 36 | #include 37 | typedef SSIZE_T ssize_t; 38 | #endif 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include "include/ArduCopterIRLockPlugin.hh" 51 | 52 | using namespace gazebo; 53 | GZ_REGISTER_SENSOR_PLUGIN(ArduCopterIRLockPlugin) 54 | 55 | namespace gazebo 56 | { 57 | class ArduCopterIRLockPluginPrivate 58 | { 59 | /// \brief Pointer to the parent camera sensor 60 | public: sensors::CameraSensorPtr parentSensor; 61 | 62 | /// \brief Selection buffer used for occlusion detection 63 | public: std::unique_ptr selectionBuffer; 64 | 65 | /// \brief All event connections. 66 | public: std::vector connections; 67 | 68 | /// \brief A list of fiducials tracked by this camera. 69 | public: std::vector fiducials; 70 | 71 | /// \brief Irlock address 72 | public: std::string irlock_addr; 73 | 74 | /// \brief Irlock port for receiver socket 75 | public: uint16_t irlock_port; 76 | 77 | public: int handle; 78 | 79 | public: struct irlockPacket 80 | { 81 | uint64_t timestamp; 82 | uint16_t num_targets; 83 | float pos_x; 84 | float pos_y; 85 | float size_x; 86 | float size_y; 87 | }; 88 | }; 89 | } 90 | 91 | ///////////////////////////////////////////////// 92 | ignition::math::Vector2i GetScreenSpaceCoords(ignition::math::Vector3d _pt, 93 | gazebo::rendering::CameraPtr _cam) 94 | { 95 | // Convert from 3D world pos to 2D screen pos 96 | Ogre::Vector3 pos = _cam->OgreCamera()->getProjectionMatrix() * 97 | _cam->OgreCamera()->getViewMatrix() * 98 | gazebo::rendering::Conversions::Convert(_pt); 99 | 100 | ignition::math::Vector2i screenPos; 101 | screenPos.X() = ((pos.x / 2.0) + 0.5) * _cam->ViewportWidth(); 102 | screenPos.Y() = (1 - ((pos.y / 2.0) + 0.5)) * _cam->ViewportHeight(); 103 | 104 | return screenPos; 105 | } 106 | 107 | ///////////////////////////////////////////////// 108 | ArduCopterIRLockPlugin::ArduCopterIRLockPlugin() 109 | : SensorPlugin(), 110 | dataPtr(new ArduCopterIRLockPluginPrivate) 111 | { 112 | // socket 113 | this->dataPtr->handle = socket(AF_INET, SOCK_DGRAM /*SOCK_STREAM*/, 0); 114 | #ifndef _WIN32 115 | // Windows does not support FD_CLOEXEC 116 | fcntl(this->dataPtr->handle, F_SETFD, FD_CLOEXEC); 117 | #endif 118 | int one = 1; 119 | setsockopt(this->dataPtr->handle, IPPROTO_TCP, TCP_NODELAY, 120 | reinterpret_cast(&one), sizeof(one)); 121 | setsockopt(this->dataPtr->handle, SOL_SOCKET, SO_REUSEADDR, 122 | reinterpret_cast(&one), sizeof(one)); 123 | 124 | #ifdef _WIN32 125 | u_long on = 1; 126 | ioctlsocket(this->dataPtr->handle, FIONBIO, 127 | reinterpret_cast(&on)); 128 | #else 129 | fcntl(this->dataPtr->handle, F_SETFL, 130 | fcntl(this->dataPtr->handle, F_GETFL, 0) | O_NONBLOCK); 131 | #endif 132 | } 133 | 134 | ///////////////////////////////////////////////// 135 | ArduCopterIRLockPlugin::~ArduCopterIRLockPlugin() 136 | { 137 | this->dataPtr->connections.clear(); 138 | this->dataPtr->parentSensor.reset(); 139 | } 140 | 141 | ///////////////////////////////////////////////// 142 | void ArduCopterIRLockPlugin::Load(sensors::SensorPtr _sensor, 143 | sdf::ElementPtr _sdf) 144 | { 145 | this->dataPtr->parentSensor = 146 | std::dynamic_pointer_cast(_sensor); 147 | 148 | if (!this->dataPtr->parentSensor) 149 | { 150 | gzerr << "ArduCopterIRLockPlugin not attached to a camera sensor\n"; 151 | return; 152 | } 153 | 154 | // load the fiducials 155 | if (_sdf->HasElement("fiducial")) 156 | { 157 | sdf::ElementPtr elem = _sdf->GetElement("fiducial"); 158 | while (elem) 159 | { 160 | this->dataPtr->fiducials.push_back(elem->Get()); 161 | elem = elem->GetNextElement("fiducial"); 162 | } 163 | } 164 | else 165 | { 166 | gzerr << "No fidicuals specified. ArduCopterIRLockPlugin will not be run." 167 | << std::endl; 168 | return; 169 | } 170 | this->dataPtr->irlock_addr = 171 | _sdf->Get("irlock_addr", static_cast("127.0.0.1")).first; 172 | this->dataPtr->irlock_port = 173 | _sdf->Get("irlock_port", 9005).first; 174 | 175 | this->dataPtr->parentSensor->SetActive(true); 176 | 177 | this->dataPtr->connections.push_back( 178 | this->dataPtr->parentSensor->Camera()->ConnectNewImageFrame( 179 | std::bind(&ArduCopterIRLockPlugin::OnNewFrame, this, 180 | std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, 181 | std::placeholders::_4, std::placeholders::_5))); 182 | } 183 | 184 | ///////////////////////////////////////////////// 185 | void ArduCopterIRLockPlugin::OnNewFrame(const unsigned char * /*_image*/, 186 | unsigned int /*_width*/, unsigned int /*_height*/, unsigned int /*_depth*/, 187 | const std::string &/*_format*/) 188 | { 189 | rendering::CameraPtr camera = this->dataPtr->parentSensor->Camera(); 190 | rendering::ScenePtr scene = camera->GetScene(); 191 | 192 | if (!this->dataPtr->selectionBuffer) 193 | { 194 | std::string cameraName = camera->OgreCamera()->getName(); 195 | this->dataPtr->selectionBuffer.reset( 196 | new rendering::SelectionBuffer(cameraName, scene->OgreSceneManager(), 197 | camera->RenderTexture()->getBuffer()->getRenderTarget())); 198 | } 199 | 200 | for (const auto &f : this->dataPtr->fiducials) 201 | { 202 | // check if fiducial is visible within the frustum 203 | rendering::VisualPtr vis = scene->GetVisual(f); 204 | if (!vis) 205 | continue; 206 | 207 | if (!camera->IsVisible(vis)) 208 | continue; 209 | 210 | ignition::math::Vector2i pt = GetScreenSpaceCoords( 211 | vis->WorldPose().Pos(), camera); 212 | 213 | // use selection buffer to check if visual is occluded by other entities 214 | // in the camera view 215 | Ogre::Entity *entity = 216 | this->dataPtr->selectionBuffer->OnSelectionClick(pt.X(), pt.Y()); 217 | 218 | rendering::VisualPtr result; 219 | if (entity && !entity->getUserObjectBindings().getUserAny().isEmpty()) 220 | { 221 | try 222 | { 223 | result = scene->GetVisual( 224 | Ogre::any_cast( 225 | entity->getUserObjectBindings().getUserAny())); 226 | } 227 | catch(Ogre::Exception &e) 228 | { 229 | gzerr << "Ogre Error:" << e.getFullDescription() << "\n"; 230 | continue; 231 | } 232 | } 233 | 234 | if (result && result->GetRootVisual() == vis) 235 | { 236 | this->Publish(vis->Name(), pt.X(), pt.Y()); 237 | } 238 | } 239 | } 240 | 241 | ///////////////////////////////////////////////// 242 | void ArduCopterIRLockPlugin::Publish(const std::string &/*_fiducial*/, 243 | unsigned int _x, unsigned int _y) 244 | { 245 | rendering::CameraPtr camera = this->dataPtr->parentSensor->Camera(); 246 | 247 | const double imageWidth = this->dataPtr->parentSensor->ImageWidth(); 248 | const double imageHeight = this->dataPtr->parentSensor->ImageHeight(); 249 | const double hfov = camera->HFOV().Radian(); 250 | const double vfov = camera->VFOV().Radian(); 251 | const double pixelsPerRadianX = imageWidth / hfov; 252 | const double pixelsPerRadianY = imageHeight / vfov; 253 | const float angleX = static_cast( 254 | (static_cast(_x) - (imageWidth * 0.5)) / pixelsPerRadianX); 255 | const float angleY = static_cast( 256 | -((imageHeight * 0.5) - static_cast(_y)) / pixelsPerRadianY); 257 | 258 | // send_packet 259 | ArduCopterIRLockPluginPrivate::irlockPacket pkt; 260 | 261 | pkt.timestamp = static_cast 262 | (1.0e3 * this->dataPtr->parentSensor->LastMeasurementTime().Double()); 263 | pkt.num_targets = static_cast(1); 264 | pkt.pos_x = angleX; 265 | pkt.pos_y = angleY; 266 | // 1x1 pixel box for now 267 | pkt.size_x = static_cast(1); 268 | pkt.size_y = static_cast(1); 269 | 270 | // std::cerr << "fiducial '" << _fiducial << "':" << _x << ", " << _y 271 | // << ", pos: " << pkt.pos_x << ", " << pkt.pos_y << std::endl; 272 | 273 | struct sockaddr_in sockaddr; 274 | memset(&sockaddr, 0, sizeof(sockaddr)); 275 | sockaddr.sin_port = htons(this->dataPtr->irlock_port); 276 | sockaddr.sin_family = AF_INET; 277 | sockaddr.sin_addr.s_addr = inet_addr(this->dataPtr->irlock_addr.c_str()); 278 | ::sendto(this->dataPtr->handle, 279 | reinterpret_cast(&pkt), 280 | sizeof(pkt), 0, 281 | (struct sockaddr *)&sockaddr, sizeof(sockaddr)); 282 | } 283 | -------------------------------------------------------------------------------- /src/ArduPilotPlugin.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 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 | #ifdef _WIN32 20 | #include 21 | #include 22 | #include 23 | #include 24 | using raw_type = char; 25 | #else 26 | #include 27 | #include 28 | #include 29 | #include 30 | using raw_type = void; 31 | #endif 32 | 33 | #if defined(_MSC_VER) 34 | #include 35 | typedef SSIZE_T ssize_t; 36 | #endif 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include "include/ArduPilotPlugin.hh" 49 | 50 | #define MAX_MOTORS 255 51 | 52 | using namespace gazebo; 53 | 54 | GZ_REGISTER_MODEL_PLUGIN(ArduPilotPlugin) 55 | 56 | /// \brief A servo packet. 57 | struct ServoPacket 58 | { 59 | /// \brief Motor speed data. 60 | /// should rename to servo_command here and in ArduPilot SIM_Gazebo.cpp 61 | float motorSpeed[MAX_MOTORS] = {0.0f}; 62 | }; 63 | 64 | /// \brief Flight Dynamics Model packet that is sent back to the ArduPilot 65 | struct fdmPacket 66 | { 67 | /// \brief packet timestamp 68 | double timestamp; 69 | 70 | /// \brief IMU angular velocity 71 | double imuAngularVelocityRPY[3]; 72 | 73 | /// \brief IMU linear acceleration 74 | double imuLinearAccelerationXYZ[3]; 75 | 76 | /// \brief IMU quaternion orientation 77 | double imuOrientationQuat[4]; 78 | 79 | /// \brief Model velocity in NED frame 80 | double velocityXYZ[3]; 81 | 82 | /// \brief Model position in NED frame 83 | double positionXYZ[3]; 84 | /* NOT MERGED IN MASTER YET 85 | /// \brief Model latitude in WGS84 system 86 | double latitude = 0.0; 87 | 88 | /// \brief Model longitude in WGS84 system 89 | double longitude = 0.0; 90 | 91 | /// \brief Model altitude from GPS 92 | double altitude = 0.0; 93 | 94 | /// \brief Model estimated from airspeed sensor (e.g. Pitot) in m/s 95 | double airspeed = 0.0; 96 | 97 | /// \brief Battery voltage. Default to -1 to use sitl estimator. 98 | double battery_voltage = -1.0; 99 | 100 | /// \brief Battery Current. 101 | double battery_current = 0.0; 102 | 103 | /// \brief Model rangefinder value. Default to -1 to use sitl rangefinder. 104 | double rangefinder = -1.0; 105 | */ 106 | }; 107 | 108 | /// \brief Control class 109 | class Control 110 | { 111 | /// \brief Constructor 112 | public: Control() 113 | { 114 | // most of these coefficients are not used yet. 115 | this->rotorVelocitySlowdownSim = this->kDefaultRotorVelocitySlowdownSim; 116 | this->frequencyCutoff = this->kDefaultFrequencyCutoff; 117 | this->samplingRate = this->kDefaultSamplingRate; 118 | 119 | this->pid.Init(0.1, 0, 0, 0, 0, 1.0, -1.0); 120 | } 121 | 122 | /// \brief copy constructor 123 | public: Control& operator=(const Control& source) = default; 124 | 125 | /// \brief control id / channel 126 | public: int channel = 0; 127 | 128 | /// \brief Next command to be applied to the propeller 129 | public: double cmd = 0; 130 | 131 | /// \brief Velocity PID for motor control 132 | public: common::PID pid; 133 | 134 | /// \brief Control type. Can be: 135 | /// VELOCITY control velocity of joint 136 | /// POSITION control position of joint 137 | /// EFFORT control effort of joint 138 | public: std::string type; 139 | 140 | /// \brief use force controler 141 | public: bool useForce = true; 142 | 143 | /// \brief Control propeller joint. 144 | public: std::string jointName; 145 | 146 | /// \brief Control propeller joint. 147 | public: physics::JointPtr joint; 148 | 149 | /// \brief direction multiplier for this control 150 | public: double multiplier = 1; 151 | 152 | /// \brief input command offset 153 | public: double offset = 0; 154 | 155 | /// \brief unused coefficients 156 | public: double rotorVelocitySlowdownSim; 157 | public: double frequencyCutoff; 158 | public: double samplingRate; 159 | public: ignition::math::OnePole filter; 160 | 161 | public: static double kDefaultRotorVelocitySlowdownSim; 162 | public: static double kDefaultFrequencyCutoff; 163 | public: static double kDefaultSamplingRate; 164 | }; 165 | 166 | double Control::kDefaultRotorVelocitySlowdownSim = 10.0; 167 | double Control::kDefaultFrequencyCutoff = 5.0; 168 | double Control::kDefaultSamplingRate = 0.2; 169 | 170 | // Private data class 171 | class gazebo::ArduPilotSocketPrivate 172 | { 173 | /// \brief constructor 174 | public: ArduPilotSocketPrivate() 175 | { 176 | // initialize socket udp socket 177 | fd = socket(AF_INET, SOCK_DGRAM, 0); 178 | #ifndef _WIN32 179 | // Windows does not support FD_CLOEXEC 180 | fcntl(fd, F_SETFD, FD_CLOEXEC); 181 | #endif 182 | } 183 | 184 | /// \brief destructor 185 | public: ~ArduPilotSocketPrivate() 186 | { 187 | if (fd != -1) 188 | { 189 | ::close(fd); 190 | fd = -1; 191 | } 192 | } 193 | 194 | /// \brief Bind to an adress and port 195 | /// \param[in] _address Address to bind to. 196 | /// \param[in] _port Port to bind to. 197 | /// \return True on success. 198 | public: bool Bind(const char *_address, const uint16_t _port) 199 | { 200 | struct sockaddr_in sockaddr; 201 | this->MakeSockAddr(_address, _port, sockaddr); 202 | 203 | if (bind(this->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) 204 | { 205 | shutdown(this->fd, 0); 206 | #ifdef _WIN32 207 | closesocket(this->fd); 208 | #else 209 | close(this->fd); 210 | #endif 211 | return false; 212 | } 213 | int one = 1; 214 | setsockopt(this->fd, SOL_SOCKET, SO_REUSEADDR, 215 | reinterpret_cast(&one), sizeof(one)); 216 | 217 | #ifdef _WIN32 218 | u_long on = 1; 219 | ioctlsocket(this->fd, FIONBIO, 220 | reinterpret_cast(&on)); 221 | #else 222 | fcntl(this->fd, F_SETFL, 223 | fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); 224 | #endif 225 | return true; 226 | } 227 | 228 | /// \brief Connect to an adress and port 229 | /// \param[in] _address Address to connect to. 230 | /// \param[in] _port Port to connect to. 231 | /// \return True on success. 232 | public : bool Connect(const char *_address, const uint16_t _port) 233 | { 234 | struct sockaddr_in sockaddr; 235 | this->MakeSockAddr(_address, _port, sockaddr); 236 | 237 | if (connect(this->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) 238 | { 239 | shutdown(this->fd, 0); 240 | #ifdef _WIN32 241 | closesocket(this->fd); 242 | #else 243 | close(this->fd); 244 | #endif 245 | return false; 246 | } 247 | int one = 1; 248 | setsockopt(this->fd, SOL_SOCKET, SO_REUSEADDR, 249 | reinterpret_cast(&one), sizeof(one)); 250 | 251 | #ifdef _WIN32 252 | u_long on = 1; 253 | ioctlsocket(this->fd, FIONBIO, 254 | reinterpret_cast(&on)); 255 | #else 256 | fcntl(this->fd, F_SETFL, 257 | fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); 258 | #endif 259 | return true; 260 | } 261 | 262 | /// \brief Make a socket 263 | /// \param[in] _address Socket address. 264 | /// \param[in] _port Socket port 265 | /// \param[out] _sockaddr New socket address structure. 266 | public: void MakeSockAddr(const char *_address, const uint16_t _port, 267 | struct sockaddr_in &_sockaddr) 268 | { 269 | memset(&_sockaddr, 0, sizeof(_sockaddr)); 270 | 271 | #ifdef HAVE_SOCK_SIN_LEN 272 | _sockaddr.sin_len = sizeof(_sockaddr); 273 | #endif 274 | 275 | _sockaddr.sin_port = htons(_port); 276 | _sockaddr.sin_family = AF_INET; 277 | _sockaddr.sin_addr.s_addr = inet_addr(_address); 278 | } 279 | 280 | public: ssize_t Send(const void *_buf, size_t _size) 281 | { 282 | return send(this->fd, _buf, _size, 0); 283 | } 284 | 285 | /// \brief Receive data 286 | /// \param[out] _buf Buffer that receives the data. 287 | /// \param[in] _size Size of the buffer. 288 | /// \param[in] _timeoutMS Milliseconds to wait for data. 289 | public: ssize_t Recv(void *_buf, const size_t _size, uint32_t _timeoutMs) 290 | { 291 | fd_set fds; 292 | struct timeval tv; 293 | 294 | FD_ZERO(&fds); 295 | FD_SET(this->fd, &fds); 296 | 297 | tv.tv_sec = _timeoutMs / 1000; 298 | tv.tv_usec = (_timeoutMs % 1000) * 1000UL; 299 | 300 | if (select(this->fd+1, &fds, NULL, NULL, &tv) != 1) 301 | { 302 | return -1; 303 | } 304 | 305 | #ifdef _WIN32 306 | return recv(this->fd, reinterpret_cast(_buf), _size, 0); 307 | #else 308 | return recv(this->fd, _buf, _size, 0); 309 | #endif 310 | } 311 | 312 | /// \brief Socket handle 313 | private: int fd; 314 | }; 315 | 316 | // Private data class 317 | class gazebo::ArduPilotPluginPrivate 318 | { 319 | /// \brief Pointer to the update event connection. 320 | public: event::ConnectionPtr updateConnection; 321 | 322 | /// \brief Pointer to the model; 323 | public: physics::ModelPtr model; 324 | 325 | /// \brief String of the model name; 326 | public: std::string modelName; 327 | 328 | /// \brief array of propellers 329 | public: std::vector controls; 330 | 331 | /// \brief keep track of controller update sim-time. 332 | public: gazebo::common::Time lastControllerUpdateTime; 333 | 334 | /// \brief Controller update mutex. 335 | public: std::mutex mutex; 336 | 337 | /// \brief Ardupilot Socket for receive motor command on gazebo 338 | public: ArduPilotSocketPrivate socket_in; 339 | 340 | /// \brief Ardupilot Socket to send state to Ardupilot 341 | public: ArduPilotSocketPrivate socket_out; 342 | 343 | /// \brief Ardupilot address 344 | public: std::string fdm_addr; 345 | 346 | /// \brief The Ardupilot listen address 347 | public: std::string listen_addr; 348 | 349 | /// \brief Ardupilot port for receiver socket 350 | public: uint16_t fdm_port_in; 351 | 352 | /// \brief Ardupilot port for sender socket 353 | public: uint16_t fdm_port_out; 354 | 355 | /// \brief Pointer to an IMU sensor 356 | public: sensors::ImuSensorPtr imuSensor; 357 | 358 | /// \brief Pointer to an GPS sensor 359 | public: sensors::GpsSensorPtr gpsSensor; 360 | 361 | /// \brief Pointer to an Rangefinder sensor 362 | public: sensors::RaySensorPtr rangefinderSensor; 363 | 364 | /// \brief false before ardupilot controller is online 365 | /// to allow gazebo to continue without waiting 366 | public: bool arduPilotOnline; 367 | 368 | /// \brief number of times ArduCotper skips update 369 | public: int connectionTimeoutCount; 370 | 371 | /// \brief number of times ArduCotper skips update 372 | /// before marking ArduPilot offline 373 | public: int connectionTimeoutMaxCount; 374 | }; 375 | 376 | ///////////////////////////////////////////////// 377 | ArduPilotPlugin::ArduPilotPlugin() 378 | : dataPtr(new ArduPilotPluginPrivate) 379 | { 380 | this->dataPtr->arduPilotOnline = false; 381 | this->dataPtr->connectionTimeoutCount = 0; 382 | } 383 | 384 | ///////////////////////////////////////////////// 385 | ArduPilotPlugin::~ArduPilotPlugin() 386 | { 387 | } 388 | 389 | ///////////////////////////////////////////////// 390 | void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 391 | { 392 | GZ_ASSERT(_model, "ArduPilotPlugin _model pointer is null"); 393 | GZ_ASSERT(_sdf, "ArduPilotPlugin _sdf pointer is null"); 394 | 395 | this->dataPtr->model = _model; 396 | this->dataPtr->modelName = this->dataPtr->model->GetName(); 397 | 398 | // modelXYZToAirplaneXForwardZDown brings us from gazebo model frame: 399 | // x-forward, y-right, z-down 400 | // to the aerospace convention: x-forward, y-left, z-up 401 | this->modelXYZToAirplaneXForwardZDown = 402 | ignition::math::Pose3d(0, 0, 0, 0, 0, 0); 403 | if (_sdf->HasElement("modelXYZToAirplaneXForwardZDown")) 404 | { 405 | this->modelXYZToAirplaneXForwardZDown = 406 | _sdf->Get("modelXYZToAirplaneXForwardZDown"); 407 | } 408 | 409 | // gazeboXYZToNED: from gazebo model frame: x-forward, y-right, z-down 410 | // to the aerospace convention: x-forward, y-left, z-up 411 | this->gazeboXYZToNED = ignition::math::Pose3d(0, 0, 0, IGN_PI, 0, 0); 412 | if (_sdf->HasElement("gazeboXYZToNED")) 413 | { 414 | this->gazeboXYZToNED = _sdf->Get("gazeboXYZToNED"); 415 | } 416 | 417 | // per control channel 418 | sdf::ElementPtr controlSDF; 419 | if (_sdf->HasElement("control")) 420 | { 421 | controlSDF = _sdf->GetElement("control"); 422 | } 423 | else if (_sdf->HasElement("rotor")) 424 | { 425 | gzwarn << "[" << this->dataPtr->modelName << "] " 426 | << "please deprecate block, use block instead.\n"; 427 | controlSDF = _sdf->GetElement("rotor"); 428 | } 429 | 430 | while (controlSDF) 431 | { 432 | Control control; 433 | 434 | if (controlSDF->HasAttribute("channel")) 435 | { 436 | control.channel = 437 | atoi(controlSDF->GetAttribute("channel")->GetAsString().c_str()); 438 | } 439 | else if (controlSDF->HasAttribute("id")) 440 | { 441 | gzwarn << "[" << this->dataPtr->modelName << "] " 442 | << "please deprecate attribute id, use channel instead.\n"; 443 | control.channel = 444 | atoi(controlSDF->GetAttribute("id")->GetAsString().c_str()); 445 | } 446 | else 447 | { 448 | control.channel = this->dataPtr->controls.size(); 449 | gzwarn << "[" << this->dataPtr->modelName << "] " 450 | << "id/channel attribute not specified, use order parsed [" 451 | << control.channel << "].\n"; 452 | } 453 | 454 | if (controlSDF->HasElement("type")) 455 | { 456 | control.type = controlSDF->Get("type"); 457 | } 458 | else 459 | { 460 | gzerr << "[" << this->dataPtr->modelName << "] " 461 | << "Control type not specified," 462 | << " using velocity control by default.\n"; 463 | control.type = "VELOCITY"; 464 | } 465 | 466 | if (control.type != "VELOCITY" && 467 | control.type != "POSITION" && 468 | control.type != "EFFORT") 469 | { 470 | gzwarn << "[" << this->dataPtr->modelName << "] " 471 | << "Control type [" << control.type 472 | << "] not recognized, must be one of VELOCITY, POSITION, EFFORT." 473 | << " default to VELOCITY.\n"; 474 | control.type = "VELOCITY"; 475 | } 476 | 477 | if (controlSDF->HasElement("useForce")) 478 | { 479 | control.useForce = controlSDF->Get("useForce"); 480 | } 481 | 482 | if (controlSDF->HasElement("jointName")) 483 | { 484 | control.jointName = controlSDF->Get("jointName"); 485 | } 486 | else 487 | { 488 | gzerr << "[" << this->dataPtr->modelName << "] " 489 | << "Please specify a jointName," 490 | << " where the control channel is attached.\n"; 491 | } 492 | 493 | // Get the pointer to the joint. 494 | control.joint = _model->GetJoint(control.jointName); 495 | if (control.joint == nullptr) 496 | { 497 | gzerr << "[" << this->dataPtr->modelName << "] " 498 | << "Couldn't find specified joint [" 499 | << control.jointName << "]. This plugin will not run.\n"; 500 | return; 501 | } 502 | 503 | if (controlSDF->HasElement("multiplier")) 504 | { 505 | // overwrite turningDirection, deprecated. 506 | control.multiplier = controlSDF->Get("multiplier"); 507 | } 508 | else if (controlSDF->HasElement("turningDirection")) 509 | { 510 | gzwarn << "[" << this->dataPtr->modelName << "] " 511 | << " is deprecated. Please use" 512 | << " . Map 'cw' to '-1' and 'ccw' to '1'.\n"; 513 | std::string turningDirection = controlSDF->Get( 514 | "turningDirection"); 515 | // special cases mimic from controls_gazebo_plugins 516 | if (turningDirection == "cw") 517 | { 518 | control.multiplier = -1; 519 | } 520 | else if (turningDirection == "ccw") 521 | { 522 | control.multiplier = 1; 523 | } 524 | else 525 | { 526 | gzdbg << "[" << this->dataPtr->modelName << "] " 527 | << "not string, check turningDirection as float\n"; 528 | control.multiplier = controlSDF->Get("turningDirection"); 529 | } 530 | } 531 | else 532 | { 533 | gzdbg << "[" << this->dataPtr->modelName << "] " 534 | << " (or deprecated ) not specified," 535 | << " Default 1 (or deprecated 'ccw').\n"; 536 | control.multiplier = 1; 537 | } 538 | 539 | if (controlSDF->HasElement("offset")) 540 | { 541 | control.offset = controlSDF->Get("offset"); 542 | } 543 | else 544 | { 545 | gzdbg << "[" << this->dataPtr->modelName << "] " 546 | << " not specified, default to 0.\n"; 547 | control.offset = 0; 548 | } 549 | 550 | control.rotorVelocitySlowdownSim = 551 | controlSDF->Get("rotorVelocitySlowdownSim", 1).first; 552 | 553 | if (ignition::math::equal(control.rotorVelocitySlowdownSim, 0.0)) 554 | { 555 | gzwarn << "[" << this->dataPtr->modelName << "] " 556 | << "control for joint [" << control.jointName 557 | << "] rotorVelocitySlowdownSim is zero," 558 | << " assume no slowdown.\n"; 559 | control.rotorVelocitySlowdownSim = 1.0; 560 | } 561 | 562 | control.frequencyCutoff = 563 | controlSDF->Get("frequencyCutoff", control.frequencyCutoff).first; 564 | control.samplingRate = 565 | controlSDF->Get("samplingRate", control.samplingRate).first; 566 | 567 | // use gazebo::math::Filter 568 | control.filter.Fc(control.frequencyCutoff, control.samplingRate); 569 | 570 | // initialize filter to zero value 571 | control.filter.Set(0.0); 572 | 573 | // note to use this filter, do 574 | // stateFiltered = filter.Process(stateRaw); 575 | 576 | // Overload the PID parameters if they are available. 577 | double param; 578 | // carry over from ArduCopter plugin 579 | param = controlSDF->Get("vel_p_gain", control.pid.GetPGain()).first; 580 | control.pid.SetPGain(param); 581 | 582 | param = controlSDF->Get("vel_i_gain", control.pid.GetIGain()).first; 583 | control.pid.SetIGain(param); 584 | 585 | param = controlSDF->Get("vel_d_gain", control.pid.GetDGain()).first; 586 | control.pid.SetDGain(param); 587 | 588 | param = controlSDF->Get("vel_i_max", control.pid.GetIMax()).first; 589 | control.pid.SetIMax(param); 590 | 591 | param = controlSDF->Get("vel_i_min", control.pid.GetIMin()).first; 592 | control.pid.SetIMin(param); 593 | 594 | param = controlSDF->Get("vel_cmd_max", control.pid.GetCmdMax()).first; 595 | control.pid.SetCmdMax(param); 596 | 597 | param = controlSDF->Get("vel_cmd_min", control.pid.GetCmdMin()).first; 598 | control.pid.SetCmdMin(param); 599 | 600 | // new params, overwrite old params if exist 601 | param = controlSDF->Get("p_gain", control.pid.GetPGain()).first; 602 | control.pid.SetPGain(param); 603 | 604 | param = controlSDF->Get("i_gain", control.pid.GetIGain()).first; 605 | control.pid.SetIGain(param); 606 | 607 | param = controlSDF->Get("d_gain", control.pid.GetDGain()).first; 608 | control.pid.SetDGain(param); 609 | 610 | param = controlSDF->Get("i_max", control.pid.GetIMax()).first; 611 | control.pid.SetIMax(param); 612 | 613 | param = controlSDF->Get("i_min", control.pid.GetIMin()).first; 614 | control.pid.SetIMin(param); 615 | 616 | param = controlSDF->Get("cmd_max", control.pid.GetCmdMax()).first; 617 | control.pid.SetCmdMax(param); 618 | 619 | param = controlSDF->Get("cmd_min", control.pid.GetCmdMin()).first; 620 | control.pid.SetCmdMin(param); 621 | 622 | // set pid initial command 623 | control.pid.SetCmd(0.0); 624 | 625 | this->dataPtr->controls.push_back(control); 626 | controlSDF = controlSDF->GetNextElement("control"); 627 | } 628 | 629 | // Get sensors 630 | std::string imuName = 631 | _sdf->Get("imuName", static_cast("imu_sensor")).first; 632 | std::vector imuScopedName = 633 | this->dataPtr->model->SensorScopedName(imuName); 634 | 635 | if (imuScopedName.size() > 1) 636 | { 637 | gzwarn << "[" << this->dataPtr->modelName << "] " 638 | << "multiple names match [" << imuName << "] using first found" 639 | << " name.\n"; 640 | for (unsigned k = 0; k < imuScopedName.size(); ++k) 641 | { 642 | gzwarn << " sensor " << k << " [" << imuScopedName[k] << "].\n"; 643 | } 644 | } 645 | 646 | if (imuScopedName.size() > 0) 647 | { 648 | this->dataPtr->imuSensor = std::dynamic_pointer_cast 649 | (sensors::SensorManager::Instance()->GetSensor(imuScopedName[0])); 650 | } 651 | 652 | if (!this->dataPtr->imuSensor) 653 | { 654 | if (imuScopedName.size() > 1) 655 | { 656 | gzwarn << "[" << this->dataPtr->modelName << "] " 657 | << "first imu_sensor scoped name [" << imuScopedName[0] 658 | << "] not found, trying the rest of the sensor names.\n"; 659 | for (unsigned k = 1; k < imuScopedName.size(); ++k) 660 | { 661 | this->dataPtr->imuSensor = std::dynamic_pointer_cast 662 | (sensors::SensorManager::Instance()->GetSensor(imuScopedName[k])); 663 | if (this->dataPtr->imuSensor) 664 | { 665 | gzwarn << "found [" << imuScopedName[k] << "]\n"; 666 | break; 667 | } 668 | } 669 | } 670 | 671 | if (!this->dataPtr->imuSensor) 672 | { 673 | gzwarn << "[" << this->dataPtr->modelName << "] " 674 | << "imu_sensor scoped name [" << imuName 675 | << "] not found, trying unscoped name.\n" << "\n"; 676 | // TODO: this fails for multi-nested models. 677 | // TODO: and transforms fail for rotated nested model, 678 | // joints point the wrong way. 679 | this->dataPtr->imuSensor = std::dynamic_pointer_cast 680 | (sensors::SensorManager::Instance()->GetSensor(imuName)); 681 | } 682 | 683 | if (!this->dataPtr->imuSensor) 684 | { 685 | gzerr << "[" << this->dataPtr->modelName << "] " 686 | << "imu_sensor [" << imuName 687 | << "] not found, abort ArduPilot plugin.\n" << "\n"; 688 | return; 689 | } 690 | } 691 | /* NOT MERGED IN MASTER YET 692 | // Get GPS 693 | std::string gpsName = _sdf->Get("imuName", static_cast("gps_sensor")).first; 694 | std::vector gpsScopedName = SensorScopedName(this->dataPtr->model, gpsName); 695 | if (gpsScopedName.size() > 1) 696 | { 697 | gzwarn << "[" << this->dataPtr->modelName << "] " 698 | << "multiple names match [" << gpsName << "] using first found" 699 | << " name.\n"; 700 | for (unsigned k = 0; k < gpsScopedName.size(); ++k) 701 | { 702 | gzwarn << " sensor " << k << " [" << gpsScopedName[k] << "].\n"; 703 | } 704 | } 705 | 706 | if (gpsScopedName.size() > 0) 707 | { 708 | this->dataPtr->gpsSensor = std::dynamic_pointer_cast 709 | (sensors::SensorManager::Instance()->GetSensor(gpsScopedName[0])); 710 | } 711 | 712 | if (!this->dataPtr->gpsSensor) 713 | { 714 | if (gpsScopedName.size() > 1) 715 | { 716 | gzwarn << "[" << this->dataPtr->modelName << "] " 717 | << "first gps_sensor scoped name [" << gpsScopedName[0] 718 | << "] not found, trying the rest of the sensor names.\n"; 719 | for (unsigned k = 1; k < gpsScopedName.size(); ++k) 720 | { 721 | this->dataPtr->gpsSensor = std::dynamic_pointer_cast 722 | (sensors::SensorManager::Instance()->GetSensor(gpsScopedName[k])); 723 | if (this->dataPtr->gpsSensor) 724 | { 725 | gzwarn << "found [" << gpsScopedName[k] << "]\n"; 726 | break; 727 | } 728 | } 729 | } 730 | 731 | if (!this->dataPtr->gpsSensor) 732 | { 733 | gzwarn << "[" << this->dataPtr->modelName << "] " 734 | << "gps_sensor scoped name [" << gpsName 735 | << "] not found, trying unscoped name.\n" << "\n"; 736 | this->dataPtr->gpsSensor = std::dynamic_pointer_cast 737 | (sensors::SensorManager::Instance()->GetSensor(gpsName)); 738 | } 739 | 740 | if (!this->dataPtr->gpsSensor) 741 | { 742 | gzwarn << "[" << this->dataPtr->modelName << "] " 743 | << "gps [" << gpsName 744 | << "] not found, skipping gps support.\n" << "\n"; 745 | } 746 | else 747 | { 748 | gzwarn << "[" << this->dataPtr->modelName << "] " 749 | << " found " << " [" << gpsName << "].\n"; 750 | } 751 | } 752 | 753 | // Get Rangefinder 754 | // TODO add sonar 755 | std::string rangefinderName = _sdf->Get("rangefinderName", 756 | static_cast("rangefinder_sensor")).first; 757 | std::vector rangefinderScopedName = SensorScopedName(this->dataPtr->model, rangefinderName); 758 | if (rangefinderScopedName.size() > 1) 759 | { 760 | gzwarn << "[" << this->dataPtr->modelName << "] " 761 | << "multiple names match [" << rangefinderName << "] using first found" 762 | << " name.\n"; 763 | for (unsigned k = 0; k < rangefinderScopedName.size(); ++k) 764 | { 765 | gzwarn << " sensor " << k << " [" << rangefinderScopedName[k] << "].\n"; 766 | } 767 | } 768 | 769 | if (rangefinderScopedName.size() > 0) 770 | { 771 | this->dataPtr->rangefinderSensor = std::dynamic_pointer_cast 772 | (sensors::SensorManager::Instance()->GetSensor(rangefinderScopedName[0])); 773 | } 774 | 775 | if (!this->dataPtr->rangefinderSensor) 776 | { 777 | if (rangefinderScopedName.size() > 1) 778 | { 779 | gzwarn << "[" << this->dataPtr->modelName << "] " 780 | << "first rangefinder_sensor scoped name [" << rangefinderScopedName[0] 781 | << "] not found, trying the rest of the sensor names.\n"; 782 | for (unsigned k = 1; k < rangefinderScopedName.size(); ++k) 783 | { 784 | this->dataPtr->rangefinderSensor = std::dynamic_pointer_cast 785 | (sensors::SensorManager::Instance()->GetSensor(rangefinderScopedName[k])); 786 | if (this->dataPtr->rangefinderSensor) 787 | { 788 | gzwarn << "found [" << rangefinderScopedName[k] << "]\n"; 789 | break; 790 | } 791 | } 792 | } 793 | 794 | if (!this->dataPtr->rangefinderSensor) 795 | { 796 | gzwarn << "[" << this->dataPtr->modelName << "] " 797 | << "rangefinder_sensor scoped name [" << rangefinderName 798 | << "] not found, trying unscoped name.\n" << "\n"; 799 | /// TODO: this fails for multi-nested models. 800 | /// TODO: and transforms fail for rotated nested model, 801 | /// joints point the wrong way. 802 | this->dataPtr->rangefinderSensor = std::dynamic_pointer_cast 803 | (sensors::SensorManager::Instance()->GetSensor(rangefinderName)); 804 | } 805 | if (!this->dataPtr->rangefinderSensor) 806 | { 807 | gzwarn << "[" << this->dataPtr->modelName << "] " 808 | << "ranfinder [" << rangefinderName 809 | << "] not found, skipping rangefinder support.\n" << "\n"; 810 | } 811 | else 812 | { 813 | gzwarn << "[" << this->dataPtr->modelName << "] " 814 | << " found " << " [" << rangefinderName << "].\n"; 815 | } 816 | } 817 | */ 818 | // Controller time control. 819 | this->dataPtr->lastControllerUpdateTime = 0; 820 | 821 | // Initialise ardupilot sockets 822 | if (!InitArduPilotSockets(_sdf)) 823 | { 824 | return; 825 | } 826 | 827 | // Missed update count before we declare arduPilotOnline status false 828 | this->dataPtr->connectionTimeoutMaxCount = 829 | _sdf->Get("connectionTimeoutMaxCount", 10).first; 830 | 831 | // Listen to the update event. This event is broadcast every simulation 832 | // iteration. 833 | this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin( 834 | std::bind(&ArduPilotPlugin::OnUpdate, this)); 835 | 836 | gzlog << "[" << this->dataPtr->modelName << "] " 837 | << "ArduPilot ready to fly. The force will be with you" << std::endl; 838 | } 839 | 840 | ///////////////////////////////////////////////// 841 | void ArduPilotPlugin::OnUpdate() 842 | { 843 | std::lock_guard lock(this->dataPtr->mutex); 844 | 845 | const gazebo::common::Time curTime = 846 | this->dataPtr->model->GetWorld()->SimTime(); 847 | 848 | // Update the control surfaces and publish the new state. 849 | if (curTime > this->dataPtr->lastControllerUpdateTime) 850 | { 851 | this->ReceiveMotorCommand(); 852 | if (this->dataPtr->arduPilotOnline) 853 | { 854 | this->ApplyMotorForces((curTime - 855 | this->dataPtr->lastControllerUpdateTime).Double()); 856 | this->SendState(); 857 | } 858 | } 859 | 860 | this->dataPtr->lastControllerUpdateTime = curTime; 861 | } 862 | 863 | ///////////////////////////////////////////////// 864 | void ArduPilotPlugin::ResetPIDs() 865 | { 866 | // Reset velocity PID for controls 867 | for (size_t i = 0; i < this->dataPtr->controls.size(); ++i) 868 | { 869 | this->dataPtr->controls[i].cmd = 0; 870 | // this->dataPtr->controls[i].pid.Reset(); 871 | } 872 | } 873 | 874 | ///////////////////////////////////////////////// 875 | bool ArduPilotPlugin::InitArduPilotSockets(sdf::ElementPtr _sdf) const 876 | { 877 | this->dataPtr->fdm_addr = 878 | _sdf->Get("fdm_addr", static_cast("127.0.0.1")).first; 879 | this->dataPtr->listen_addr = 880 | _sdf->Get("listen_addr", static_cast("127.0.0.1")).first; 881 | this->dataPtr->fdm_port_in = 882 | _sdf->Get("fdm_port_in", static_cast(9002)).first; 883 | this->dataPtr->fdm_port_out = 884 | _sdf->Get("fdm_port_out", static_cast(9003)).first; 885 | 886 | if (!this->dataPtr->socket_in.Bind(this->dataPtr->listen_addr.c_str(), 887 | this->dataPtr->fdm_port_in)) 888 | { 889 | gzerr << "[" << this->dataPtr->modelName << "] " 890 | << "failed to bind with " << this->dataPtr->listen_addr 891 | << ":" << this->dataPtr->fdm_port_in << " aborting plugin.\n"; 892 | return false; 893 | } 894 | 895 | if (!this->dataPtr->socket_out.Connect(this->dataPtr->fdm_addr.c_str(), 896 | this->dataPtr->fdm_port_out)) 897 | { 898 | gzerr << "[" << this->dataPtr->modelName << "] " 899 | << "failed to bind with " << this->dataPtr->fdm_addr 900 | << ":" << this->dataPtr->fdm_port_out << " aborting plugin.\n"; 901 | return false; 902 | } 903 | 904 | return true; 905 | } 906 | 907 | ///////////////////////////////////////////////// 908 | void ArduPilotPlugin::ApplyMotorForces(const double _dt) 909 | { 910 | // update velocity PID for controls and apply force to joint 911 | for (size_t i = 0; i < this->dataPtr->controls.size(); ++i) 912 | { 913 | if (this->dataPtr->controls[i].useForce) 914 | { 915 | if (this->dataPtr->controls[i].type == "VELOCITY") 916 | { 917 | const double velTarget = this->dataPtr->controls[i].cmd / 918 | this->dataPtr->controls[i].rotorVelocitySlowdownSim; 919 | const double vel = this->dataPtr->controls[i].joint->GetVelocity(0); 920 | const double error = vel - velTarget; 921 | const double force = this->dataPtr->controls[i].pid.Update(error, _dt); 922 | this->dataPtr->controls[i].joint->SetForce(0, force); 923 | } 924 | else if (this->dataPtr->controls[i].type == "POSITION") 925 | { 926 | const double posTarget = this->dataPtr->controls[i].cmd; 927 | const double pos = this->dataPtr->controls[i].joint->Position(); 928 | const double error = pos - posTarget; 929 | const double force = this->dataPtr->controls[i].pid.Update(error, _dt); 930 | this->dataPtr->controls[i].joint->SetForce(0, force); 931 | } 932 | else if (this->dataPtr->controls[i].type == "EFFORT") 933 | { 934 | const double force = this->dataPtr->controls[i].cmd; 935 | this->dataPtr->controls[i].joint->SetForce(0, force); 936 | } 937 | else 938 | { 939 | // do nothing 940 | } 941 | } 942 | else 943 | { 944 | if (this->dataPtr->controls[i].type == "VELOCITY") 945 | { 946 | this->dataPtr->controls[i].joint->SetVelocity(0, this->dataPtr->controls[i].cmd); 947 | } 948 | else if (this->dataPtr->controls[i].type == "POSITION") 949 | { 950 | this->dataPtr->controls[i].joint->SetPosition(0, this->dataPtr->controls[i].cmd); 951 | } 952 | else if (this->dataPtr->controls[i].type == "EFFORT") 953 | { 954 | const double force = this->dataPtr->controls[i].cmd; 955 | this->dataPtr->controls[i].joint->SetForce(0, force); 956 | } 957 | else 958 | { 959 | // do nothing 960 | } 961 | } 962 | } 963 | } 964 | 965 | ///////////////////////////////////////////////// 966 | void ArduPilotPlugin::ReceiveMotorCommand() 967 | { 968 | // Added detection for whether ArduPilot is online or not. 969 | // If ArduPilot is detected (receive of fdm packet from someone), 970 | // then socket receive wait time is increased from 1ms to 1 sec 971 | // to accomodate network jitter. 972 | // If ArduPilot is not detected, receive call blocks for 1ms 973 | // on each call. 974 | // Once ArduPilot presence is detected, it takes this many 975 | // missed receives before declaring the FCS offline. 976 | 977 | ServoPacket pkt; 978 | uint32_t waitMs; 979 | if (this->dataPtr->arduPilotOnline) 980 | { 981 | // increase timeout for receive once we detect a packet from 982 | // ArduPilot FCS. 983 | waitMs = 1000; 984 | } 985 | else 986 | { 987 | // Otherwise skip quickly and do not set control force. 988 | waitMs = 1; 989 | } 990 | ssize_t recvSize = 991 | this->dataPtr->socket_in.Recv(&pkt, sizeof(ServoPacket), waitMs); 992 | 993 | // Drain the socket in the case we're backed up 994 | int counter = 0; 995 | ServoPacket last_pkt; 996 | while (true) 997 | { 998 | // last_pkt = pkt; 999 | const ssize_t recvSize_last = 1000 | this->dataPtr->socket_in.Recv(&last_pkt, sizeof(ServoPacket), 0ul); 1001 | if (recvSize_last == -1) 1002 | { 1003 | break; 1004 | } 1005 | counter++; 1006 | pkt = last_pkt; 1007 | recvSize = recvSize_last; 1008 | } 1009 | if (counter > 0) 1010 | { 1011 | gzdbg << "[" << this->dataPtr->modelName << "] " 1012 | << "Drained n packets: " << counter << std::endl; 1013 | } 1014 | 1015 | if (recvSize == -1) 1016 | { 1017 | // didn't receive a packet 1018 | // gzdbg << "no packet\n"; 1019 | gazebo::common::Time::NSleep(100); 1020 | if (this->dataPtr->arduPilotOnline) 1021 | { 1022 | gzwarn << "[" << this->dataPtr->modelName << "] " 1023 | << "Broken ArduPilot connection, count [" 1024 | << this->dataPtr->connectionTimeoutCount 1025 | << "/" << this->dataPtr->connectionTimeoutMaxCount 1026 | << "]\n"; 1027 | if (++this->dataPtr->connectionTimeoutCount > 1028 | this->dataPtr->connectionTimeoutMaxCount) 1029 | { 1030 | this->dataPtr->connectionTimeoutCount = 0; 1031 | this->dataPtr->arduPilotOnline = false; 1032 | gzwarn << "[" << this->dataPtr->modelName << "] " 1033 | << "Broken ArduPilot connection, resetting motor control.\n"; 1034 | this->ResetPIDs(); 1035 | } 1036 | } 1037 | } 1038 | else 1039 | { 1040 | const ssize_t expectedPktSize = 1041 | sizeof(pkt.motorSpeed[0]) * this->dataPtr->controls.size(); 1042 | if (recvSize < expectedPktSize) 1043 | { 1044 | gzerr << "[" << this->dataPtr->modelName << "] " 1045 | << "got less than model needs. Got: " << recvSize 1046 | << "commands, expected size: " << expectedPktSize << "\n"; 1047 | } 1048 | const ssize_t recvChannels = recvSize / sizeof(pkt.motorSpeed[0]); 1049 | // for(unsigned int i = 0; i < recvChannels; ++i) 1050 | // { 1051 | // gzdbg << "servo_command [" << i << "]: " << pkt.motorSpeed[i] << "\n"; 1052 | // } 1053 | 1054 | if (!this->dataPtr->arduPilotOnline) 1055 | { 1056 | gzdbg << "[" << this->dataPtr->modelName << "] " 1057 | << "ArduPilot controller online detected.\n"; 1058 | // made connection, set some flags 1059 | this->dataPtr->connectionTimeoutCount = 0; 1060 | this->dataPtr->arduPilotOnline = true; 1061 | } 1062 | 1063 | // compute command based on requested motorSpeed 1064 | for (unsigned i = 0; i < this->dataPtr->controls.size(); ++i) 1065 | { 1066 | if (i < MAX_MOTORS) 1067 | { 1068 | if (this->dataPtr->controls[i].channel < recvChannels) 1069 | { 1070 | // bound incoming cmd between 0 and 1 1071 | const double cmd = ignition::math::clamp( 1072 | pkt.motorSpeed[this->dataPtr->controls[i].channel], 1073 | -1.0f, 1.0f); 1074 | this->dataPtr->controls[i].cmd = 1075 | this->dataPtr->controls[i].multiplier * 1076 | (this->dataPtr->controls[i].offset + cmd); 1077 | // gzdbg << "apply input chan[" << this->dataPtr->controls[i].channel 1078 | // << "] to control chan[" << i 1079 | // << "] with joint name [" 1080 | // << this->dataPtr->controls[i].jointName 1081 | // << "] raw cmd [" 1082 | // << pkt.motorSpeed[this->dataPtr->controls[i].channel] 1083 | // << "] adjusted cmd [" << this->dataPtr->controls[i].cmd 1084 | // << "].\n"; 1085 | } 1086 | else 1087 | { 1088 | gzerr << "[" << this->dataPtr->modelName << "] " 1089 | << "control[" << i << "] channel [" 1090 | << this->dataPtr->controls[i].channel 1091 | << "] is greater than incoming commands size[" 1092 | << recvChannels 1093 | << "], control not applied.\n"; 1094 | } 1095 | } 1096 | else 1097 | { 1098 | gzerr << "[" << this->dataPtr->modelName << "] " 1099 | << "too many motors, skipping [" << i 1100 | << " > " << MAX_MOTORS << "].\n"; 1101 | } 1102 | } 1103 | } 1104 | } 1105 | 1106 | ///////////////////////////////////////////////// 1107 | void ArduPilotPlugin::SendState() const 1108 | { 1109 | // send_fdm 1110 | fdmPacket pkt; 1111 | 1112 | pkt.timestamp = this->dataPtr->model->GetWorld()->SimTime().Double(); 1113 | 1114 | // asssumed that the imu orientation is: 1115 | // x forward 1116 | // y right 1117 | // z down 1118 | 1119 | // get linear acceleration in body frame 1120 | const ignition::math::Vector3d linearAccel = 1121 | this->dataPtr->imuSensor->LinearAcceleration(); 1122 | 1123 | // copy to pkt 1124 | pkt.imuLinearAccelerationXYZ[0] = linearAccel.X(); 1125 | pkt.imuLinearAccelerationXYZ[1] = linearAccel.Y(); 1126 | pkt.imuLinearAccelerationXYZ[2] = linearAccel.Z(); 1127 | // gzerr << "lin accel [" << linearAccel << "]\n"; 1128 | 1129 | // get angular velocity in body frame 1130 | const ignition::math::Vector3d angularVel = 1131 | this->dataPtr->imuSensor->AngularVelocity(); 1132 | 1133 | // copy to pkt 1134 | pkt.imuAngularVelocityRPY[0] = angularVel.X(); 1135 | pkt.imuAngularVelocityRPY[1] = angularVel.Y(); 1136 | pkt.imuAngularVelocityRPY[2] = angularVel.Z(); 1137 | 1138 | // get inertial pose and velocity 1139 | // position of the uav in world frame 1140 | // this position is used to calcualte bearing and distance 1141 | // from starting location, then use that to update gps position. 1142 | // The algorithm looks something like below (from ardupilot helper 1143 | // libraries): 1144 | // bearing = to_degrees(atan2(position.y, position.x)); 1145 | // distance = math.sqrt(self.position.x**2 + self.position.y**2) 1146 | // (self.latitude, self.longitude) = util.gps_newpos( 1147 | // self.home_latitude, self.home_longitude, bearing, distance) 1148 | // where xyz is in the NED directions. 1149 | // Gazebo world xyz is assumed to be N, -E, -D, so flip some stuff 1150 | // around. 1151 | // orientation of the uav in world NED frame - 1152 | // assuming the world NED frame has xyz mapped to NED, 1153 | // imuLink is NED - z down 1154 | 1155 | // model world pose brings us to model, 1156 | // which for example zephyr has -y-forward, x-left, z-up 1157 | // adding modelXYZToAirplaneXForwardZDown rotates 1158 | // from: model XYZ 1159 | // to: airplane x-forward, y-left, z-down 1160 | const ignition::math::Pose3d gazeboXYZToModelXForwardZDown = 1161 | this->modelXYZToAirplaneXForwardZDown + 1162 | this->dataPtr->model->WorldPose(); 1163 | 1164 | // get transform from world NED to Model frame 1165 | const ignition::math::Pose3d NEDToModelXForwardZUp = 1166 | gazeboXYZToModelXForwardZDown - this->gazeboXYZToNED; 1167 | 1168 | // gzerr << "ned to model [" << NEDToModelXForwardZUp << "]\n"; 1169 | 1170 | // N 1171 | pkt.positionXYZ[0] = NEDToModelXForwardZUp.Pos().X(); 1172 | 1173 | // E 1174 | pkt.positionXYZ[1] = NEDToModelXForwardZUp.Pos().Y(); 1175 | 1176 | // D 1177 | pkt.positionXYZ[2] = NEDToModelXForwardZUp.Pos().Z(); 1178 | 1179 | // imuOrientationQuat is the rotation from world NED frame 1180 | // to the uav frame. 1181 | pkt.imuOrientationQuat[0] = NEDToModelXForwardZUp.Rot().W(); 1182 | pkt.imuOrientationQuat[1] = NEDToModelXForwardZUp.Rot().X(); 1183 | pkt.imuOrientationQuat[2] = NEDToModelXForwardZUp.Rot().Y(); 1184 | pkt.imuOrientationQuat[3] = NEDToModelXForwardZUp.Rot().Z(); 1185 | 1186 | // gzdbg << "imu [" << gazeboXYZToModelXForwardZDown.rot.GetAsEuler() 1187 | // << "]\n"; 1188 | // gzdbg << "ned [" << this->gazeboXYZToNED.rot.GetAsEuler() << "]\n"; 1189 | // gzdbg << "rot [" << NEDToModelXForwardZUp.rot.GetAsEuler() << "]\n"; 1190 | 1191 | // Get NED velocity in body frame * 1192 | // or... 1193 | // Get model velocity in NED frame 1194 | const ignition::math::Vector3d velGazeboWorldFrame = 1195 | this->dataPtr->model->GetLink()->WorldLinearVel(); 1196 | const ignition::math::Vector3d velNEDFrame = 1197 | this->gazeboXYZToNED.Rot().RotateVectorReverse(velGazeboWorldFrame); 1198 | pkt.velocityXYZ[0] = velNEDFrame.X(); 1199 | pkt.velocityXYZ[1] = velNEDFrame.Y(); 1200 | pkt.velocityXYZ[2] = velNEDFrame.Z(); 1201 | /* NOT MERGED IN MASTER YET 1202 | if (!this->dataPtr->gpsSensor) 1203 | { 1204 | 1205 | } 1206 | else { 1207 | pkt.longitude = this->dataPtr->gpsSensor->Longitude().Degree(); 1208 | pkt.latitude = this->dataPtr->gpsSensor->Latitude().Degree(); 1209 | pkt.altitude = this->dataPtr->gpsSensor->Altitude(); 1210 | } 1211 | 1212 | // TODO : make generic enough to accept sonar/gpuray etc. too 1213 | if (!this->dataPtr->rangefinderSensor) 1214 | { 1215 | 1216 | } else { 1217 | // Rangefinder value can not be send as Inf to ardupilot 1218 | const double range = this->dataPtr->rangefinderSensor->Range(0); 1219 | pkt.rangefinder = std::isinf(range) ? 0.0 : range; 1220 | } 1221 | 1222 | // airspeed : wind = Vector3(environment.wind.x, environment.wind.y, environment.wind.z) 1223 | // pkt.airspeed = (pkt.velocity - wind).length() 1224 | */ 1225 | this->dataPtr->socket_out.Send(&pkt, sizeof(pkt)); 1226 | } 1227 | -------------------------------------------------------------------------------- /src/GimbalSmall2dPlugin.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 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 "gazebo/common/PID.hh" 21 | #include "gazebo/physics/physics.hh" 22 | #include "gazebo/transport/transport.hh" 23 | #include "GimbalSmall2dPlugin.hh" 24 | 25 | using namespace gazebo; 26 | using namespace std; 27 | 28 | GZ_REGISTER_MODEL_PLUGIN(GimbalSmall2dPlugin) 29 | 30 | /// \brief Private data class 31 | class gazebo::GimbalSmall2dPluginPrivate 32 | { 33 | /// \brief Callback when a command string is received. 34 | /// \param[in] _msg Mesage containing the command string 35 | public: void OnStringMsg(ConstGzStringPtr &_msg); 36 | 37 | /// \brief A list of event connections 38 | public: std::vector connections; 39 | 40 | /// \brief Subscriber to the gimbal command topic 41 | public: transport::SubscriberPtr sub; 42 | 43 | /// \brief Publisher to the gimbal status topic 44 | public: transport::PublisherPtr pub; 45 | 46 | /// \brief Parent model of this plugin 47 | public: physics::ModelPtr model; 48 | 49 | /// \brief Joint for tilting the gimbal 50 | public: physics::JointPtr tiltJoint; 51 | 52 | /// \brief Command that updates the gimbal tilt angle 53 | public: double command = IGN_PI_2; 54 | 55 | /// \brief Pointer to the transport node 56 | public: transport::NodePtr node; 57 | 58 | /// \brief PID controller for the gimbal 59 | public: common::PID pid; 60 | 61 | /// \brief Last update sim time 62 | public: common::Time lastUpdateTime; 63 | }; 64 | 65 | ///////////////////////////////////////////////// 66 | GimbalSmall2dPlugin::GimbalSmall2dPlugin() 67 | : dataPtr(new GimbalSmall2dPluginPrivate) 68 | { 69 | this->dataPtr->pid.Init(1, 0, 0, 0, 0, 1.0, -1.0); 70 | } 71 | 72 | ///////////////////////////////////////////////// 73 | void GimbalSmall2dPlugin::Load(physics::ModelPtr _model, 74 | sdf::ElementPtr _sdf) 75 | { 76 | this->dataPtr->model = _model; 77 | 78 | std::string jointName = "tilt_joint"; 79 | if (_sdf->HasElement("joint")) 80 | { 81 | jointName = _sdf->Get("joint"); 82 | } 83 | this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(jointName); 84 | if (!this->dataPtr->tiltJoint) 85 | { 86 | std::string scopedJointName = _model->GetScopedName() + "::" + jointName; 87 | gzwarn << "joint [" << jointName 88 | << "] not found, trying again with scoped joint name [" 89 | << scopedJointName << "]\n"; 90 | this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(scopedJointName); 91 | } 92 | if (!this->dataPtr->tiltJoint) 93 | { 94 | gzerr << "GimbalSmall2dPlugin::Load ERROR! Can't get joint '" 95 | << jointName << "' " << endl; 96 | } 97 | } 98 | 99 | ///////////////////////////////////////////////// 100 | void GimbalSmall2dPlugin::Init() 101 | { 102 | this->dataPtr->node = transport::NodePtr(new transport::Node()); 103 | this->dataPtr->node->Init(this->dataPtr->model->GetWorld()->GetName()); 104 | 105 | this->dataPtr->lastUpdateTime = 106 | this->dataPtr->model->GetWorld()->GetSimTime(); 107 | 108 | std::string topic = std::string("~/") + this->dataPtr->model->GetName() + 109 | "/gimbal_tilt_cmd"; 110 | this->dataPtr->sub = this->dataPtr->node->Subscribe(topic, 111 | &GimbalSmall2dPluginPrivate::OnStringMsg, this->dataPtr.get()); 112 | 113 | this->dataPtr->connections.push_back(event::Events::ConnectWorldUpdateBegin( 114 | std::bind(&GimbalSmall2dPlugin::OnUpdate, this))); 115 | 116 | topic = std::string("~/") + 117 | this->dataPtr->model->GetName() + "/gimbal_tilt_status"; 118 | 119 | this->dataPtr->pub = 120 | this->dataPtr->node->Advertise(topic); 121 | } 122 | 123 | ///////////////////////////////////////////////// 124 | void GimbalSmall2dPluginPrivate::OnStringMsg(ConstGzStringPtr &_msg) 125 | { 126 | this->command = atof(_msg->data().c_str()); 127 | } 128 | 129 | ///////////////////////////////////////////////// 130 | void GimbalSmall2dPlugin::OnUpdate() 131 | { 132 | if (!this->dataPtr->tiltJoint) 133 | return; 134 | 135 | double angle = this->dataPtr->tiltJoint->GetAngle(0).Radian(); 136 | 137 | common::Time time = this->dataPtr->model->GetWorld()->GetSimTime(); 138 | if (time < this->dataPtr->lastUpdateTime) 139 | { 140 | this->dataPtr->lastUpdateTime = time; 141 | return; 142 | } 143 | else if (time > this->dataPtr->lastUpdateTime) 144 | { 145 | double dt = (this->dataPtr->lastUpdateTime - time).Double(); 146 | double error = angle - this->dataPtr->command; 147 | double force = this->dataPtr->pid.Update(error, dt); 148 | this->dataPtr->tiltJoint->SetForce(0, force); 149 | this->dataPtr->lastUpdateTime = time; 150 | } 151 | 152 | static int i = 1000; 153 | if (++i > 100) 154 | { 155 | i = 0; 156 | std::stringstream ss; 157 | ss << angle; 158 | gazebo::msgs::GzString m; 159 | m.set_data(ss.str()); 160 | this->dataPtr->pub->Publish(m); 161 | } 162 | } 163 | -------------------------------------------------------------------------------- /worlds/iris_arducopter_cmac.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -5 0 1 0 0.2 0 7 | 8 | 9 | 10 | 11 | 12 | quick 13 | 100 14 | 1.0 15 | 16 | 17 | 0.0 18 | 0.2 19 | 0.1 20 | 0.0 21 | 22 | 23 | -1 24 | 25 | 26 | 0 0 -9.8 27 | 28 | model://sun 29 | 30 | 31 | 32 | model://ground_plane 33 | 34 | 35 | 36 | 37 |
-35.363261 149.165230
38 | 300 39 | satellite 40 | YOU_API_KEY 41 | true 42 | 0 0 0.2 0 0 -1.5708 43 | 44 | 45 | 46 | 47 | model://iris_with_ardupilot 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /worlds/iris_arducopter_runway.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -5 0 1 0 0.2 0 7 | 8 | 9 | 10 | 11 | 12 | quick 13 | 100 14 | 1.0 15 | 16 | 17 | 0.0 18 | 0.2 19 | 0.1 20 | 0.0 21 | 22 | 23 | -1 24 | 25 | 26 | 0 0 -9.8 27 | 28 | model://sun 29 | 30 | 31 | 32 | true 33 | 34 | 35 | 36 | 37 | 0 0 1 38 | 5000 5000 39 | 40 | 41 | 42 | 43 | 44 | 100 45 | 50 46 | 47 | 48 | 49 | 50 | 51 | 000 0 0.005 0 0 0 52 | false 53 | 54 | 55 | 0 0 1 56 | 1829 45 57 | 58 | 59 | 60 | 64 | 65 | 66 | 67 | 68 | 0 0 -0.1 0 0 0 69 | false 70 | 71 | 72 | 0 0 1 73 | 5000 5000 74 | 75 | 76 | 77 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | model://iris_with_ardupilot 90 | 91 | 92 | 93 | 94 | --------------------------------------------------------------------------------