├── AutoQuad ├── .catkin_workspace ├── CMakeLists.txt ├── build_isolated │ ├── .built_by │ └── catkin_make_isolated.cache ├── devel_isolated │ ├── .built_by │ ├── _setup_util.py │ ├── env.sh │ ├── setup.bash │ ├── setup.sh │ └── setup.zsh ├── launch │ ├── 788everything.launch │ ├── cameras.launch │ ├── everything.launch │ └── img_rect.launch ├── package.xml └── src │ ├── DstarLite │ ├── Dstar.cpp │ ├── graph.h │ ├── heap.cpp │ ├── heap.h │ └── sourceCode_DstarLite.cpp │ ├── control │ ├── control.py │ └── position_control.py │ ├── odometry │ └── odometry.py │ ├── planning │ ├── planner.py │ └── target_planner.py │ ├── px4Flow │ ├── px4.py │ ├── px4Flow.py │ └── px4Flow.pyc │ ├── serialComm │ ├── serialComm.py │ └── serial_testing.py │ ├── servo │ ├── servo.py │ └── servotesting.py │ └── tfmini │ ├── tfmini.py │ └── tfmini_test.py └── CMakeLists.txt /AutoQuad/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /AutoQuad/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(AutoQuad) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | 17 | catkin_package() 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # std_msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES AutoQuad 110 | # CATKIN_DEPENDS roscpp rospy std_msgs 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | add_executable(Dstar src/DstarLite/Dstar.cpp) 118 | target_link_libraries(Dstar ${catkin_LIBRARIES}) 119 | add_dependencies(Dstar beginner_tutorials_generate_messages_cpp) 120 | #target_link_libraries(Dstar ${catkin_LIBRARIES}) 121 | set_target_properties(Dstar PROPERTIES LINKER_LANGUAGE CXX) 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | include_directories( 126 | # include 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | # add_library(${PROJECT_NAME} 132 | # src/${PROJECT_NAME}/AutoQuad.cpp 133 | # ) 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Declare a C++ executable 141 | ## With catkin_make all packages are built within a single CMake context 142 | ## The recommended prefix ensures that target names across packages don't collide 143 | # add_executable(${PROJECT_NAME}_node src/AutoQuad_node.cpp) 144 | 145 | ## Rename C++ executable without prefix 146 | ## The above recommended prefix causes long target names, the following renames the 147 | ## target back to the shorter version for ease of user use 148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 150 | 151 | ## Add cmake target dependencies of the executable 152 | ## same as for the library above 153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Specify libraries to link a library or executable target against 156 | # target_link_libraries(${PROJECT_NAME}_node 157 | # ${catkin_LIBRARIES} 158 | # ) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 176 | # install(TARGETS ${PROJECT_NAME}_node 177 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark libraries for installation 181 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 182 | # install(TARGETS ${PROJECT_NAME} 183 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 184 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 185 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 186 | # ) 187 | 188 | ## Mark cpp header files for installation 189 | # install(DIRECTORY include/${PROJECT_NAME}/ 190 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 191 | # FILES_MATCHING PATTERN "*.h" 192 | # PATTERN ".svn" EXCLUDE 193 | # ) 194 | 195 | ## Mark other files for installation (e.g. launch and bag files, etc.) 196 | # install(FILES 197 | # # myfile1 198 | # # myfile2 199 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 200 | # ) 201 | 202 | ############# 203 | ## Testing ## 204 | ############# 205 | 206 | ## Add gtest based cpp test target and link libraries 207 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_AutoQuad.cpp) 208 | # if(TARGET ${PROJECT_NAME}-test) 209 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 210 | # endif() 211 | 212 | ## Add folders to be run by python nosetests 213 | # catkin_add_nosetests(test) 214 | -------------------------------------------------------------------------------- /AutoQuad/build_isolated/.built_by: -------------------------------------------------------------------------------- 1 | catkin_make_isolated -------------------------------------------------------------------------------- /AutoQuad/build_isolated/catkin_make_isolated.cache: -------------------------------------------------------------------------------- 1 | 2 | -G Unix Makefiles -DCATKIN_DEVEL_PREFIX=/home/ubuntu/catkin_ws/src/AutoQuad/devel_isolated -DCMAKE_INSTALL_PREFIX=/home/ubuntu/catkin_ws/src/AutoQuad/install_isolated -------------------------------------------------------------------------------- /AutoQuad/devel_isolated/.built_by: -------------------------------------------------------------------------------- 1 | catkin_make_isolated -------------------------------------------------------------------------------- /AutoQuad/devel_isolated/_setup_util.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD License) 5 | # 6 | # Copyright (c) 2012, Willow Garage, Inc. 7 | # All rights reserved. 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions 11 | # are met: 12 | # 13 | # * Redistributions of source code must retain the above copyright 14 | # notice, this list of conditions and the following disclaimer. 15 | # * Redistributions in binary form must reproduce the above 16 | # copyright notice, this list of conditions and the following 17 | # disclaimer in the documentation and/or other materials provided 18 | # with the distribution. 19 | # * Neither the name of Willow Garage, Inc. nor the names of its 20 | # contributors may be used to endorse or promote products derived 21 | # from this software without specific prior written permission. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | # POSSIBILITY OF SUCH DAMAGE. 35 | 36 | '''This file generates shell code for the setup.SHELL scripts to set environment variables''' 37 | 38 | from __future__ import print_function 39 | import argparse 40 | import copy 41 | import errno 42 | import os 43 | import platform 44 | import sys 45 | 46 | CATKIN_MARKER_FILE = '.catkin' 47 | 48 | system = platform.system() 49 | IS_DARWIN = (system == 'Darwin') 50 | IS_WINDOWS = (system == 'Windows') 51 | 52 | PATH_TO_ADD_SUFFIX = ['bin'] 53 | if IS_WINDOWS: 54 | # while catkin recommends putting dll's into bin, 3rd party packages often put dll's into lib 55 | # since Windows finds dll's via the PATH variable, prepend it with path to lib 56 | PATH_TO_ADD_SUFFIX.extend(['lib']) 57 | 58 | # subfolder of workspace prepended to CMAKE_PREFIX_PATH 59 | ENV_VAR_SUBFOLDERS = { 60 | 'CMAKE_PREFIX_PATH': '', 61 | 'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': 'lib', 62 | 'PATH': PATH_TO_ADD_SUFFIX, 63 | 'PKG_CONFIG_PATH': os.path.join('lib', 'pkgconfig'), 64 | 'PYTHONPATH': 'lib/python2.7/dist-packages', 65 | } 66 | 67 | 68 | def rollback_env_variables(environ, env_var_subfolders): 69 | ''' 70 | Generate shell code to reset environment variables 71 | by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH. 72 | This does not cover modifications performed by environment hooks. 73 | ''' 74 | lines = [] 75 | unmodified_environ = copy.copy(environ) 76 | for key in sorted(env_var_subfolders.keys()): 77 | subfolders = env_var_subfolders[key] 78 | if not isinstance(subfolders, list): 79 | subfolders = [subfolders] 80 | value = _rollback_env_variable(unmodified_environ, key, subfolders) 81 | if value is not None: 82 | environ[key] = value 83 | lines.append(assignment(key, value)) 84 | if lines: 85 | lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH')) 86 | return lines 87 | 88 | 89 | def _rollback_env_variable(environ, name, subfolders): 90 | ''' 91 | For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder. 92 | 93 | :param subfolders: list of str '' or subfoldername that may start with '/' 94 | :returns: the updated value of the environment variable. 95 | ''' 96 | value = environ[name] if name in environ else '' 97 | env_paths = [path for path in value.split(os.pathsep) if path] 98 | value_modified = False 99 | for subfolder in subfolders: 100 | if subfolder: 101 | if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)): 102 | subfolder = subfolder[1:] 103 | if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)): 104 | subfolder = subfolder[:-1] 105 | for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True): 106 | path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path 107 | path_to_remove = None 108 | for env_path in env_paths: 109 | env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path 110 | if env_path_clean == path_to_find: 111 | path_to_remove = env_path 112 | break 113 | if path_to_remove: 114 | env_paths.remove(path_to_remove) 115 | value_modified = True 116 | new_value = os.pathsep.join(env_paths) 117 | return new_value if value_modified else None 118 | 119 | 120 | def _get_workspaces(environ, include_fuerte=False, include_non_existing=False): 121 | ''' 122 | Based on CMAKE_PREFIX_PATH return all catkin workspaces. 123 | 124 | :param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool`` 125 | ''' 126 | # get all cmake prefix paths 127 | env_name = 'CMAKE_PREFIX_PATH' 128 | value = environ[env_name] if env_name in environ else '' 129 | paths = [path for path in value.split(os.pathsep) if path] 130 | # remove non-workspace paths 131 | workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))] 132 | return workspaces 133 | 134 | 135 | def prepend_env_variables(environ, env_var_subfolders, workspaces): 136 | ''' 137 | Generate shell code to prepend environment variables 138 | for the all workspaces. 139 | ''' 140 | lines = [] 141 | lines.append(comment('prepend folders of workspaces to environment variables')) 142 | 143 | paths = [path for path in workspaces.split(os.pathsep) if path] 144 | 145 | prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '') 146 | lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix)) 147 | 148 | for key in sorted([key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH']): 149 | subfolder = env_var_subfolders[key] 150 | prefix = _prefix_env_variable(environ, key, paths, subfolder) 151 | lines.append(prepend(environ, key, prefix)) 152 | return lines 153 | 154 | 155 | def _prefix_env_variable(environ, name, paths, subfolders): 156 | ''' 157 | Return the prefix to prepend to the environment variable NAME, adding any path in NEW_PATHS_STR without creating duplicate or empty items. 158 | ''' 159 | value = environ[name] if name in environ else '' 160 | environ_paths = [path for path in value.split(os.pathsep) if path] 161 | checked_paths = [] 162 | for path in paths: 163 | if not isinstance(subfolders, list): 164 | subfolders = [subfolders] 165 | for subfolder in subfolders: 166 | path_tmp = path 167 | if subfolder: 168 | path_tmp = os.path.join(path_tmp, subfolder) 169 | # skip nonexistent paths 170 | if not os.path.exists(path_tmp): 171 | continue 172 | # exclude any path already in env and any path we already added 173 | if path_tmp not in environ_paths and path_tmp not in checked_paths: 174 | checked_paths.append(path_tmp) 175 | prefix_str = os.pathsep.join(checked_paths) 176 | if prefix_str != '' and environ_paths: 177 | prefix_str += os.pathsep 178 | return prefix_str 179 | 180 | 181 | def assignment(key, value): 182 | if not IS_WINDOWS: 183 | return 'export %s="%s"' % (key, value) 184 | else: 185 | return 'set %s=%s' % (key, value) 186 | 187 | 188 | def comment(msg): 189 | if not IS_WINDOWS: 190 | return '# %s' % msg 191 | else: 192 | return 'REM %s' % msg 193 | 194 | 195 | def prepend(environ, key, prefix): 196 | if key not in environ or not environ[key]: 197 | return assignment(key, prefix) 198 | if not IS_WINDOWS: 199 | return 'export %s="%s$%s"' % (key, prefix, key) 200 | else: 201 | return 'set %s=%s%%%s%%' % (key, prefix, key) 202 | 203 | 204 | def find_env_hooks(environ, cmake_prefix_path): 205 | ''' 206 | Generate shell code with found environment hooks 207 | for the all workspaces. 208 | ''' 209 | lines = [] 210 | lines.append(comment('found environment hooks in workspaces')) 211 | 212 | generic_env_hooks = [] 213 | generic_env_hooks_workspace = [] 214 | specific_env_hooks = [] 215 | specific_env_hooks_workspace = [] 216 | generic_env_hooks_by_filename = {} 217 | specific_env_hooks_by_filename = {} 218 | generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh' 219 | specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None 220 | # remove non-workspace paths 221 | workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))] 222 | for workspace in reversed(workspaces): 223 | env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d') 224 | if os.path.isdir(env_hook_dir): 225 | for filename in sorted(os.listdir(env_hook_dir)): 226 | if filename.endswith('.%s' % generic_env_hook_ext): 227 | # remove previous env hook with same name if present 228 | if filename in generic_env_hooks_by_filename: 229 | i = generic_env_hooks.index(generic_env_hooks_by_filename[filename]) 230 | generic_env_hooks.pop(i) 231 | generic_env_hooks_workspace.pop(i) 232 | # append env hook 233 | generic_env_hooks.append(os.path.join(env_hook_dir, filename)) 234 | generic_env_hooks_workspace.append(workspace) 235 | generic_env_hooks_by_filename[filename] = generic_env_hooks[-1] 236 | elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext): 237 | # remove previous env hook with same name if present 238 | if filename in specific_env_hooks_by_filename: 239 | i = specific_env_hooks.index(specific_env_hooks_by_filename[filename]) 240 | specific_env_hooks.pop(i) 241 | specific_env_hooks_workspace.pop(i) 242 | # append env hook 243 | specific_env_hooks.append(os.path.join(env_hook_dir, filename)) 244 | specific_env_hooks_workspace.append(workspace) 245 | specific_env_hooks_by_filename[filename] = specific_env_hooks[-1] 246 | env_hooks = generic_env_hooks + specific_env_hooks 247 | env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace 248 | count = len(env_hooks) 249 | lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count)) 250 | for i in range(count): 251 | lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i])) 252 | lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i])) 253 | return lines 254 | 255 | 256 | def _parse_arguments(args=None): 257 | parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.') 258 | parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context') 259 | parser.add_argument('--local', action='store_true', help='Only consider this prefix path and ignore other prefix path in the environment') 260 | return parser.parse_known_args(args=args)[0] 261 | 262 | 263 | if __name__ == '__main__': 264 | try: 265 | try: 266 | args = _parse_arguments() 267 | except Exception as e: 268 | print(e, file=sys.stderr) 269 | sys.exit(1) 270 | 271 | if not args.local: 272 | # environment at generation time 273 | CMAKE_PREFIX_PATH = '/home/ubuntu/catkin_ws/devel_isolated/apriltag_ros;/home/ubuntu/catkin_ws/devel_isolated/AutoQuad;/home/ubuntu/catkin_ws/devel;/opt/ros/kinetic;/home/ubuntu/catkin_ws/devel_isolated/apriltag'.split(';') 274 | else: 275 | # don't consider any other prefix path than this one 276 | CMAKE_PREFIX_PATH = [] 277 | # prepend current workspace if not already part of CPP 278 | base_path = os.path.dirname(__file__) 279 | # CMAKE_PREFIX_PATH uses forward slash on all platforms, but __file__ is platform dependent 280 | # base_path on Windows contains backward slashes, need to be converted to forward slashes before comparison 281 | if os.path.sep != '/': 282 | base_path = base_path.replace(os.path.sep, '/') 283 | 284 | if base_path not in CMAKE_PREFIX_PATH: 285 | CMAKE_PREFIX_PATH.insert(0, base_path) 286 | CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH) 287 | 288 | environ = dict(os.environ) 289 | lines = [] 290 | if not args.extend: 291 | lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS) 292 | lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH) 293 | lines += find_env_hooks(environ, CMAKE_PREFIX_PATH) 294 | print('\n'.join(lines)) 295 | 296 | # need to explicitly flush the output 297 | sys.stdout.flush() 298 | except IOError as e: 299 | # and catch potential "broken pipe" if stdout is not writable 300 | # which can happen when piping the output to a file but the disk is full 301 | if e.errno == errno.EPIPE: 302 | print(e, file=sys.stderr) 303 | sys.exit(2) 304 | raise 305 | 306 | sys.exit(0) 307 | -------------------------------------------------------------------------------- /AutoQuad/devel_isolated/env.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/templates/env.sh.in 3 | 4 | if [ $# -eq 0 ] ; then 5 | /bin/echo "Usage: env.sh COMMANDS" 6 | /bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually." 7 | exit 1 8 | fi 9 | 10 | # ensure to not use different shell type which was set before 11 | CATKIN_SHELL=sh 12 | 13 | # source setup.sh from same directory as this file 14 | _CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd) 15 | . "$_CATKIN_SETUP_DIR/setup.sh" 16 | exec "$@" 17 | -------------------------------------------------------------------------------- /AutoQuad/devel_isolated/setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # generated from catkin/cmake/templates/setup.bash.in 3 | 4 | CATKIN_SHELL=bash 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd) 8 | . "$_CATKIN_SETUP_DIR/setup.sh" 9 | -------------------------------------------------------------------------------- /AutoQuad/devel_isolated/setup.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from catkin/cmake/template/setup.sh.in 3 | 4 | # Sets various environment variables and sources additional environment hooks. 5 | # It tries it's best to undo changes from a previously sourced setup file before. 6 | # Supported command line options: 7 | # --extend: skips the undoing of changes from a previously sourced setup file 8 | # --local: only considers this workspace but not the chained ones 9 | # In plain sh shell which doesn't support arguments for sourced scripts you can 10 | # set the environment variable `CATKIN_SETUP_UTIL_ARGS=--extend/--local` instead. 11 | 12 | # since this file is sourced either use the provided _CATKIN_SETUP_DIR 13 | # or fall back to the destination set at configure time 14 | : ${_CATKIN_SETUP_DIR:=/home/ubuntu/catkin_ws/src/AutoQuad/devel_isolated} 15 | _SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py" 16 | unset _CATKIN_SETUP_DIR 17 | 18 | if [ ! -f "$_SETUP_UTIL" ]; then 19 | echo "Missing Python script: $_SETUP_UTIL" 20 | return 22 21 | fi 22 | 23 | # detect if running on Darwin platform 24 | _UNAME=`uname -s` 25 | _IS_DARWIN=0 26 | if [ "$_UNAME" = "Darwin" ]; then 27 | _IS_DARWIN=1 28 | fi 29 | unset _UNAME 30 | 31 | # make sure to export all environment variables 32 | export CMAKE_PREFIX_PATH 33 | if [ $_IS_DARWIN -eq 0 ]; then 34 | export LD_LIBRARY_PATH 35 | else 36 | export DYLD_LIBRARY_PATH 37 | fi 38 | unset _IS_DARWIN 39 | export PATH 40 | export PKG_CONFIG_PATH 41 | export PYTHONPATH 42 | 43 | # remember type of shell if not already set 44 | if [ -z "$CATKIN_SHELL" ]; then 45 | CATKIN_SHELL=sh 46 | fi 47 | 48 | # invoke Python script to generate necessary exports of environment variables 49 | # use TMPDIR if it exists, otherwise fall back to /tmp 50 | if [ -d "${TMPDIR:-}" ]; then 51 | _TMPDIR="${TMPDIR}" 52 | else 53 | _TMPDIR=/tmp 54 | fi 55 | _SETUP_TMP=`mktemp "${_TMPDIR}/setup.sh.XXXXXXXXXX"` 56 | unset _TMPDIR 57 | if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then 58 | echo "Could not create temporary file: $_SETUP_TMP" 59 | return 1 60 | fi 61 | CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ ${CATKIN_SETUP_UTIL_ARGS:-} >> "$_SETUP_TMP" 62 | _RC=$? 63 | if [ $_RC -ne 0 ]; then 64 | if [ $_RC -eq 2 ]; then 65 | echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?" 66 | else 67 | echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC" 68 | fi 69 | unset _RC 70 | unset _SETUP_UTIL 71 | rm -f "$_SETUP_TMP" 72 | unset _SETUP_TMP 73 | return 1 74 | fi 75 | unset _RC 76 | unset _SETUP_UTIL 77 | . "$_SETUP_TMP" 78 | rm -f "$_SETUP_TMP" 79 | unset _SETUP_TMP 80 | 81 | # source all environment hooks 82 | _i=0 83 | while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do 84 | eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i 85 | unset _CATKIN_ENVIRONMENT_HOOKS_$_i 86 | eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE 87 | unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE 88 | # set workspace for environment hook 89 | CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace 90 | . "$_envfile" 91 | unset CATKIN_ENV_HOOK_WORKSPACE 92 | _i=$((_i + 1)) 93 | done 94 | unset _i 95 | 96 | unset _CATKIN_ENVIRONMENT_HOOKS_COUNT 97 | -------------------------------------------------------------------------------- /AutoQuad/devel_isolated/setup.zsh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | # generated from catkin/cmake/templates/setup.zsh.in 3 | 4 | CATKIN_SHELL=zsh 5 | 6 | # source setup.sh from same directory as this file 7 | _CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd) 8 | emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh"' 9 | -------------------------------------------------------------------------------- /AutoQuad/launch/788everything.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 13 | 14 | -------------------------------------------------------------------------------- /AutoQuad/launch/cameras.launch: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 38 | 39 | 40 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /AutoQuad/launch/everything.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /AutoQuad/launch/img_rect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /AutoQuad/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | AutoQuad 4 | 0.0.0 5 | The AutoQuad package 6 | 7 | 8 | 9 | 10 | nickrehm 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /AutoQuad/src/DstarLite/Dstar.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "std_msgs/Float64.h" 4 | #include "std_msgs/Int32.h" 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | using namespace std; 17 | 18 | #include "heap.h" 19 | #include "heap.cpp" 20 | #include "graph.h" 21 | 22 | //////////////////////////////////////////////////////////////////////// 23 | 24 | //DECLARE GLOBAL VARIABLES 25 | int boobs = 0; 26 | int mission_planner = 1; 27 | float x_obs = 0.0; 28 | float y_obs = 0.0; 29 | float r_obs = 0.0; 30 | int NodeID = 0; 31 | 32 | //CALLBACK FUNCTIONS 33 | void testCallback(const std_msgs::Int32::ConstPtr& msg) { 34 | //int boobs; 35 | boobs = msg->data; 36 | //cout << "boobs"; 37 | //cout << boobs; 38 | } 39 | 40 | void mpCallback(const std_msgs::Int32::ConstPtr& msg) { 41 | mission_planner = msg->data; 42 | } 43 | 44 | void xobsCallback(const std_msgs::Float64::ConstPtr& msg) { 45 | x_obs = msg->data; 46 | } 47 | 48 | void yobsCallback(const std_msgs::Float64::ConstPtr& msg) { 49 | y_obs = msg->data; 50 | } 51 | 52 | void robsCallback(const std_msgs::Float64::ConstPtr& msg) { 53 | r_obs = msg->data; 54 | } 55 | 56 | void NodeIDCallback(const std_msgs::Int32::ConstPtr& msg) { 57 | NodeID = msg->data; 58 | } 59 | 60 | 61 | 62 | //REGULAR FUNCTIONS 63 | 64 | float rand_f() 65 | { 66 | return (float)rand() / (float)RAND_MAX; 67 | } 68 | 69 | bool saveBlankEdgeFile(const char* Filename, const char* NewFilename) { 70 | FILE * pFile; 71 | 72 | int numEdges; 73 | 74 | // open the edge file 75 | pFile = fopen(Filename, "r"); 76 | if (pFile==NULL) 77 | { 78 | printf("unable to open file %s\n", Filename); 79 | return false; 80 | } 81 | 82 | // read the number of edges 83 | if(fscanf(pFile, "%d\n", &numEdges) < 1) 84 | { 85 | printf("problem reading edge number from file\n"); 86 | return false; 87 | } 88 | 89 | int node1_arr [numEdges]; 90 | int node2_arr [numEdges]; 91 | float cost_arr [numEdges]; 92 | 93 | // now read the edges one at a time 94 | int startNodeID, endNodeID; 95 | float cost; 96 | for(int m = 0; m < numEdges; m++) 97 | { 98 | if(fscanf(pFile, "%d, %d, %f\n", &startNodeID, &endNodeID, &cost) < 2) 99 | { 100 | printf("problem reading the %d-th edge from file\n",m); 101 | return false; 102 | } 103 | 104 | node1_arr[m] = startNodeID; 105 | node2_arr[m] = endNodeID; 106 | cost_arr[m] = cost; 107 | } 108 | 109 | // close the node file 110 | fclose(pFile); 111 | 112 | /////////////////////////////// 113 | 114 | pFile = fopen(NewFilename,"w"); 115 | if(pFile == NULL) 116 | { 117 | return false; 118 | } 119 | 120 | //ADD THE MF HEADER!!!! 121 | fprintf(pFile, "%d\n", numEdges); 122 | 123 | for(int i = 0; i a2) { 139 | return a2; 140 | } 141 | else { 142 | return a1; 143 | } 144 | 145 | } 146 | 147 | 148 | 149 | float calculateKey1(Node* thisNode, Node* startNode, float km) { 150 | float key; 151 | float heuristic; 152 | 153 | heuristic = sqrt((thisNode->x - startNode->x)*(thisNode->x - startNode->x) + (thisNode->y - startNode->y)*(thisNode->y - startNode->y)); //distance to goal heuristic 154 | 155 | //heuristic = abs(thisNode->x - startNode->x) + abs(thisNode->y - startNode->y); 156 | 157 | key = smallest(thisNode->g, thisNode->rhs) + heuristic + km; 158 | thisNode->key = key; 159 | 160 | return key; 161 | } 162 | 163 | float calculateKey2(Node* thisNode) { 164 | float key; 165 | //float heuristic; 166 | 167 | //heuristic = sqrt((thisNode->x - startNode->x)*(thisNode->x - startNode->x) + (thisNode->y - startNode->y)*(thisNode->y - startNode->y)); //distance to goal heuristic 168 | 169 | //heuristic = abs(thisNode->x - startNode->x) + abs(thisNode->y - startNode->y); 170 | 171 | key = smallest(thisNode->g, thisNode->rhs); 172 | //thisNode->key = key; 173 | 174 | return key; 175 | } 176 | 177 | 178 | 179 | 180 | void updateNode(Node* thisNode, Node* startNode, Node* goalNode, Heap &H, float km) { 181 | double smallest_cost = LARGE; 182 | 183 | //thisNode->parentNode = NULL; //this removes node from search tree, so that it isn't re-added if edge-costs change 184 | 185 | if (thisNode->id != goalNode->id) { 186 | //thisNode->rhs = LARGE; //this isn't in the paper... 187 | for (int n = 0; n < thisNode->numOutgoingEdges; n++) { //sucessors 188 | Edge* thisEdge = thisNode->outgoingEdges[n]; //pointer to this edge 189 | Node* sucessor = thisEdge->endNode; //pointer to the node on the other end of this edge 190 | //thisNode->rhs = smallest(thisNode->rhs, sucessor->g + thisEdge->edgeCost); 191 | if (smallest_cost > sucessor->g + thisEdge->edgeCost) { //this is to set the parent node to store the solutions, not included in paper 192 | smallest_cost = sucessor->g + thisEdge->edgeCost; 193 | thisNode->rhs = sucessor->g + thisEdge->edgeCost; 194 | thisNode->parentNode = sucessor; 195 | } 196 | } 197 | 198 | smallest_cost = LARGE; 199 | for (int n = 0; n < thisNode->numIncommingEdges; n++) { //predecessors.....used to assign parents, not part of d* Lite 200 | Edge* thisEdge = thisNode->incommingEdges[n]; //pointer to this edge 201 | Node* predecessor = thisEdge->startNode; //pointer to the node on the other end of this edge 202 | if (smallest_cost > predecessor->g + thisEdge->edgeCost) { //this is to set the parent node to store the solutions, not included in paper 203 | smallest_cost = predecessor->g + thisEdge->edgeCost; 204 | 205 | //thisNode->parentNode = predecessor; 206 | //sucessor->parentNode = thisNode; //nope 207 | } 208 | } 209 | } 210 | 211 | if (thisNode->inHeap) { 212 | H.removeNodeFromHeap(thisNode); 213 | } 214 | 215 | if (thisNode->g != thisNode->rhs) { //inconsistent, add back to heap 216 | H.addToHeap(thisNode, calculateKey1(thisNode,startNode,km)); 217 | } 218 | } 219 | 220 | 221 | bool compareKeys(float k1, float k2, float kp1, float kp2) { 222 | bool first = false; 223 | bool second = false; 224 | 225 | double eps = 0;//0.0000000000000001; 226 | 227 | if (k1 < (kp1 + eps)) { 228 | first = true; 229 | } 230 | if (k1 == kp1 && k2 < (kp2 + eps)) { 231 | second = true; 232 | } 233 | 234 | if (first || second) { 235 | return true; 236 | } 237 | else { 238 | return false; 239 | } 240 | 241 | } 242 | 243 | 244 | void computeShortestPath(Graph &G, Heap &H, Node* startNode, Node* goalNode, int startNodeIndex, int goalNodeIndex, float km) { 245 | //cout << "computing shortest path..."; 246 | float eps = 0.01; 247 | int overflow = 0; 248 | 249 | //while ((H.topKey() < (calculateKey1(startNode,startNode,km) + eps)) || (startNode->rhs > startNode->g)) { 250 | //while (compareKeys(H.topKey(), calculateKey2(&G.nodes[H.topKeyIndex()]), calculateKey1(startNode,startNode,km)+eps, calculateKey2(startNode)+eps) || (startNode->rhs > startNode->g)) { 251 | while (true) { 252 | float kold [2] = {H.topKey(), calculateKey2(&G.nodes[H.topKeyIndex()])}; 253 | Node* thisNode = H.popHeap(); 254 | overflow = overflow + 1; 255 | 256 | //if (kold < calculateKey1(thisNode, startNode, km)) { 257 | if (compareKeys(kold[0],kold[1],calculateKey1(thisNode, startNode, km),calculateKey2(thisNode))) { 258 | H.addToHeap(thisNode, calculateKey1(thisNode,startNode,km)); 259 | } 260 | 261 | else if (thisNode->g > thisNode->rhs) { 262 | thisNode->g = thisNode->rhs; 263 | for(int n = 0; n < thisNode->numIncommingEdges; n++) { //predecessor 264 | Edge* thisEdge = thisNode->incommingEdges[n]; //pointer to this edge 265 | Node* predecessor = thisEdge->startNode; //pointer to the node on the other end of this edge 266 | updateNode(predecessor,startNode,goalNode,H,km); 267 | } 268 | } 269 | 270 | else { 271 | thisNode->g = LARGE; 272 | for(int n = 0; n < thisNode->numIncommingEdges; n++) { //predecessor 273 | Edge* thisEdge = thisNode->incommingEdges[n]; //pointer to this edge 274 | Node* predecessor = thisEdge->startNode; //pointer to the node on the other end of this edge 275 | updateNode(predecessor,startNode,goalNode,H,km); 276 | } 277 | updateNode(thisNode,startNode,goalNode,H,km); //thisNode is union with all predecessors 278 | } 279 | 280 | if (H.topHeap() == NULL ) { 281 | cout << "emptied heap in computeShortestPath :( "; 282 | break; 283 | } 284 | if (overflow > 100000) { 285 | break; 286 | } 287 | 288 | } 289 | } 290 | 291 | 292 | bool updateEdgeCostsandNodes(Graph &G, Heap &H, Node* startNode, Node* goalNode, float km, const char* nodeFilename, const char* edgeFilename, float x_obs, float y_obs, float r_obs) { 293 | FILE * pFile; 294 | int numNodes, numEdges; 295 | 296 | // open the node file 297 | pFile = fopen( nodeFilename , "r"); 298 | if(pFile==NULL) 299 | { 300 | printf("unable to open file %s\n", nodeFilename); 301 | return false; 302 | } 303 | 304 | // read the number of nodes 305 | if(fscanf(pFile, "%d\n", &numNodes) < 1) 306 | { 307 | printf("problem reading node number from file\n"); 308 | return false; 309 | } 310 | 311 | int id_arr [numNodes]; 312 | float x_arr [numNodes]; 313 | float y_arr [numNodes]; 314 | 315 | // now read the nodes one at a time 316 | int id; 317 | float x, y; 318 | for(int n = 0; n < numNodes; n++) 319 | { 320 | if(fscanf(pFile, "%d, %f, %f\n", &id, &x, &y) < 3) 321 | { 322 | printf("problem reading the %d-th node from file\n",n); 323 | return false; 324 | } 325 | //input the data 326 | id_arr[n] = id-1; //c++ 0-based indexing 327 | x_arr[n] = x; 328 | y_arr[n] = y; 329 | } 330 | 331 | // close the node file 332 | fclose(pFile); 333 | 334 | 335 | // open the edge file 336 | pFile = fopen(edgeFilename, "r"); 337 | if (pFile==NULL) 338 | { 339 | printf("unable to open file %s\n", edgeFilename); 340 | return false; 341 | } 342 | 343 | // read the number of edges 344 | if(fscanf(pFile, "%d\n", &numEdges) < 1) 345 | { 346 | printf("problem reading edge number from file\n"); 347 | return false; 348 | } 349 | 350 | int node1_arr [numEdges]; 351 | int node2_arr [numEdges]; 352 | float cost_arr [numEdges]; 353 | 354 | 355 | // now read the edges one at a time 356 | int startNodeID, endNodeID; 357 | float cost; 358 | for(int m = 0; m < numEdges; m++) 359 | { 360 | if(fscanf(pFile, "%d, %d, %f\n", &startNodeID, &endNodeID, &cost) < 2) 361 | { 362 | printf("problem reading the %d-th edge from file\n",m); 363 | return false; 364 | } 365 | 366 | node1_arr[m] = startNodeID; 367 | node2_arr[m] = endNodeID; 368 | cost_arr[m] = cost; 369 | } 370 | 371 | // close the node file 372 | fclose(pFile); 373 | 374 | ///////////////////////////////////////////////////////////////////////////////////// 375 | //NOW WE DO THE BIG BOY SHIT 376 | //nodes: id_arr, x_arr, y_arr 377 | //edges: node1_arr, node2_arr, cost_arr 378 | //obstacle to be injected: x_obs, y_obs, r_obs 379 | 380 | vector obstructed_node_ids; 381 | float dist_check; 382 | 383 | //loop through nodes arrays and store id if x and y coords are within the obstacle 384 | for (int i = 0; iid == obstructed_node_ids[j] || G.edges[i].endNode->id == obstructed_node_ids[j]) { 427 | G.edges[i].edgeCost = LARGE; 428 | //G.edges[i].endNode->key = LARGE; 429 | //G.edges[i].startNode->key = LARGE; 430 | G.edges[i].endNode->parentNode = NULL; 431 | G.edges[i].endNode->g = LARGE; 432 | G.edges[i].endNode->rhs = LARGE; 433 | G.edges[i].startNode->parentNode = NULL; 434 | G.edges[i].startNode->g = LARGE; 435 | G.edges[i].startNode->rhs = LARGE; 436 | 437 | updateNode(G.edges[i].endNode, startNode, goalNode, H, km); 438 | updateNode(G.edges[i].startNode, startNode, goalNode, H, km); 439 | } 440 | } 441 | 442 | /* 443 | for(int k = 0; k < G.numNodes; k++) { 444 | if (G.nodes[k].id == obstructed_node_ids[j]) { 445 | updateNode(&G.nodes[k], startNode, goalNode, H); //need to pass startNode and goalNode into this function... 446 | } 447 | } 448 | */ 449 | 450 | } 451 | 452 | //cout << "finished updating shit"; 453 | 454 | return true; 455 | } 456 | 457 | 458 | 459 | int main(int argc, char **argv) 460 | { 461 | //INITIALIZE NODE 462 | ros::init(argc, argv, "Dstar"); 463 | ros::NodeHandle n; 464 | 465 | //DECLARE TOPICS TO PUBLISH 466 | ros::Publisher mission_planner_pub = n.advertise("mission_planner", 1); 467 | 468 | //SUBSCRIBE TO NODES 469 | ros::Subscriber sub = n.subscribe("testInt",100, testCallback); 470 | ros::Subscriber sub1 = n.subscribe("mission_planner",100,mpCallback); 471 | ros::Subscriber sub2 = n.subscribe("/obstacle/x_obs",100,xobsCallback); 472 | ros::Subscriber sub3 = n.subscribe("/obstacle/y_obs",100,yobsCallback); 473 | ros::Subscriber sub4 = n.subscribe("/obstacle/r_obs",100,robsCallback); 474 | ros::Subscriber sub5 = n.subscribe("/control/NodeID",100,NodeIDCallback); 475 | 476 | ros::Rate loop_rate(30); 477 | 478 | 479 | //Inialize shit for D*Lite 480 | int replan_counter = 0; 481 | //we want to find a path that goes from here to here 482 | int startNodeIndex = 20; //subtract 1 from desired 483 | int goalNodeIndex = 3310; //subtract 1 from desired 484 | 485 | saveBlankEdgeFile("/home/ubuntu/.ros/edges_blank.txt", "/home/ubuntu/.ros/edges.txt"); //makes a copy of blank edge file that we will update with addition of obstacles later 486 | 487 | //create graph 488 | Graph G; 489 | G.readGraphFromFiles("/home/ubuntu/.ros/nodes_blank.txt", "/home/ubuntu/.ros/edges_blank.txt"); 490 | 491 | //set rhs and g values in G to default values 492 | for (int i = 0; i < G.numNodes; i++) { 493 | Node* thisNode = &G.nodes[i]; 494 | thisNode->g = LARGE; //'infinity' 495 | thisNode->rhs = LARGE; //'infinity' 496 | } 497 | 498 | float km = 0; //key modifier 499 | 500 | //initialize heap 501 | Heap H(100); // this is the heap (starts with space for 100 items 502 | // but will grow automatically as needed). 503 | 504 | //these are pointers to the start and end nodes 505 | Node* startNode = &G.nodes[startNodeIndex]; 506 | Node* lastNode = startNode; 507 | Node* goalNode = &G.nodes[goalNodeIndex]; 508 | goalNode->rhs = 0; 509 | 510 | H.addToHeap(goalNode, calculateKey1(goalNode,startNode,km)); 511 | printf("Start and end nodes for this flight are: "); 512 | cout << startNodeIndex; 513 | cout << endl; 514 | cout << goalNodeIndex; 515 | cout << endl; 516 | printf("PREPPED FOR FLIGHT. WAITING FOR MISSION_PLANNER=7 FOR MOTION PLANNING\n"); 517 | 518 | //THE MAIN SHOW 519 | while (ros::ok()) 520 | { 521 | 522 | //Compute initial solution 523 | if (mission_planner == 7) { 524 | //run the shit for first solution 525 | computeShortestPath(G,H,startNode,goalNode,startNodeIndex,goalNodeIndex,km); 526 | G.saveSearchTreeToFile("/home/ubuntu/.ros/search_tree1.txt"); 527 | G.savePathToFile("/home/ubuntu/.ros/output_path1.txt", startNode); 528 | 529 | //publish 2 to mission planner 530 | mission_planner = 2; 531 | std_msgs::Int32 pub_mission_planner; 532 | pub_mission_planner.data = mission_planner; 533 | mission_planner_pub.publish(pub_mission_planner); 534 | } 535 | 536 | 537 | //New obstacle detected 538 | if (mission_planner == 6) { 539 | //move the start node to the next position along the solution path 540 | Node* startNode = &G.nodes[NodeID]; 541 | Node* goalNode = &G.nodes[goalNodeIndex]; 542 | 543 | //update the edge costs to affected nodes 544 | float h = sqrt((lastNode->x - startNode->x)*(lastNode->x - startNode->x) + (lastNode->y - startNode->y)*(lastNode->y - startNode->y)); 545 | km = km + h; 546 | lastNode = startNode; 547 | updateEdgeCostsandNodes(G, H, startNode, goalNode, km, "/home/ubuntu/.ros/nodes_blank.txt", "/home/ubuntu/.ros/edges.txt", x_obs, y_obs, r_obs); //note: use the other edge file as this one gets modified 548 | 549 | //re-compute shortest path 550 | computeShortestPath(G,H,startNode,goalNode,startNodeIndex,goalNodeIndex,km); 551 | replan_counter = replan_counter + 1; 552 | 553 | if (replan_counter == 1) { 554 | G.saveSearchTreeToFile("/home/ubuntu/.ros/search_tree2.txt"); 555 | G.savePathToFile("/home/ubuntu/.ros/output_path2.txt", startNode); 556 | //publish 3 to mission planner 557 | mission_planner = 3; 558 | std_msgs::Int32 pub_mission_planner; 559 | pub_mission_planner.data = mission_planner; 560 | mission_planner_pub.publish(pub_mission_planner); 561 | } 562 | if (replan_counter == 2) { 563 | G.saveSearchTreeToFile("/home/ubuntu/.ros/search_tree3.txt"); 564 | G.savePathToFile("/home/ubuntu/.ros/output_path3.txt", startNode); 565 | //publish 4 to mission planner 566 | mission_planner = 4; 567 | std_msgs::Int32 pub_mission_planner; 568 | pub_mission_planner.data = mission_planner; 569 | mission_planner_pub.publish(pub_mission_planner); 570 | } 571 | if (replan_counter == 3) { 572 | G.saveSearchTreeToFile("/home/ubuntu/.ros/search_tree4.txt"); 573 | G.savePathToFile("/home/ubuntu/.ros/output_path4.txt", startNode); 574 | //publish 5 to mission planner 575 | mission_planner = 5; 576 | std_msgs::Int32 pub_mission_planner; 577 | pub_mission_planner.data = mission_planner; 578 | mission_planner_pub.publish(pub_mission_planner); 579 | } 580 | } 581 | 582 | 583 | 584 | 585 | //PUBLISH TO TOPICS 586 | //std_msgs::Int32 pub_mission_planner; 587 | //pub_mission_planner.data = mission_planner; 588 | //mission_planner_pub.publish(pub_mission_planner); 589 | 590 | 591 | //////////////////////////////////////////////////////////////////// 592 | 593 | ros::spinOnce(); 594 | loop_rate.sleep(); 595 | } 596 | 597 | return 0; 598 | } 599 | -------------------------------------------------------------------------------- /AutoQuad/src/DstarLite/graph.h: -------------------------------------------------------------------------------- 1 | /* Copyright Michael Otte, 2018 2 | * 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 8 | * 9 | * Basic random graph with nodes and edges 10 | * 11 | * Note: that this reads nodes from file with 1-based indexing and 12 | * switches node IDs to use 0-based indexing. In other words, 13 | * nodes from a file have their IDs reduced by 1 within this code. 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | 25 | struct Node; // NOTE: we'll define this in detail later, but we need it now 26 | 27 | 28 | // make the basic edge data stucture 29 | struct Edge 30 | { 31 | Node* startNode; // the edge starts at this node 32 | Node* endNode; // the edge goes to this node 33 | 34 | double edgeCost; // going along the edge cost this much 35 | }; 36 | 37 | 38 | // this is a basic graph node for a 2D graph 39 | struct Node 40 | { 41 | int id; // this is the id of the node 42 | // it is alos the position in the node array in the 43 | // graph data structure where this node is stored 44 | 45 | double x; // physical x location 46 | double y; // physical y location 47 | 48 | 49 | // NOTE, in an undirect graph outgoingEdges and incommingEdges are the same 50 | // while in a directed graph they are not 51 | 52 | int numOutgoingEdges; // the number of outgoing edges 53 | 54 | Edge** outgoingEdges; // these are edges to {neighbors accessible from this node} 55 | // outgoingEdges[j] is a pointer to the j-th one 56 | 57 | int numIncommingEdges; // the number of outgoing edges 58 | 59 | Edge** incommingEdges; // these are edges from {nodes which this node can be accessed} 60 | // incommingEdges[k] is a pointer the k-th one 61 | 62 | 63 | Node* parentNode; // used for graph search, this is a pointer to this node's parent 64 | // node in the search tree 65 | 66 | int status; // used for grah search, 0 = not visited yet 67 | // 1 = in open list (i.e., in heap) 68 | // 2 = in closed list 69 | 70 | // the following fields are assumed by the heap data structure that I've been using 71 | 72 | bool inHeap; // init to false and set to true iff the node is in the heap 73 | int heapIndex; // enables quick access to the node in the heap 74 | 75 | double costToStart; //added by NJR 2/12/21 76 | double costToGoal; //added by NJR 4/1/21 77 | double key; //added NJR 4/5/21 78 | double g; 79 | double rhs; 80 | 81 | }; 82 | 83 | 84 | 85 | 86 | // this is the graph data structure 87 | class Graph 88 | { 89 | public: 90 | int numNodes; // the number of nodes in the graph 91 | 92 | Node* nodes; // an array that contains all of the nodes 93 | // nodes[i] contains the node with id i 94 | 95 | int numEdges; // the number of edges in the graph 96 | 97 | Edge* edges; // an array that contains all of the edges 98 | // edge[j] contains the j-th edge 99 | 100 | 101 | // default constructor 102 | Graph() 103 | { 104 | printf("building a default graph\n"); 105 | numNodes = 0; 106 | nodes = NULL; 107 | numEdges = 0; 108 | edges = NULL; 109 | } 110 | 111 | // default destructor 112 | ~Graph() 113 | { 114 | printf("deleting a graph\n"); 115 | if(nodes != NULL) 116 | { 117 | for(int n = 0; n < numNodes; n++) 118 | { 119 | if(nodes[n].outgoingEdges != NULL) 120 | { 121 | free(nodes[n].outgoingEdges); 122 | } 123 | nodes[n].numOutgoingEdges = 0; 124 | 125 | if(nodes[n].incommingEdges != NULL) 126 | { 127 | free(nodes[n].incommingEdges); 128 | } 129 | nodes[n].numIncommingEdges = 0; 130 | } 131 | free(nodes); 132 | } 133 | numNodes = 0; 134 | nodes = NULL; 135 | 136 | if(edges != NULL) 137 | { 138 | free(edges); 139 | } 140 | numEdges = 0; 141 | edges = NULL; 142 | } 143 | 144 | // copy constructor 145 | // Graph(const Graph &H) 146 | 147 | 148 | // loads the graph from the files 149 | // (e.g., generated using generate_weighted_random_graph.m) 150 | // returns true on success 151 | bool readGraphFromFiles(const char* nodeFilename, const char* edgeFilename) 152 | { 153 | 154 | FILE * pFile; 155 | 156 | // open the node file 157 | pFile = fopen( nodeFilename , "r"); 158 | if(pFile==NULL) 159 | { 160 | printf("unable to open file %s\n", nodeFilename); 161 | return false; 162 | } 163 | 164 | // read the number of nodes 165 | if(fscanf(pFile, "%d\n", &numNodes) < 1) 166 | { 167 | printf("problem reading node number from file\n"); 168 | return false; 169 | } 170 | 171 | // allocate space for the nodes 172 | nodes = (Node*)calloc(numNodes, sizeof(Node)); 173 | 174 | // now read the nodes one at a time 175 | int id; 176 | float x, y; 177 | for(int n = 0; n < numNodes; n++) 178 | { 179 | if(fscanf(pFile, "%d, %f, %f\n", &id, &x, &y) < 3) 180 | { 181 | printf("problem reading the %d-th node from file\n",n); 182 | return false; 183 | } 184 | // allocate the node 185 | nodes[n].id = id-1; // NOTE: switches to c++ 0-based indexing!!! 186 | nodes[n].x = x; 187 | nodes[n].y = y; 188 | 189 | nodes[n].numOutgoingEdges = 0; // dummy value for now, reset lower down 190 | nodes[n].outgoingEdges = NULL; // dummy value for now, reset lower down 191 | nodes[n].numIncommingEdges = 0; // dummy value for now, reset lower down 192 | nodes[n].incommingEdges = NULL; // dummy value for now, reset lower down 193 | 194 | nodes[n].parentNode = NULL; // used for graph search 195 | nodes[n].status = 0; // everything starts not visited 196 | 197 | nodes[n].inHeap = false; // used by heap 198 | nodes[n].heapIndex = -1; // used by heap 199 | 200 | 201 | //printf("read node: %d, (%f, %f) from file\n", nodes[n].id, nodes[n].x, nodes[n].y); 202 | } 203 | 204 | // close the node file 205 | fclose(pFile); 206 | 207 | // open the edge file 208 | pFile = fopen(edgeFilename, "r"); 209 | if (pFile==NULL) 210 | { 211 | printf("unable to open file %s\n", edgeFilename); 212 | return false; 213 | } 214 | 215 | // read the number of edges 216 | if(fscanf(pFile, "%d\n", &numEdges) < 1) 217 | { 218 | printf("problem reading edge number from file\n"); 219 | return false; 220 | } 221 | 222 | // allocate space for the edges 223 | edges = (Edge*)calloc(numEdges, sizeof(Edge)); 224 | 225 | // now read the edges one at a time 226 | int startNodeID, endNodeID; 227 | float cost; 228 | for(int m = 0; m < numEdges; m++) 229 | { 230 | if(fscanf(pFile, "%d, %d, %f\n", &startNodeID, &endNodeID, &cost) < 2) 231 | { 232 | printf("problem reading the %d-th edge from file\n",m); 233 | return false; 234 | } 235 | // allocate the edge 236 | // NOTE: switches to c++ 0-based indexing!!! 237 | // also note that edges store pointers to nodes that they are the edge for 238 | edges[m].startNode = &nodes[startNodeID-1]; 239 | edges[m].endNode = &nodes[endNodeID-1]; 240 | 241 | // cost from file 242 | edges[m].edgeCost = cost; 243 | 244 | // uncomment to use euclidian distance as cost instead 245 | //double x1 = nodes[startNodeID-1].x; 246 | //double y1 = nodes[startNodeID-1].y; 247 | //double x2 = nodes[endNodeID-1].x; 248 | //double y2 = nodes[endNodeID-1].y; 249 | //sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)); 250 | 251 | //printf("read edge: (%d, %d) from file\n", edges[m].startNode->id, edges[m].endNode->id); 252 | } 253 | 254 | // close the node file 255 | fclose(pFile); 256 | 257 | 258 | // now we need to go through the nodes and set up the neighbor 259 | // lists for each node. In this code we are using edges arrays 260 | // for this part. we will use three passes, on the first 261 | // pass we figure out how many incomming and outgoing edges 262 | // each node has, next we allocte all required space, 263 | // finally, we actually set the edges up 264 | 265 | 266 | // we'll also init some temp variables to help us 267 | // remember how many edges have been set so far in the final pass 268 | int* outgoingEdgesCount = (int*)calloc(numNodes, sizeof(int)); 269 | int* incommingEdgeCount = (int*)calloc(numNodes, sizeof(int)); 270 | for(int n = 0; n < numNodes; n++) 271 | { 272 | outgoingEdgesCount[n] = 0; 273 | incommingEdgeCount[n] = 0; 274 | nodes[n].numOutgoingEdges = 0; // set this to 0 while we're at it 275 | nodes[n].numIncommingEdges = 0; // set this to 0 while we're at it 276 | } 277 | 278 | // first pass: go through all edges to see how many incomming 279 | // and outgoing edges each node has, and recored these values 280 | for(int m = 0; m < numEdges; m++) 281 | { 282 | outgoingEdgesCount[edges[m].startNode->id]++; 283 | incommingEdgeCount[edges[m].endNode->id]++; 284 | } 285 | 286 | // second: go through all nodes and allocate space for their 287 | // neighbor lists, 288 | for(int n = 0; n < numNodes; n++) 289 | { 290 | nodes[n].outgoingEdges = (Edge**)calloc(outgoingEdgesCount[n], sizeof(Edge*)); 291 | nodes[n].incommingEdges = (Edge**)calloc(incommingEdgeCount[n], sizeof(Edge*)); 292 | } 293 | 294 | // third: go through all the edges again and record pointers to them 295 | // in the neighbor lists 296 | for(int m = 0; m < numEdges; m++) 297 | { 298 | // use local pointers to the start and end nodes to make 299 | // things things easier to read 300 | Node* startNodePtr = edges[m].startNode; 301 | Node* endNodePtr = edges[m].endNode; 302 | 303 | //printf("start node: %d\n", startNodePtr->id); 304 | //printf("end node: %d\n", endNodePtr->id); 305 | 306 | startNodePtr->outgoingEdges[startNodePtr->numOutgoingEdges] = &edges[m]; 307 | endNodePtr->incommingEdges[endNodePtr->numIncommingEdges] = &edges[m]; 308 | 309 | startNodePtr->numOutgoingEdges++; 310 | endNodePtr->numIncommingEdges++; 311 | } 312 | 313 | // cleanup 314 | free(outgoingEdgesCount); 315 | free(incommingEdgeCount); 316 | 317 | 318 | printf("done reading graph from file\n"); 319 | return true; 320 | } 321 | 322 | 323 | // prints out the graph data on the command line 324 | void printGraph() 325 | { 326 | for(int n = 0; n < numNodes; n++) 327 | { 328 | printf("node, id: %d, location: (%f, %f)\n", nodes[n].id, nodes[n].x, nodes[n].y); 329 | printf(" %d outgoing edges:\n", nodes[n].numOutgoingEdges); 330 | for(int i = 0; i < nodes[n].numOutgoingEdges; i++) 331 | { 332 | printf(" from %d (%f, %f) to %d (%f, %f) at cost: %f\n", 333 | nodes[n].outgoingEdges[i]->startNode->id, 334 | nodes[n].outgoingEdges[i]->startNode->x, 335 | nodes[n].outgoingEdges[i]->startNode->y, 336 | nodes[n].outgoingEdges[i]->endNode->id, 337 | nodes[n].outgoingEdges[i]->endNode->x, 338 | nodes[n].outgoingEdges[i]->endNode->y, 339 | nodes[n].outgoingEdges[i]->edgeCost); 340 | } 341 | 342 | printf(" %d incomming edges:\n", nodes[n].numIncommingEdges); 343 | for(int i = 0; i < nodes[n].numIncommingEdges; i++) 344 | { 345 | printf(" from %d (%f, %f) to %d (%f, %f) at cost: %f\n", 346 | nodes[n].incommingEdges[i]->startNode->id, 347 | nodes[n].incommingEdges[i]->startNode->x, 348 | nodes[n].incommingEdges[i]->startNode->y, 349 | nodes[n].incommingEdges[i]->endNode->id, 350 | nodes[n].incommingEdges[i]->endNode->x, 351 | nodes[n].incommingEdges[i]->endNode->y, 352 | nodes[n].incommingEdges[i]->edgeCost); 353 | } 354 | } 355 | } 356 | 357 | 358 | 359 | // saves the path to a file, extracts it backward from the goal node 360 | // note that 1 is added to the indicies so that the result is compatible 361 | // with any 1-indexed input files that may have been used 362 | // returns true on success 363 | bool savePathToFile(const char* pathFile, Node* goalNode) 364 | { 365 | FILE * pFile = fopen(pathFile,"w"); 366 | if(pFile == NULL) 367 | { 368 | return false; 369 | } 370 | 371 | Node* thisNode = goalNode; 372 | while(thisNode != NULL) 373 | { 374 | // format is id, x, y 375 | // NOTE: incrimenting ids by 1 to convert to 1-based-indexing 376 | fprintf(pFile, "%d, %f, %f\n",thisNode->id+1, thisNode->x, thisNode->y); 377 | thisNode = thisNode->parentNode; 378 | } 379 | 380 | fclose(pFile); 381 | printf("saved path in %s\n", pathFile); 382 | 383 | return true; 384 | } 385 | 386 | 387 | // saves the search tree to a file 388 | // note that 1 is added to the indicies so that the result is compatible 389 | // with any 1-indexed input files that may have been used 390 | // returns true on success 391 | bool saveSearchTreeToFile(const char* searchTreeFile) 392 | { 393 | FILE * pFile = fopen(searchTreeFile,"w"); 394 | if(pFile == NULL) 395 | { 396 | return false; 397 | } 398 | 399 | for(int n = 0; n < numNodes; n++) 400 | { 401 | Node* thisNode = &nodes[n]; 402 | 403 | if(thisNode->parentNode != NULL) 404 | { 405 | //cout << thisNode->parentNode->id; 406 | //cout << " "; 407 | 408 | // format is id1, x1, y1, id2, x2, y2 409 | // NOTE: incrimenting ids by 1 to convert to 1-based-indexing 410 | fprintf(pFile, "%d, %f, %f, %d, %f, %f\n", 411 | thisNode->id+1, thisNode->x, thisNode->y, 412 | thisNode->parentNode->id+1, thisNode->parentNode->x, thisNode->parentNode->y); 413 | } 414 | } 415 | fclose(pFile); 416 | printf("saved search tree in %s\n", searchTreeFile); 417 | 418 | return true; 419 | } 420 | 421 | 422 | }; 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | -------------------------------------------------------------------------------- /AutoQuad/src/DstarLite/heap.cpp: -------------------------------------------------------------------------------- 1 | /* Michael Otte, University of Colorado, 9-22-08 2 | * 3 | Copyright 2008 and 2018, Michael Otte 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | * 11 | * 12 | * 13 | * basic binary heap implimentation 14 | * 15 | * NOTE: Modified in 2018 to make into a more general c++ class 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | 29 | // sets up the memory for the heap 30 | template 31 | void Heap::buildHeap(int heapCapacity) 32 | { 33 | 34 | heapCost = (double*)calloc(heapCapacity, sizeof(double)); 35 | heapNode = (T**)calloc(heapCapacity, sizeof(T*)); 36 | indexOfLast = -1; 37 | parentOfLast = -1; 38 | tempCost = LARGE; 39 | tempNode = NULL; 40 | 41 | capacity = heapCapacity; 42 | int i; 43 | for(i = 0; i < heapCapacity; i++) 44 | heapCost[i] = LARGE; 45 | } 46 | 47 | 48 | 49 | 50 | 51 | 52 | // compares a node n with its parent, and switches them if the parent's 53 | // cost is more than the node's cost. Repeats if a switch happens. 54 | // returns the index that the node ended up at 55 | template 56 | int Heap::bubbleUp(int n) 57 | { 58 | tempInd = (n-1)/2; 59 | while(n != 0 & heapCost[tempInd] > heapCost[n]) 60 | { 61 | // swap costs 62 | tempCost = heapCost[tempInd]; 63 | heapCost[tempInd] = heapCost[n]; 64 | heapCost[n] = tempCost; 65 | 66 | // swap graph node pointers 67 | tempNode = heapNode[tempInd]; 68 | heapNode[tempInd] = heapNode[n]; 69 | heapNode[n] = tempNode; 70 | 71 | // update graph node heap index values 72 | heapNode[tempInd]->heapIndex = tempInd; 73 | heapNode[n]->heapIndex = n; 74 | 75 | // get new node and parent indicies 76 | n = tempInd; 77 | tempInd = (n-1)/2; 78 | } 79 | return n; 80 | } 81 | 82 | // compares a node n with its children, and switches them if a child's cost 83 | // is less than the node's cost. Repeats if a switch happens. 84 | template 85 | void Heap::bubbleDown(int n) 86 | { 87 | // find child with smallest value 88 | if(heapCost[2*n+1] < heapCost[2*n+2]) 89 | tempInd = 2*n+1; 90 | else 91 | tempInd = 2*n+2; 92 | 93 | while(n <= parentOfLast & heapCost[tempInd] < heapCost[n]) 94 | { 95 | // swap costs 96 | tempCost = heapCost[tempInd]; 97 | heapCost[tempInd] = heapCost[n]; 98 | heapCost[n] = tempCost; 99 | 100 | // swap graph node pointers 101 | tempNode = heapNode[tempInd]; 102 | heapNode[tempInd] = heapNode[n]; 103 | heapNode[n] = tempNode; 104 | 105 | // update graph node heap index values 106 | heapNode[tempInd]->heapIndex = tempInd; 107 | heapNode[n]->heapIndex = n; 108 | 109 | // get new node and child indicies 110 | n = tempInd; 111 | if(heapCost[2*n+1] < heapCost[2*n+2]) 112 | tempInd = 2*n+1; 113 | else 114 | tempInd = 2*n+2; 115 | } 116 | } 117 | 118 | // add thisNode to the heap, with the key value 119 | template 120 | void Heap::addToHeap(T* thisNode, float keyValue) 121 | { 122 | if(indexOfLast == capacity-1) 123 | { 124 | increaseHeapSize(); 125 | } 126 | 127 | if(thisNode->inHeap == false) 128 | { 129 | indexOfLast++; 130 | parentOfLast = (indexOfLast-1)/2; 131 | heapNode[indexOfLast] = thisNode; 132 | heapCost[indexOfLast] = keyValue; 133 | thisNode->heapIndex = indexOfLast; 134 | bubbleUp(indexOfLast); 135 | thisNode->inHeap = true; 136 | } 137 | } 138 | 139 | // returns a pointer to the node that is on the top of the heap 140 | template 141 | T* Heap::topHeap() 142 | { 143 | if(indexOfLast >= 0) 144 | { 145 | return heapNode[0]; 146 | } 147 | return NULL; 148 | } 149 | 150 | 151 | template 152 | float Heap::topKey() //returns smallest key in heap 153 | { 154 | double min_cost = LARGE; 155 | /* 156 | if (indexOfLast == NULL) { //return infinity if heap is empty 157 | return LARGE; 158 | } 159 | */ 160 | for(int i = 0; i <= indexOfLast; i++) { 161 | if (heapCost[i] < min_cost) { 162 | min_cost = heapCost[i]; 163 | } 164 | } 165 | 166 | return min_cost; 167 | } 168 | 169 | template 170 | int Heap::topKeyIndex() //returns index of node with smallest key in heap 171 | { 172 | double min_cost = LARGE; 173 | int idx_mincost; 174 | /* 175 | if (indexOfLast == NULL) { //return infinity if heap is empty 176 | return LARGE; 177 | } 178 | */ 179 | for(int i = 0; i <= indexOfLast; i++) { 180 | if (heapCost[i] < min_cost) { 181 | min_cost = heapCost[i]; 182 | idx_mincost = heapNode[i]->id; 183 | } 184 | } 185 | 186 | return idx_mincost; 187 | } 188 | 189 | 190 | 191 | 192 | // removes the top valued node from the heap and returns a pointer to it 193 | template 194 | T* Heap::popHeap() 195 | { 196 | T* oldTopNode = heapNode[0]; 197 | heapNode[0] = heapNode[indexOfLast]; 198 | heapCost[0] = heapCost[indexOfLast]; 199 | heapNode[0]->heapIndex = 0; 200 | heapNode[indexOfLast] = NULL; 201 | heapCost[indexOfLast] = LARGE; 202 | indexOfLast--; 203 | parentOfLast = (indexOfLast-1)/2; 204 | bubbleDown(0); 205 | oldTopNode->inHeap = false; 206 | oldTopNode->heapIndex = -1; 207 | return oldTopNode; 208 | } 209 | 210 | 211 | // removes the particular node from the heap 212 | // (even if that node is internal to the heap) 213 | // and then rebalances the heap, returns a pointer 214 | // to the node that has been removed 215 | template 216 | T* Heap::removeNodeFromHeap(T* thisNode) 217 | { 218 | if(!thisNode->inHeap) 219 | { 220 | return NULL; 221 | } 222 | 223 | int ind = thisNode->heapIndex; 224 | 225 | heapNode[ind] = heapNode[indexOfLast]; 226 | heapCost[ind] = heapCost[indexOfLast]; 227 | heapNode[ind]->heapIndex = ind; 228 | 229 | heapNode[indexOfLast] = NULL; 230 | heapCost[indexOfLast] = LARGE; 231 | indexOfLast--; 232 | parentOfLast = (indexOfLast-1)/2; 233 | 234 | ind = bubbleUp(ind); 235 | bubbleDown(ind); 236 | 237 | thisNode->inHeap = false; 238 | thisNode->heapIndex = -1; 239 | return thisNode; 240 | } 241 | 242 | 243 | // updates the position of the particular node within the heap 244 | // to reflect the new key value 245 | // (even if that node is internal to the heap) 246 | // and then rebalances the heap) 247 | // NOTE: this will insert the node if it is not in the heap already 248 | template 249 | void Heap::updateNodeInHeap(T* thisNode, float keyValue) 250 | { 251 | // we'll just do the easy way 252 | if(thisNode->inHeap) 253 | { 254 | removeNodeFromHeap(thisNode); 255 | } 256 | addToHeap(thisNode, keyValue); 257 | 258 | 259 | } 260 | 261 | 262 | 263 | // prints the heap values on the command line 264 | // note this will break if T does not have field "id" 265 | template 266 | void Heap::printHeap() 267 | { 268 | printf("heap costs:\n"); 269 | 270 | int i,p; 271 | char ch[10]; 272 | for(i = 0, p = 0; i <= indexOfLast; i++) 273 | { 274 | printf("%f ", heapCost[i]); 275 | if(i == p) 276 | { 277 | printf("\n"); 278 | p = p+2^p; 279 | } 280 | } 281 | printf("\n\n"); 282 | 283 | 284 | printf("heap node ids:\n"); 285 | for(i = 0, p = 0; i <= indexOfLast; i++) 286 | { 287 | printf("(%d) ", heapNode[i]->id); 288 | if(i == p) 289 | { 290 | printf("\n"); 291 | p = p+2^p; 292 | } 293 | } 294 | printf("\n\n"); 295 | } 296 | 297 | 298 | // returns 1 if heap is good, 0 if bad, also prints a command line message 299 | template 300 | int Heap::checkHeap() 301 | { 302 | int i; 303 | for(i = 0; i <= indexOfLast; i++) 304 | { 305 | if(heapCost[i] < heapCost[(i-1)/2] || heapNode[i]->heapIndex != i) 306 | { 307 | printf("There is a problem with the heap \n"); 308 | getchar(); 309 | return false; 310 | } 311 | } 312 | printf("The heap is OK \n"); 313 | return true; 314 | } 315 | 316 | // cleans up and deletes the memory used by the heap 317 | // note this is called automatically be the destructor 318 | template 319 | void Heap::deleteHeap() 320 | { 321 | free(heapCost); 322 | heapCost = NULL; 323 | free(heapNode); 324 | heapNode = NULL; 325 | indexOfLast = -1; 326 | parentOfLast = -1; 327 | tempInd = -1; 328 | tempNode = NULL; 329 | tempCost = LARGE; 330 | } 331 | 332 | 333 | // increases the heap size by a factor of two 334 | // this should only be used for debugging as it will really slow down the code 335 | template 336 | void Heap::increaseHeapSize() 337 | { 338 | 339 | //printf("growing heap\n"); 340 | int oldCapacity = capacity; 341 | capacity = capacity * 2; 342 | if(capacity < 100) 343 | { 344 | capacity = 100; 345 | } 346 | 347 | double* newHeapCost = (double*)calloc(capacity, sizeof(double)); 348 | T** newHeapNode = (T**)calloc(capacity, sizeof(T*)); 349 | 350 | std::memcpy(newHeapCost, heapCost, oldCapacity*sizeof(double)); 351 | std::memcpy(newHeapNode, heapNode, oldCapacity*sizeof(T*)); 352 | 353 | free(heapCost); 354 | free(heapNode); 355 | 356 | heapCost = newHeapCost; 357 | heapNode = newHeapNode; 358 | 359 | //printf("heap can now hold up to %d items\n", capacity); 360 | } 361 | 362 | -------------------------------------------------------------------------------- /AutoQuad/src/DstarLite/heap.h: -------------------------------------------------------------------------------- 1 | /* Michael Otte, University of Colorado, 9-22-08 2 | * 3 | Copyright 2008 and 2018, Michael Otte 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | 7 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | 9 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 10 | 11 | 12 | * basic binary heap implimentation 13 | * 14 | * NOTE: Modified in 2018 to make into a more general c++ class 15 | * 16 | * NOTE: the data type of the elements stored in the heap 17 | * is assumed to have the following fields initialized as follows: 18 | * bool inHeap = false 19 | * int heapIndex = -1 20 | * 21 | * For sorting speed, the heap is stored in two parallel arrays, one with 22 | * cost, and the other containing the associated node pointers. 23 | */ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | // make things easier (LARGE really just needs to be any number 35 | // >> than any graph cost we might encounter, but this is the best) 36 | double LARGE = std::numeric_limits::infinity(); 37 | 38 | using namespace std; 39 | 40 | template 41 | class Heap 42 | { 43 | // private: 44 | public: 45 | double* heapCost; // cost values 46 | T** heapNode; // pointers to the corresponding map nodes 47 | // (or whatever is stored in the heap) 48 | 49 | int parentOfLast; // stores the index of the parent of the last node 50 | int tempInd; // used to help swap nodes 51 | double tempCost; // used to help swap nodes 52 | T* tempNode; // used to help swap nodes 53 | 54 | int capacity; // the number of things this heap can store without 55 | // being increased in size 56 | // public: 57 | 58 | int indexOfLast; // the index of the last node in the heap array 59 | 60 | // private: 61 | 62 | // sets up the memory for the heap 63 | // assumes a reserve size of heapCapacity 64 | void buildHeap(int heapCapacity); 65 | 66 | // compares a node n with its parent, and switches them if the parent's 67 | // cost is more than the node's cost. Repeats if a switch happens. 68 | // returns the index that the node ended up at 69 | int bubbleUp(int n); 70 | 71 | // compares a node n with its children, and switches them if a child's cost 72 | // is less than the node's cost. Repeats if a switch happens. 73 | void bubbleDown(int n); 74 | 75 | // cleans up and deletes the memory used by the heap 76 | void deleteHeap(); 77 | 78 | // increases the heap size by a factor of two 79 | void increaseHeapSize(); 80 | 81 | // public: 82 | 83 | 84 | // default constructor 85 | Heap() 86 | { 87 | printf("building a heap\n"); 88 | buildHeap(100); 89 | } 90 | 91 | // constructor that should be used most of the time 92 | Heap(int heapCapacity) 93 | { 94 | printf("building a heap with %d elements\n", heapCapacity); 95 | buildHeap(heapCapacity); 96 | } 97 | 98 | 99 | // destructor 100 | ~Heap() 101 | { 102 | printf("deleting a heap\n"); 103 | deleteHeap(); 104 | } 105 | 106 | 107 | // copy constructor 108 | // Heap(const Heap &H) 109 | 110 | 111 | 112 | 113 | // add thisNode to the heap 114 | void addToHeap(T* thisNode, float keyValue); 115 | 116 | // returns a pointer to the node that is on the top of the heap 117 | T* topHeap(); 118 | 119 | float topKey(); //returns lowest key of any node in the queue 120 | int topKeyIndex(); //returns index of lowest key of any node in the queue 121 | 122 | // removes the top valued node from the heap and returns a pointer to it 123 | T* popHeap(); 124 | 125 | 126 | // removes the particular node from the heap 127 | // (even if that node is internal to the heap) 128 | // and then rebalances the heap, returns a pointer 129 | // to the node that has been removed 130 | T* removeNodeFromHeap(T* thisNode); 131 | 132 | // updates the position of the particular node within the heap 133 | // to reflect the new key value 134 | // (even if that node is internal to the heap) 135 | // and then rebalances the heap 136 | // NOTE: this will insert the node if it is not in the heap already 137 | void updateNodeInHeap(T* thisNode, float keyValue); 138 | 139 | 140 | // prints the heap values on the command line 141 | void printHeap(); 142 | 143 | // returns 1 if heap is good, 0 if bad, also prints a command line message 144 | // this should only be used for debugging as it will really slow down the code 145 | int checkHeap(); 146 | 147 | }; 148 | 149 | 150 | 151 | 152 | -------------------------------------------------------------------------------- /AutoQuad/src/DstarLite/sourceCode_DstarLite.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2018, Michael Otte 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 5 | 6 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 7 | 8 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 9 | */ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | using namespace std; 22 | 23 | 24 | #include "heap.h" 25 | #include "heap.cpp" 26 | 27 | #include "graph.h" 28 | 29 | // returns random number between 0 and 1 30 | float rand_f() 31 | { 32 | return (float)rand() / (float)RAND_MAX; 33 | } 34 | 35 | bool saveBlankEdgeFile(const char* Filename, const char* NewFilename) { 36 | FILE * pFile; 37 | 38 | int numEdges; 39 | 40 | // open the edge file 41 | pFile = fopen(Filename, "r"); 42 | if (pFile==NULL) 43 | { 44 | printf("unable to open file %s\n", Filename); 45 | return false; 46 | } 47 | 48 | // read the number of edges 49 | if(fscanf(pFile, "%d\n", &numEdges) < 1) 50 | { 51 | printf("problem reading edge number from file\n"); 52 | return false; 53 | } 54 | 55 | int node1_arr [numEdges]; 56 | int node2_arr [numEdges]; 57 | float cost_arr [numEdges]; 58 | 59 | // now read the edges one at a time 60 | int startNodeID, endNodeID; 61 | float cost; 62 | for(int m = 0; m < numEdges; m++) 63 | { 64 | if(fscanf(pFile, "%d, %d, %f\n", &startNodeID, &endNodeID, &cost) < 2) 65 | { 66 | printf("problem reading the %d-th edge from file\n",m); 67 | return false; 68 | } 69 | 70 | node1_arr[m] = startNodeID; 71 | node2_arr[m] = endNodeID; 72 | cost_arr[m] = cost; 73 | } 74 | 75 | // close the node file 76 | fclose(pFile); 77 | 78 | /////////////////////////////// 79 | 80 | pFile = fopen(NewFilename,"w"); 81 | if(pFile == NULL) 82 | { 83 | return false; 84 | } 85 | 86 | //ADD THE MF HEADER!!!! 87 | fprintf(pFile, "%d\n", numEdges); 88 | 89 | for(int i = 0; i a2) { 105 | return a2; 106 | } 107 | else { 108 | return a1; 109 | } 110 | 111 | } 112 | 113 | 114 | 115 | float calculateKey1(Node* thisNode, Node* startNode, float km) { 116 | float key; 117 | float heuristic; 118 | 119 | heuristic = sqrt((thisNode->x - startNode->x)*(thisNode->x - startNode->x) + (thisNode->y - startNode->y)*(thisNode->y - startNode->y)); //distance to goal heuristic 120 | 121 | //heuristic = abs(thisNode->x - startNode->x) + abs(thisNode->y - startNode->y); 122 | 123 | key = smallest(thisNode->g, thisNode->rhs) + heuristic + km; 124 | thisNode->key = key; 125 | 126 | return key; 127 | } 128 | 129 | float calculateKey2(Node* thisNode) { 130 | float key; 131 | //float heuristic; 132 | 133 | //heuristic = sqrt((thisNode->x - startNode->x)*(thisNode->x - startNode->x) + (thisNode->y - startNode->y)*(thisNode->y - startNode->y)); //distance to goal heuristic 134 | 135 | //heuristic = abs(thisNode->x - startNode->x) + abs(thisNode->y - startNode->y); 136 | 137 | key = smallest(thisNode->g, thisNode->rhs); 138 | //thisNode->key = key; 139 | 140 | return key; 141 | } 142 | 143 | 144 | 145 | 146 | void updateNode(Node* thisNode, Node* startNode, Node* goalNode, Heap &H, float km) { 147 | double smallest_cost = LARGE; 148 | 149 | //thisNode->parentNode = NULL; //this removes node from search tree, so that it isn't re-added if edge-costs change 150 | 151 | if (thisNode->id != goalNode->id) { 152 | //thisNode->rhs = LARGE; //this isn't in the paper... 153 | for (int n = 0; n < thisNode->numOutgoingEdges; n++) { //sucessors 154 | Edge* thisEdge = thisNode->outgoingEdges[n]; //pointer to this edge 155 | Node* sucessor = thisEdge->endNode; //pointer to the node on the other end of this edge 156 | //thisNode->rhs = smallest(thisNode->rhs, sucessor->g + thisEdge->edgeCost); 157 | if (smallest_cost > sucessor->g + thisEdge->edgeCost) { //this is to set the parent node to store the solutions, not included in paper 158 | smallest_cost = sucessor->g + thisEdge->edgeCost; 159 | thisNode->rhs = sucessor->g + thisEdge->edgeCost; 160 | thisNode->parentNode = sucessor; 161 | } 162 | } 163 | 164 | smallest_cost = LARGE; 165 | for (int n = 0; n < thisNode->numIncommingEdges; n++) { //predecessors.....used to assign parents, not part of d* Lite 166 | Edge* thisEdge = thisNode->incommingEdges[n]; //pointer to this edge 167 | Node* predecessor = thisEdge->startNode; //pointer to the node on the other end of this edge 168 | if (smallest_cost > predecessor->g + thisEdge->edgeCost) { //this is to set the parent node to store the solutions, not included in paper 169 | smallest_cost = predecessor->g + thisEdge->edgeCost; 170 | 171 | //thisNode->parentNode = predecessor; 172 | //sucessor->parentNode = thisNode; //nope 173 | } 174 | } 175 | } 176 | 177 | if (thisNode->inHeap) { 178 | H.removeNodeFromHeap(thisNode); 179 | } 180 | 181 | if (thisNode->g != thisNode->rhs) { //inconsistent, add back to heap 182 | H.addToHeap(thisNode, calculateKey1(thisNode,startNode,km)); 183 | } 184 | } 185 | 186 | 187 | bool compareKeys(float k1, float k2, float kp1, float kp2) { 188 | bool first = false; 189 | bool second = false; 190 | 191 | double eps = 0;//0.0000000000000001; 192 | 193 | if (k1 < (kp1 + eps)) { 194 | first = true; 195 | } 196 | if (k1 == kp1 && k2 < (kp2 + eps)) { 197 | second = true; 198 | } 199 | 200 | if (first || second) { 201 | return true; 202 | } 203 | else { 204 | return false; 205 | } 206 | 207 | } 208 | 209 | 210 | void computeShortestPath(Graph &G, Heap &H, Node* startNode, Node* goalNode, int startNodeIndex, int goalNodeIndex, float km) { 211 | //cout << "computing shortest path..."; 212 | float eps = 0.01; 213 | int overflow = 0; 214 | 215 | //while ((H.topKey() < (calculateKey1(startNode,startNode,km) + eps)) || (startNode->rhs > startNode->g)) { 216 | //while (compareKeys(H.topKey(), calculateKey2(&G.nodes[H.topKeyIndex()]), calculateKey1(startNode,startNode,km)+eps, calculateKey2(startNode)+eps) || (startNode->rhs > startNode->g)) { 217 | while (true) { 218 | float kold [2] = {H.topKey(), calculateKey2(&G.nodes[H.topKeyIndex()])}; 219 | Node* thisNode = H.popHeap(); 220 | overflow = overflow + 1; 221 | 222 | //if (kold < calculateKey1(thisNode, startNode, km)) { 223 | if (compareKeys(kold[0],kold[1],calculateKey1(thisNode, startNode, km),calculateKey2(thisNode))) { 224 | H.addToHeap(thisNode, calculateKey1(thisNode,startNode,km)); 225 | } 226 | 227 | else if (thisNode->g > thisNode->rhs) { 228 | thisNode->g = thisNode->rhs; 229 | for(int n = 0; n < thisNode->numIncommingEdges; n++) { //predecessor 230 | Edge* thisEdge = thisNode->incommingEdges[n]; //pointer to this edge 231 | Node* predecessor = thisEdge->startNode; //pointer to the node on the other end of this edge 232 | updateNode(predecessor,startNode,goalNode,H,km); 233 | } 234 | } 235 | 236 | else { 237 | thisNode->g = LARGE; 238 | for(int n = 0; n < thisNode->numIncommingEdges; n++) { //predecessor 239 | Edge* thisEdge = thisNode->incommingEdges[n]; //pointer to this edge 240 | Node* predecessor = thisEdge->startNode; //pointer to the node on the other end of this edge 241 | updateNode(predecessor,startNode,goalNode,H,km); 242 | } 243 | updateNode(thisNode,startNode,goalNode,H,km); //thisNode is union with all predecessors 244 | } 245 | 246 | if (H.topHeap() == NULL ) { 247 | cout << "emptied heap in computeShortestPath :( "; 248 | break; 249 | } 250 | if (overflow > 100000) { 251 | break; 252 | } 253 | 254 | } 255 | } 256 | 257 | 258 | bool updateEdgeCostsandNodes(Graph &G, Heap &H, Node* startNode, Node* goalNode, float km, const char* nodeFilename, const char* edgeFilename, float x_obs, float y_obs, float r_obs) { 259 | FILE * pFile; 260 | int numNodes, numEdges; 261 | 262 | // open the node file 263 | pFile = fopen( nodeFilename , "r"); 264 | if(pFile==NULL) 265 | { 266 | printf("unable to open file %s\n", nodeFilename); 267 | return false; 268 | } 269 | 270 | // read the number of nodes 271 | if(fscanf(pFile, "%d\n", &numNodes) < 1) 272 | { 273 | printf("problem reading node number from file\n"); 274 | return false; 275 | } 276 | 277 | int id_arr [numNodes]; 278 | float x_arr [numNodes]; 279 | float y_arr [numNodes]; 280 | 281 | // now read the nodes one at a time 282 | int id; 283 | float x, y; 284 | for(int n = 0; n < numNodes; n++) 285 | { 286 | if(fscanf(pFile, "%d, %f, %f\n", &id, &x, &y) < 3) 287 | { 288 | printf("problem reading the %d-th node from file\n",n); 289 | return false; 290 | } 291 | //input the data 292 | id_arr[n] = id-1; //c++ 0-based indexing 293 | x_arr[n] = x; 294 | y_arr[n] = y; 295 | } 296 | 297 | // close the node file 298 | fclose(pFile); 299 | 300 | 301 | // open the edge file 302 | pFile = fopen(edgeFilename, "r"); 303 | if (pFile==NULL) 304 | { 305 | printf("unable to open file %s\n", edgeFilename); 306 | return false; 307 | } 308 | 309 | // read the number of edges 310 | if(fscanf(pFile, "%d\n", &numEdges) < 1) 311 | { 312 | printf("problem reading edge number from file\n"); 313 | return false; 314 | } 315 | 316 | int node1_arr [numEdges]; 317 | int node2_arr [numEdges]; 318 | float cost_arr [numEdges]; 319 | 320 | 321 | // now read the edges one at a time 322 | int startNodeID, endNodeID; 323 | float cost; 324 | for(int m = 0; m < numEdges; m++) 325 | { 326 | if(fscanf(pFile, "%d, %d, %f\n", &startNodeID, &endNodeID, &cost) < 2) 327 | { 328 | printf("problem reading the %d-th edge from file\n",m); 329 | return false; 330 | } 331 | 332 | node1_arr[m] = startNodeID; 333 | node2_arr[m] = endNodeID; 334 | cost_arr[m] = cost; 335 | } 336 | 337 | // close the node file 338 | fclose(pFile); 339 | 340 | ///////////////////////////////////////////////////////////////////////////////////// 341 | //NOW WE DO THE BIG BOY SHIT 342 | //nodes: id_arr, x_arr, y_arr 343 | //edges: node1_arr, node2_arr, cost_arr 344 | //obstacle to be injected: x_obs, y_obs, r_obs 345 | 346 | vector obstructed_node_ids; 347 | float dist_check; 348 | 349 | //loop through nodes arrays and store id if x and y coords are within the obstacle 350 | for (int i = 0; iid == obstructed_node_ids[j] || G.edges[i].endNode->id == obstructed_node_ids[j]) { 393 | G.edges[i].edgeCost = LARGE; 394 | //G.edges[i].endNode->key = LARGE; 395 | //G.edges[i].startNode->key = LARGE; 396 | G.edges[i].endNode->parentNode = NULL; 397 | G.edges[i].endNode->g = LARGE; 398 | G.edges[i].endNode->rhs = LARGE; 399 | G.edges[i].startNode->parentNode = NULL; 400 | G.edges[i].startNode->g = LARGE; 401 | G.edges[i].startNode->rhs = LARGE; 402 | 403 | updateNode(G.edges[i].endNode, startNode, goalNode, H, km); 404 | updateNode(G.edges[i].startNode, startNode, goalNode, H, km); 405 | } 406 | } 407 | 408 | /* 409 | for(int k = 0; k < G.numNodes; k++) { 410 | if (G.nodes[k].id == obstructed_node_ids[j]) { 411 | updateNode(&G.nodes[k], startNode, goalNode, H); //need to pass startNode and goalNode into this function... 412 | } 413 | } 414 | */ 415 | 416 | } 417 | 418 | //cout << "finished updating shit"; 419 | 420 | return true; 421 | 422 | } 423 | 424 | 425 | 426 | 427 | 428 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 429 | 430 | int main() 431 | { 432 | int replan_counter = 0; 433 | //we want to find a path that goes from here to here 434 | int startNodeIndex = 12; //subtract 1 from desired 435 | int goalNodeIndex = 952; //subtract 1 from desired 436 | 437 | saveBlankEdgeFile("edges_blank.txt", "edges.txt"); //makes a copy of blank edge file that we will update with addition of obstacles later 438 | 439 | //create graph 440 | Graph G; 441 | G.readGraphFromFiles("nodes_blank.txt", "edges_blank.txt"); 442 | 443 | //set rhs and g values in G to default values 444 | for (int i = 0; i < G.numNodes; i++) { 445 | Node* thisNode = &G.nodes[i]; 446 | thisNode->g = LARGE; //'infinity' 447 | thisNode->rhs = LARGE; //'infinity' 448 | } 449 | 450 | float km = 0; //key modifier 451 | 452 | //initialize heap 453 | Heap H(100); // this is the heap (start's with space for 100 items 454 | // but will grow automatically as needed). 455 | 456 | //these are pointers to the start and end nodes 457 | Node* startNode = &G.nodes[startNodeIndex]; 458 | Node* lastNode = startNode; 459 | Node* goalNode = &G.nodes[goalNodeIndex]; 460 | goalNode->rhs = 0; 461 | 462 | H.addToHeap(goalNode, calculateKey1(goalNode,startNode,km)); 463 | 464 | //COMPUTE INITIAL SOLUTION 465 | computeShortestPath(G,H,startNode,goalNode,startNodeIndex,goalNodeIndex,km); 466 | 467 | G.saveSearchTreeToFile("search_tree1.txt"); 468 | G.savePathToFile("output_path1.txt", startNode); 469 | 470 | 471 | //this is the D* Lite loop that moves us along the path, checking for obstacles and replanning 472 | Node* thisNode = startNode; //uncomment for reverse search rooted at goal 473 | while(startNode->id != goalNode->id) { //update this to stop when we traverse to goal successfully 474 | 475 | 476 | //move the start node to the next position along the solution path 477 | thisNode = thisNode->parentNode; 478 | Node* startNode = &G.nodes[thisNode->id]; 479 | Node* goalNode = &G.nodes[goalNodeIndex]; 480 | startNodeIndex = startNode->id; 481 | cout << startNode->id; 482 | 483 | //get user input for obstacle injection 484 | int inject; 485 | float x_obs, y_obs,r_obs; 486 | 487 | cout << " inject obstacle?????? 1=yes 0=no "; 488 | cin >> inject; 489 | 490 | //get obstacle data and then update all the shit 491 | if (inject==1) { 492 | cout << "Enter obstacle x-coord: "; 493 | cin >> x_obs; 494 | cout << "Enter obstacle y-coord: "; 495 | cin >> y_obs; 496 | cout << "Enter obstacle radius: "; 497 | cin >> r_obs; 498 | 499 | //update the edge costs to affected nodes 500 | float h = sqrt((lastNode->x - startNode->x)*(lastNode->x - startNode->x) + (lastNode->y - startNode->y)*(lastNode->y - startNode->y)); 501 | km = km + h; 502 | lastNode = startNode; 503 | updateEdgeCostsandNodes(G, H, startNode, goalNode, km, "nodes_blank.txt", "edges.txt", x_obs, y_obs, r_obs); //note: use the other edge file as this one gets modified 504 | 505 | //re-compute shortest path 506 | computeShortestPath(G,H,startNode,goalNode,startNodeIndex,goalNodeIndex,km); 507 | replan_counter = replan_counter + 1; 508 | 509 | if (replan_counter == 1) { 510 | G.saveSearchTreeToFile("search_tree2.txt"); 511 | G.savePathToFile("output_path2.txt", startNode); 512 | } 513 | if (replan_counter == 2) { 514 | G.saveSearchTreeToFile("search_tree3.txt"); 515 | G.savePathToFile("output_path3.txt", startNode); 516 | } 517 | if (replan_counter == 3) { 518 | G.saveSearchTreeToFile("search_tree4.txt"); 519 | G.savePathToFile("output_path4.txt", startNode); 520 | } 521 | } 522 | } 523 | 524 | 525 | return 0; 526 | } 527 | 528 | 529 | 530 | 531 | 532 | 533 | -------------------------------------------------------------------------------- /AutoQuad/src/control/control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import String, Float64, Empty, Int32 5 | from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped 6 | from nav_msgs.msg import Odometry 7 | from math import atan2, sin, cos, sqrt, asin, acos, atan 8 | import numpy 9 | import traceback 10 | import time 11 | from tf.transformations import quaternion_matrix 12 | 13 | 14 | ################## 15 | # Initialization # 16 | ################## 17 | 18 | 19 | 20 | 21 | ############# 22 | # Functions # 23 | ############# 24 | 25 | def odom_pose_feedback(data): 26 | global altitude 27 | altitude = data.pose.position.z 28 | 29 | def Vx_feedback(data): 30 | global Vx 31 | Vx = data.data 32 | 33 | def Vy_feedback(data): 34 | global Vy 35 | Vy = data.data 36 | 37 | def alt_des_feedback(data): 38 | global alt_des 39 | alt_des = data.data 40 | 41 | def Vx_des_feedback(data): 42 | global Vx_des 43 | Vx_des = data.data 44 | 45 | def Vy_des_feedback(data): 46 | global Vy_des 47 | Vy_des = data.data 48 | 49 | def yaw_des_feedback(data): 50 | global yaw_des 51 | yaw_des = data.data 52 | 53 | def accel_feedback(data): 54 | global Ax_IMU, Ay_IMU, Az_IMU 55 | Ax_IMU = data.accel.linear.x 56 | Ay_IMU = data.accel.linear.y 57 | Az_IMU = data.accel.linear.z 58 | 59 | def RC_feedback(data): 60 | global radio_command 61 | radio_command = data.data 62 | 63 | def target_pose_feedback(data): 64 | global target_orientation, target_x, target_y, target_z 65 | x = data.pose.position.x 66 | y = data.pose.position.y 67 | z = data.pose.position.z 68 | q0 = data.pose.orientation.w 69 | q1 = data.pose.orientation.x 70 | q2 = data.pose.orientation.y 71 | q3 = data.pose.orientation.z 72 | 73 | matrix = quaternion_matrix([q1, q2, q3, q0]) 74 | v = numpy.dot(numpy.transpose([x, y, z, 0.0]),matrix) 75 | target_x = -v[1] 76 | target_y = v[0] 77 | target_z = -v[2] 78 | 79 | pitch, roll, yaw = quaternion_to_euler(q0, q1, q2, q3) 80 | target_orientation = yaw*180.0/3.14159 81 | 82 | def target_detected_feedback(data): 83 | global target_detected 84 | target_detected = data.data 85 | 86 | def constrain(val, min_val, max_val): 87 | if val < min_val: return min_val 88 | if val > max_val: return max_val 89 | return val 90 | 91 | def quaternion_to_euler(w, x, y, z): 92 | t0 = +2.0 * (w * x + y * z) 93 | t1 = +1.0 - 2.0 * (x * x + y * y) 94 | roll = atan2(t0, t1) 95 | t2 = +2.0 * (w * y - z * x) 96 | t2 = +1.0 if t2 > +1.0 else t2 97 | t2 = -1.0 if t2 < -1.0 else t2 98 | pitch = asin(t2) 99 | t3 = +2.0 * (w * z + x * y) 100 | t4 = +1.0 - 2.0 * (y * y + z * z) 101 | yaw = atan2(t3, t4) 102 | return [roll, pitch, yaw] 103 | 104 | def controlAlt_odom(Kp,Ki,Kd,K,hov_pwm): 105 | global Az_IMU, dt 106 | global alt_des, altitude, integral_alt_prev, thro_pwm 107 | global altitude_prev 108 | global derivative_alt 109 | # PI controller for altitude, setpoint in meters 110 | error_limit = 0.3 #meters 111 | i_limit = 5.0 #arbitrary units 112 | 113 | error = alt_des - altitude 114 | error = constrain(error, -error_limit, error_limit) 115 | integral = integral_alt_prev + error*dt 116 | integral = constrain(integral, -i_limit, i_limit) #prevent windup 117 | if(radio_command == 1): #reset integral when entering alt hold 118 | integral = 0.0 119 | integral_alt_prev = integral 120 | B_der = 0.25 121 | dz = altitude - altitude_prev 122 | derivative_alt = (1.0 - B_der)*derivative_alt + B_der*dz 123 | altitude_prev = altitude 124 | PID = K*(Kp*error + Ki*integral - Kd*derivative_alt) 125 | #print"{:12.4f}".format(K*Ki*integral) 126 | 127 | # Convert command to int between 1000 and 2000 128 | thro_pwm = hov_pwm + PID 129 | thro_pwm = constrain(thro_pwm, hov_pwm - 125.0, hov_pwm + 125.0) 130 | thro_pwm = int(thro_pwm) 131 | 132 | def controlVelocity_odom(Kp,Ki,K): 133 | global Vx_des, Vy_des, Vx, Vy, roll_pwm, pitch_pwm, integral_x, integral_y 134 | # PI controller for velocity, setpoint in m/s 135 | error_limit = 0.8 #m/s 136 | i_limit = 250 #pwm 137 | 138 | # x-direction 139 | error = Vx_des - Vx 140 | error = constrain(error, -error_limit, error_limit) 141 | integral_x = integral_x + error 142 | if(radio_command == 1): #reset integral when entering autonomy 143 | integral_x = 0.0 144 | PIDx = K*Kp*error + constrain(K*Ki*integral_x, -i_limit, i_limit) 145 | 146 | # y-direction 147 | error = Vy_des - Vy 148 | error = constrain(error, -error_limit, error_limit) 149 | integral_y = integral_y + error 150 | if(radio_command == 1): #reset integral when entering autonomy 151 | integral_y = 0.0 152 | PIDy = K*Kp*error + constrain(K*Ki*integral_y, -i_limit, i_limit) 153 | 154 | # Convert command to int between 1000 and 2000 155 | roll_pwm = 1500.0 - PIDy 156 | roll_pwm = constrain(roll_pwm, 1000.0, 2000.0) 157 | roll_pwm = int(roll_pwm) 158 | pitch_pwm = 1500.0 + PIDx 159 | pitch_pwm = constrain(pitch_pwm, 1000.0, 2000.0) 160 | pitch_pwm = int(pitch_pwm) 161 | 162 | def controlAlt_target(Kp,Ki,Kd,K,hov_pwm): 163 | global dt 164 | global alt_des, target_z, integral_alt_prev, thro_pwm 165 | global altitude_prev 166 | global derivative_alt 167 | # PI controller for altitude, setpoint in meters 168 | error_limit = 0.3 #meters 169 | i_limit = 5.0 #arbitrary units 170 | 171 | error = alt_des - target_z 172 | error = constrain(error, -error_limit, error_limit) 173 | integral = integral_alt_prev + error*dt 174 | integral = constrain(integral, -i_limit, i_limit) #prevent windup 175 | if(radio_command == 1): #reset integral when entering alt hold 176 | integral = 0.0 177 | integral_alt_prev = integral 178 | B_der = 0.2 179 | dz = target_z - altitude_prev 180 | derivative_alt = (1.0 - B_der)*derivative_alt + B_der*dz 181 | altitude_prev = target_z 182 | PID = K*(Kp*error**3 + Ki*integral - Kd*derivative_alt) 183 | 184 | altitude_prev = target_z 185 | 186 | # Convert command to int between 1000 and 2000 187 | thro_pwm = hov_pwm + PID 188 | thro_pwm = constrain(thro_pwm, hov_pwm - 125.0, hov_pwm + 125.0) 189 | thro_pwm = int(thro_pwm) 190 | 191 | def controlPos_target(Kp,Ki,Kd,K,x_offset,y_offset): 192 | global dt 193 | global target_x, target_y, Vx, Vy, roll_pwm, pitch_pwm, integral_tx, integral_ty 194 | global roll_pwm, pitch_pwm 195 | global ex_prev, ey_prev 196 | # PID controller for position, setpoint x = 0.0m, y = 0.0m (plus offset) 197 | error_limit = 0.3 #meters 198 | i_limit = 50 #pwm 199 | B = 0.1 200 | 201 | # x-direction 202 | error = x_offset - target_x 203 | error = (1.0 - B)*ex_prev + B*error 204 | error = constrain(error, -error_limit, error_limit) 205 | integral_tx = integral_tx + error 206 | if(Ki != 0.0): 207 | integral_tx = constrain(K*Ki*integral_tx, -i_limit, i_limit)/(K*Ki) 208 | if(radio_command == 1): #reset integral when entering autonomy 209 | integral_tx = 0.0 210 | derivative = Vx 211 | PIDx = K*Kp*error + constrain(K*Ki*integral_tx, -i_limit, i_limit) - K*Kd*derivative 212 | ex_prev = error 213 | 214 | # y-direction 215 | error = y_offset - target_y 216 | error = (1.0 - B)*ey_prev + B*error 217 | error = constrain(error, -error_limit, error_limit) 218 | integral_ty = integral_ty + error 219 | if(Ki != 0.0): 220 | integral_ty = constrain(K*Ki*integral_ty, -i_limit, i_limit)/(K*Ki) 221 | if(radio_command == 1): #reset integral when entering autonomy 222 | integral_ty = 0.0 223 | derivative = Vy 224 | PIDy = K*Kp*error + constrain(K*Ki*integral_y, -i_limit, i_limit) - K*Kd*derivative 225 | ey_prev = error 226 | 227 | # Convert command to int between 1000 and 2000 228 | roll_pwm = 1500.0 - PIDy 229 | roll_pwm = constrain(roll_pwm, 1000.0, 2000.0) 230 | roll_pwm = int(roll_pwm) 231 | pitch_pwm = 1500.0 + PIDx 232 | pitch_pwm = constrain(pitch_pwm, 1000.0, 2000.0) 233 | pitch_pwm = int(pitch_pwm) 234 | 235 | 236 | def controlYaw(Kp,Ki,K): 237 | global dt 238 | global target_orientation, integral_yaw 239 | global radio_command 240 | global yaw_pwm 241 | # PI controller for orientation, setpoint in degrees (forced to 0.0) 242 | error_limit = 20.0 #degrees 243 | i_limit = 30 #pwm 244 | 245 | error = -target_orientation 246 | error = constrain(error, -error_limit, error_limit) 247 | integral_yaw = integral_yaw + error 248 | integral_yaw = constrain(K*Ki*integral_yaw, -i_limit, i_limit)/(K*Ki) 249 | if(radio_command == 1): #reset integral when entering autonomy 250 | integral_tx = 0.0 251 | PID = K*Kp*error + constrain(K*Ki*integral_yaw, -i_limit, i_limit) 252 | 253 | # Convert command to int between 1000 and 2000 254 | yaw_pwm = 1500.0 + PID 255 | yaw_pwm = constrain(yaw_pwm, 1000.0, 2000.0) 256 | yaw_pwm = int(yaw_pwm) 257 | 258 | def request_target_velocity(Kp,B): 259 | global Vx_des, Vy_des 260 | global target_x, target_y 261 | global Vx_des_prev, Vy_des_prev 262 | v_limit = 0.07 263 | 264 | Vx_des = -Kp*target_x 265 | Vy_des = -Kp*target_y #check sign 266 | Vx_des = constrain(Vx_des,-v_limit, v_limit) 267 | Vy_des = constrain(Vy_des,-v_limit, v_limit) 268 | 269 | Vx_des = (1.0 - B)*Vx_des_prev + B*Vx_des 270 | Vy_des = (1.0 - B)*Vy_des_prev + B*Vy_des 271 | Vx_des_prev = Vx_des 272 | Vy_des_prev = Vy_des 273 | 274 | 275 | ############# 276 | # MAIN LOOP # 277 | ############# 278 | 279 | def main(): 280 | # Declare variables 281 | global Ax_IMU, Ay_IMU, Az_IMU 282 | global altitude, Vx, Vy 283 | global alt_des, Vx_des, Vy_des, yaw_des 284 | global thro_pwm, roll_pwm, pitch_pwm, yaw_pwm 285 | global integral_alt_prev, altitude_prev 286 | global derivative_alt 287 | global integral_x, integral_y 288 | global integral_tx, integral_ty 289 | global radio_command 290 | global dt 291 | global target_orientation, target_x, target_y, target_z, target_detected 292 | global integral_yaw 293 | global ex_prev, ey_prev 294 | global Vx_des_prev, Vy_des_prev 295 | thro_pwm = 1000 296 | roll_pwm = pitch_pwm = yaw_pwm = 1500 297 | alt_des = Vx_des = Vy_des = yaw_des = 0.0 298 | integral_alt_prev = derivative_alt = altitude = altitude_prev = 0.0 299 | Vx = Vy = 0.0 300 | integral_x = integral_y = 0.0 301 | integral_tx = integral_ty = 0.0 302 | integral_yaw = 0.0 303 | Az_IMU = 0.0 304 | ex_prev = ey_prev = 0.0 305 | Vx_des_prev = Vy_des_prev = 0.0 306 | radio_command = 1 307 | target_detected = 0 308 | alt_des = 1.0 309 | 310 | # Initialize node 311 | rospy.init_node('control', anonymous=True) 312 | rate = rospy.Rate(50) # Hz 313 | dt = 1.0/50.0 314 | 315 | # Initialize topics to publish 316 | pub_thro = rospy.Publisher('/control/thro_pwm', Int32, queue_size=1) 317 | pub_roll = rospy.Publisher('/control/roll_pwm', Int32, queue_size=1) 318 | pub_pitch = rospy.Publisher('/control/pitch_pwm', Int32, queue_size=1) 319 | pub_yaw = rospy.Publisher('/control/yaw_pwm', Int32, queue_size=1) 320 | 321 | # Subscribe to topics with reference to callback functions 322 | rospy.Subscriber('/odom/pose', PoseStamped, odom_pose_feedback) 323 | rospy.Subscriber('/odom/Vel_x', Float64, Vx_feedback) 324 | rospy.Subscriber('/odom/Vel_y', Float64, Vy_feedback) 325 | rospy.Subscriber('/control/alt_des', Float64, alt_des_feedback) 326 | rospy.Subscriber('/control/Vx_des', Float64, Vx_des_feedback) 327 | rospy.Subscriber('/control/Vy_des', Float64, Vy_des_feedback) 328 | rospy.Subscriber('/control/yaw_des', Float64, yaw_des_feedback) 329 | rospy.Subscriber('/IMU/accel', AccelStamped, accel_feedback) 330 | rospy.Subscriber('/radio_command', Int32, RC_feedback) 331 | rospy.Subscriber('/target/pose', PoseStamped, target_pose_feedback) 332 | rospy.Subscriber('/target/target_detected', Int32, target_detected_feedback) 333 | 334 | 335 | 336 | while not rospy.is_shutdown(): 337 | try: 338 | # Do stuff 339 | 340 | 341 | if(target_detected == 1): 342 | alt_des = alt_des - dt*.08 #descend 343 | alt_des = constrain(alt_des, 0.0, 1.5) 344 | if (alt_des < 0.3): 345 | alt_des = 0.0 346 | Vx_des = 0.0 347 | Vy_des = 0.0 348 | 349 | #controlAlt_target(Kp = 0.12, Ki = 0.015, Kd = 2.0, K = 1000.0, hov_pwm = 1400.0) 350 | #controlPos_target(Kp = 0.15, Ki = 0.005, Kd = 0.12, K = 1000.0, x_offset = 0.0 , y_offset = 0.0) 351 | controlAlt_odom(Kp = 0.15, Ki = .1, Kd = 3.5, K = 1000.0, hov_pwm = 1450.0) 352 | request_target_velocity(Kp = 0.07, B = 0.4) 353 | controlVelocity_odom(Kp = 0.15, Ki = 0.0033, K = 1000.0) 354 | controlYaw(Kp = 0.15, Ki = 0.0003, K = 10.0) 355 | else: 356 | if (alt_des < 0.4): 357 | alt_des = 0.0 358 | Vx_des = 0.0 359 | Vy_des = 0.0 360 | else: 361 | alt_des = alt_des + dt*.08 #ascend 362 | alt_des = constrain(alt_des, 0.0, 1.5) 363 | Vx_des = 0.01 364 | Vy_des = 0.0 365 | 366 | yaw_pwm = 1500 367 | controlAlt_odom(Kp = 0.15, Ki = .1, Kd = 3.5, K = 1000.0, hov_pwm = 1450.0) 368 | controlVelocity_odom(Kp = 0.15, Ki = 0.0033, K = 1000.0) 369 | 370 | #print"{:12.9f}".format(Vx_des), 371 | #print"{:12.9f}".format(Vy_des) 372 | 373 | # Publish to topics 374 | pub_thro.publish(thro_pwm) 375 | pub_roll.publish(roll_pwm) 376 | pub_pitch.publish(pitch_pwm) 377 | pub_yaw.publish(yaw_pwm) 378 | 379 | 380 | except Exception: 381 | traceback.print_exc() 382 | #rospy.loginfo('Some error ocurred in control.py') 383 | indent = 1 384 | 385 | rate.sleep() 386 | 387 | 388 | if __name__ == '__main__': 389 | try: 390 | main() 391 | except rospy.ROSInterruptException: 392 | pass 393 | 394 | 395 | -------------------------------------------------------------------------------- /AutoQuad/src/control/position_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import String, Float64, Empty, Int32 5 | from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped 6 | from nav_msgs.msg import Odometry 7 | from math import atan2, sin, cos, sqrt, asin, acos, atan 8 | import numpy 9 | import traceback 10 | import time 11 | from tf.transformations import quaternion_matrix 12 | 13 | 14 | ################## 15 | # Initialization # 16 | ################## 17 | 18 | 19 | 20 | 21 | ############# 22 | # Functions # 23 | ############# 24 | 25 | def tfmini_feedback(data): 26 | global altitude_tfmini 27 | altitude_tfmini = data.data 28 | 29 | def alt_des_feedback(data): 30 | global alt_des 31 | alt_des = data.data 32 | 33 | def Xpos_des_feedback(data): 34 | global Xpos_des 35 | Xpos_des = data.data 36 | 37 | def Ypos_des_feedback(data): 38 | global Ypos_des 39 | Ypos_des = data.data 40 | 41 | def yaw_des_feedback(data): 42 | global yaw_des 43 | yaw_des = data.data 44 | 45 | def RC_feedback(data): 46 | global radio_command 47 | radio_command = data.data 48 | 49 | def odom_pose_feedback(data): 50 | global yaw_angle, Xpos, Ypos, alt, dx, dy, dalt 51 | x = data.pose.pose.position.x 52 | y = data.pose.pose.position.y 53 | z = data.pose.pose.position.z 54 | q0 = data.pose.pose.orientation.w 55 | q1 = data.pose.pose.orientation.x 56 | q2 = data.pose.pose.orientation.y 57 | q3 = data.pose.pose.orientation.z 58 | 59 | dx = data.twist.twist.linear.x 60 | dy = -data.twist.twist.linear.y 61 | dalt = -data.twist.twist.linear.z 62 | 63 | #matrix = quaternion_matrix([q1, q2, q3, q0]) 64 | #v = numpy.dot(numpy.transpose([x, y, z, 0.0]),matrix) #converts to body frame 65 | Xpos = x*1.33 #v[0] 66 | Ypos = y*1.33 #v[1] 67 | alt = z #-v[2] 68 | 69 | pitch, roll, yaw = quaternion_to_euler(q0, q1, q2, q3) 70 | yaw_angle = yaw*180.0/3.14159 71 | #if yaw_angle < 0: 72 | # yaw_angle = yaw_angle+180.0 73 | #else: 74 | # yaw_angle = yaw_angle-180.0 75 | 76 | 77 | def constrain(val, min_val, max_val): 78 | if val < min_val: return min_val 79 | if val > max_val: return max_val 80 | return val 81 | 82 | def quaternion_to_euler(w, x, y, z): 83 | t0 = +2.0 * (w * x + y * z) 84 | t1 = +1.0 - 2.0 * (x * x + y * y) 85 | roll = atan2(t0, t1) 86 | t2 = +2.0 * (w * y - z * x) 87 | t2 = +1.0 if t2 > +1.0 else t2 88 | t2 = -1.0 if t2 < -1.0 else t2 89 | pitch = asin(t2) 90 | t3 = +2.0 * (w * z + x * y) 91 | t4 = +1.0 - 2.0 * (y * y + z * z) 92 | yaw = atan2(t3, t4) 93 | return [roll, pitch, yaw] 94 | 95 | def controlAlt(Kp,Ki,Kd,K,hov_pwm): 96 | global dt 97 | global alt_des, alt, dalt, integral_alt_prev, thro_pwm 98 | global altitude_prev 99 | global derivative_alt 100 | # PID controller for altitude, setpoint in meters 101 | error_limit = 0.3 #meters 102 | i_limit = 5.0 #arbitrary units 103 | 104 | error = alt_des - alt 105 | error = constrain(error, -error_limit, error_limit) 106 | integral = integral_alt_prev + error*dt 107 | integral = constrain(integral, -i_limit, i_limit) #prevent windup 108 | if(radio_command == 1): #reset integral when entering alt hold 109 | integral = 0.0 110 | integral_alt_prev = integral 111 | #B_der = 0.3 112 | #dz = alt - altitude_prev 113 | #derivative_alt = (1.0 - B_der)*derivative_alt + B_der*dz #LP filter derivative signal 114 | altitude_prev = alt 115 | PID = K*(Kp*error + Ki*integral + Kd*dalt) 116 | 117 | altitude_prev = alt 118 | 119 | # Convert command to int between 1000 and 2000 120 | thro_pwm = hov_pwm + PID 121 | thro_pwm = constrain(thro_pwm, hov_pwm - 200.0, hov_pwm + 200.0) 122 | thro_pwm = int(thro_pwm) 123 | 124 | def controlAlt_tfmini(Kp,Ki,Kd,K,hov_pwm): 125 | global dt 126 | global altitude_tfmini, altitude_tfmini_prev 127 | global alt_des, alt, dalt, integral_alt_prev, thro_pwm 128 | global altitude_prev 129 | global derivative_alt 130 | # PID controller for altitude, setpoint in meters 131 | error_limit = 0.3 #meters 132 | i_limit = 5.0 #arbitrary units 133 | B = 0.35 134 | 135 | #filter tfmini altitude estimate 136 | dz = (altitude_tfmini - altitude_tfmini_prev) 137 | altitude_tfmini = (1.0 - B)*altitude_tfmini_prev + B*altitude_tfmini 138 | altitude_tfmini_prev = altitude_tfmini 139 | 140 | error = alt_des - altitude_tfmini 141 | error = constrain(error, -error_limit, error_limit) 142 | integral = integral_alt_prev + error*dt 143 | integral = constrain(integral, -i_limit, i_limit) #prevent windup 144 | if(radio_command == 1): #reset integral when entering alt hold 145 | integral = 0.0 146 | integral_alt_prev = integral 147 | PID = K*(Kp*error + Ki*integral - Kd*dz) 148 | 149 | #altitude_prev = alt 150 | 151 | # Convert command to int between 1000 and 2000 152 | thro_pwm = hov_pwm + PID 153 | thro_pwm = constrain(thro_pwm, hov_pwm - 200.0, hov_pwm + 200.0) 154 | thro_pwm = int(thro_pwm) 155 | 156 | def controlXY(Kp,Ki,Kd,K,x_offset,y_offset): 157 | global dt 158 | global Xpos_des, Ypos_des, Xpos, Ypos, dx, dy, roll_pwm, pitch_pwm, integral_x, integral_y 159 | global roll_pwm, pitch_pwm 160 | global ex_prev, ey_prev, derivative_x, derivative_x_prev, derivative_y, derivative_y_prev 161 | # PID controller for position 162 | error_limit = 0.6 #meters 163 | i_limit = 50 #pwm 164 | B = 0.2 165 | 166 | # x-direction 167 | error = x_offset + Xpos - Xpos_des 168 | #error = (1.0 - B)*ex_prev + B*error 169 | error = constrain(error, -error_limit, error_limit) 170 | integral_x = integral_x + error 171 | if(Ki != 0.0): 172 | integral_x = constrain(K*Ki*integral_x, -i_limit, i_limit)/(K*Ki) 173 | if(radio_command == 1): #reset integral when entering autonomy 174 | integral_x = 0.0 175 | derivative_x = (1.0 - B)*derivative_x_prev + B*dx 176 | derivative_x_prev = derivative_x 177 | PIDx = K*Kp*error + constrain(K*Ki*integral_x, -i_limit, i_limit) + K*Kd*derivative_x 178 | ex_prev = error 179 | 180 | # y-direction 181 | error = y_offset + Ypos - Ypos_des 182 | #error = (1.0 - B)*ey_prev + B*error 183 | error = constrain(error, -error_limit, error_limit) 184 | integral_y = integral_y + error 185 | if(Ki != 0.0): 186 | integral_y = constrain(K*Ki*integral_y, -i_limit, i_limit)/(K*Ki) 187 | if(radio_command == 1): #reset integral when entering autonomy 188 | integral_y = 0.0 189 | derivative_y = (1.0 - B)*derivative_y_prev + B*dy 190 | derivative_y_prev = derivative_y 191 | PIDy = K*Kp*error + constrain(K*Ki*integral_y, -i_limit, i_limit) - K*Kd*derivative_y 192 | ey_prev = error 193 | 194 | # Convert command to int between 1000 and 2000 195 | roll_pwm = 1500.0 + PIDy 196 | roll_pwm = constrain(roll_pwm, 1000.0, 2000.0) 197 | roll_pwm = int(roll_pwm) 198 | pitch_pwm = 1500.0 - PIDx 199 | pitch_pwm = constrain(pitch_pwm, 1000.0, 2000.0) 200 | pitch_pwm = int(pitch_pwm) 201 | 202 | 203 | def controlYaw(Kp,Ki,K): 204 | global dt 205 | global yaw_des, yaw_angle, integral_yaw 206 | global radio_command 207 | global yaw_pwm 208 | # PI controller for orientation, setpoint in degrees (forced to 0.0) 209 | error_limit = 20.0 #degrees 210 | i_limit = 30 #pwm 211 | 212 | 213 | error = (yaw_des - yaw_angle) 214 | error = constrain(error, -error_limit, error_limit) 215 | integral_yaw = integral_yaw + error 216 | integral_yaw = constrain(K*Ki*integral_yaw, -i_limit, i_limit)/(K*Ki) 217 | if(radio_command == 1): #reset integral when entering autonomy 218 | integral_yaw = 0.0 219 | PID = K*Kp*error + constrain(K*Ki*integral_yaw, -i_limit, i_limit) 220 | 221 | # Convert command to int between 1000 and 2000 222 | yaw_pwm = 1500.0 + PID 223 | yaw_pwm = constrain(yaw_pwm, 1000.0, 2000.0) 224 | yaw_pwm = int(yaw_pwm) 225 | 226 | 227 | 228 | 229 | ############# 230 | # MAIN LOOP # 231 | ############# 232 | 233 | def main(): 234 | # Declare variables 235 | global alt, Xpos, Ypos 236 | global alt_des, Xpos_des, Ypos_des, yaw_des, yaw_angle, dx, dy, dalt 237 | global thro_pwm, roll_pwm, pitch_pwm, yaw_pwm 238 | global integral_alt_prev, altitude_prev 239 | global derivative_alt 240 | global integral_x, integral_y 241 | global radio_command 242 | global dt 243 | global integral_yaw 244 | global ex_prev, ey_prev 245 | global altitude_tfmini, altitude_tfmini_prev 246 | global derivative_x, derivative_x_prev, derivative_y, derivative_y_prev 247 | derivative_x = derivative_x_prev = derivative_y = derivative_y_prev = 0.0 248 | altitude_tfmini = altitude_tfmini_prev = 0.0 249 | thro_pwm = 1000 250 | roll_pwm = pitch_pwm = yaw_pwm = 1500 251 | radio_command = 1 252 | alt_des = Xpos_des = Ypos_des = yaw_des = 0.0 253 | Xpos = Ypos = alt = yaw_angle = 0.0 254 | dx = dy = dalt = 0.0 255 | 256 | integral_alt_prev = derivative_alt = altitude = altitude_prev = 0.0 257 | integral_x = integral_y = 0.0 258 | integral_yaw = 0.0 259 | ex_prev = ey_prev = 0.0 260 | 261 | 262 | # Initialize node 263 | rospy.init_node('control', anonymous=True) 264 | rate = rospy.Rate(50) # Hz 265 | dt = 1.0/50.0 266 | 267 | # Initialize topics to publish 268 | pub_thro = rospy.Publisher('/control/thro_pwm', Int32, queue_size=1) 269 | pub_roll = rospy.Publisher('/control/roll_pwm', Int32, queue_size=1) 270 | pub_pitch = rospy.Publisher('/control/pitch_pwm', Int32, queue_size=1) 271 | pub_yaw = rospy.Publisher('/control/yaw_pwm', Int32, queue_size=1) 272 | 273 | # Subscribe to topics with reference to callback functions 274 | rospy.Subscriber('/camera/odom/sample', Odometry, odom_pose_feedback) 275 | rospy.Subscriber('/control/alt_des', Float64, alt_des_feedback) 276 | rospy.Subscriber('/control/Xpos_des', Float64, Xpos_des_feedback) 277 | rospy.Subscriber('/control/Ypos_des', Float64, Ypos_des_feedback) 278 | rospy.Subscriber('/control/yaw_des', Float64, yaw_des_feedback) 279 | rospy.Subscriber('/radio_command', Int32, RC_feedback) 280 | rospy.Subscriber('/tfmini/altitude', Float64, tfmini_feedback) 281 | 282 | 283 | 284 | while not rospy.is_shutdown(): 285 | try: 286 | # Do stuff 287 | 288 | #IMPORTANT VARIABLES: yaw_angle, Xpos, Ypos, alt, alt_des, Xpos_des, Ypos_des, yaw_des, dx, dy, dalt 289 | yaw_des = 0.0; #do not change!!! 290 | alt_des = 0.5; #set constant for runs 291 | 292 | 293 | controlAlt(Kp = 0.15, Ki = 0.1, Kd = 0.3, K = 1000.0, hov_pwm = 1440.0) #controls altitude from t265 z estimate 294 | #controlAlt_tfmini(Kp = 0.15, Ki = .1, Kd = 2.1, K = 1000.0, hov_pwm = 1450.0) 295 | controlXY(Kp = 0.28, Ki = 0.003, Kd = 0.4, K = 1000.0, x_offset = 0.0 , y_offset = 0.0) #kd = 0.12 296 | controlYaw(Kp = 0.2, Ki = 0.0002, K = 10.0) 297 | 298 | #print(Xpos,Ypos) 299 | 300 | 301 | # Publish to topics 302 | pub_thro.publish(thro_pwm) 303 | pub_roll.publish(roll_pwm) 304 | pub_pitch.publish(pitch_pwm) 305 | pub_yaw.publish(yaw_pwm) 306 | 307 | 308 | except Exception: 309 | traceback.print_exc() 310 | #rospy.loginfo('Some error ocurred in position_control.py') 311 | indent = 1 312 | 313 | rate.sleep() 314 | 315 | 316 | if __name__ == '__main__': 317 | try: 318 | main() 319 | except rospy.ROSInterruptException: 320 | pass 321 | 322 | 323 | -------------------------------------------------------------------------------- /AutoQuad/src/odometry/odometry.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import String, Float64, Empty, Int32 5 | from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped 6 | from nav_msgs.msg import Odometry 7 | from math import atan2, sin, cos, sqrt, asin, acos, atan 8 | import traceback 9 | import time 10 | 11 | 12 | ################## 13 | # Initialization # 14 | ################## 15 | 16 | 17 | 18 | 19 | ############# 20 | # Functions # 21 | ############# 22 | 23 | def pose_feedback(data): 24 | global q0, q1, q2, q3 25 | global roll, pitch, yaw 26 | q0 = data.pose.orientation.w 27 | q1 = data.pose.orientation.x 28 | q2 = data.pose.orientation.y 29 | q3 = data.pose.orientation.z 30 | roll, pitch, yaw = quaternion_to_euler(q0, q1, q2, q3) 31 | 32 | def accel_feedback(data): 33 | global Ax_IMU, Ay_IMU, Az_IMU 34 | Ax_IMU = data.accel.linear.x 35 | Ay_IMU = data.accel.linear.y 36 | Az_IMU = data.accel.linear.z 37 | 38 | def twist_feedback(data): 39 | global Gx, Gy, Gz 40 | Gx = data.twist.angular.x 41 | Gy = data.twist.angular.y 42 | Gz = data.twist.angular.z 43 | 44 | def x_vel_feedback(data): 45 | global Vx_px4, dt, Px 46 | Vx_px4 = data.data 47 | # integrate velocity for position estimate - mostly for visualization 48 | Px = Px + Vx_px4*dt 49 | 50 | 51 | def y_vel_feedback(data): 52 | global Vy_px4, dt, Py 53 | Vy_px4 = -data.data 54 | # integrate velocity for position estimate - mostly for visualization 55 | Py = Py + Vy_px4*dt 56 | 57 | def altitude_feedback(data): 58 | global altitude_px4 59 | altitude_px4 = data.data 60 | 61 | def quality_feedback(data): 62 | global quality_px4 63 | quality_px4 = data.data 64 | 65 | def tfmini_feedback(data): 66 | global altitude_tfmini 67 | altitude_tfmini = data.data 68 | 69 | def publishPose(): 70 | p = PoseStamped() 71 | p.header.frame_id = "map" 72 | p.header.stamp = rospy.Time.now() 73 | p.pose.position.x = Px 74 | p.pose.position.y = Py 75 | p.pose.position.z = Pz 76 | p.pose.orientation.x = q1 77 | p.pose.orientation.y = q2 78 | p.pose.orientation.z = q3 79 | p.pose.orientation.w = q0 80 | return p 81 | 82 | 83 | def quaternion_to_euler(w, x, y, z): 84 | t0 = +2.0 * (w * x + y * z) 85 | t1 = +1.0 - 2.0 * (x * x + y * y) 86 | roll = atan2(t0, t1) 87 | t2 = +2.0 * (w * y - z * x) 88 | t2 = +1.0 if t2 > +1.0 else t2 89 | t2 = -1.0 if t2 < -1.0 else t2 90 | pitch = asin(t2) 91 | t3 = +2.0 * (w * z + x * y) 92 | t4 = +1.0 - 2.0 * (y * y + z * z) 93 | yaw = atan2(t3, t4) 94 | return [roll, pitch, yaw] 95 | 96 | def fuseData(): 97 | global q0, q1, q2, q3 98 | global roll, pitch, yaw 99 | global Ax_IMU, Ay_IMU, Az_IMU, Vx_IMU, Vy_IMU 100 | global Gx, Gy, Gz 101 | global Vx_px4, Vy_px4, altitude_px4, quality_px4 102 | global Vx_px4_LP, Vx_px4_LP_prev, Vy_px4_LP, Vy_px4_LP_prev 103 | global Vx, Vx_prev, Vy, Vy_prev 104 | global Px, Py, Pz, Pz_prev, altitude_px4_prev 105 | global Pz_prev_re 106 | global alt_counter 107 | global dt 108 | 109 | # use orientation to get linear body accelerations 110 | #~ Ax = Ax_IMU + sin(pitch) 111 | #~ Ay = Ay_IMU - sin(roll) 112 | #~ Az = Az_IMU + abs(Ax_IMU*sin(roll)) + abs(Ay_IMU*sin(pitch)) - 1.0 113 | 114 | # LP filter px4 velocity measurements 115 | B_px4 = 0.13 #0.03 116 | Vx_px4_LP = (1.0 - B_px4)*Vx_px4_LP_prev + B_px4*Vx_px4 117 | Vy_px4_LP = (1.0 - B_px4)*Vy_px4_LP_prev + B_px4*Vy_px4 118 | Vx_px4_LP_prev = Vx_px4_LP 119 | Vy_px4_LP_prev = Vy_px4_LP 120 | Vx_px4_LP = Vx_px4_LP*1.0 121 | Vy_px4_LP = Vy_px4_LP*1.0 122 | 123 | # fuse IMU velocity estimate with px4 velocity estimate 124 | #~ B_comp = 1.0 #0.9 125 | #~ Vx = (1.0 - B_comp)*(Vx + Ax*dt*9.81) + B_comp*Vx_px4_LP 126 | #~ Vy = (1.0 - B_comp)*(Vy + Ay*dt*9.81) - B_comp*Vy_px4_LP 127 | Vx = Vx_px4_LP 128 | Vy = Vy_px4_LP 129 | 130 | #print"{:12.9f}".format(Vx), 131 | #print"{:12.9f}".format(Vy), 132 | #print"{:12.9f}".format(Px), 133 | #print"{:12.9f}".format(Py) 134 | 135 | # reject bad altitude data 136 | if(abs(altitude_tfmini - Pz_prev_re)>0.5): 137 | Pz_rejected = Pz_prev_re 138 | alt_counter = alt_counter + 1 139 | if (alt_counter == 35): #if 35 consecutive readings agree, probably true 140 | Pz_rejected = altitude_tfmini 141 | alt_counter = 0 142 | else: 143 | Pz_rejected = altitude_tfmini 144 | alt_counter = 0 145 | 146 | # filter rejected altitude estimate 147 | B_comp = 0.35 #0.008 148 | Pz = (1.0 - B_comp)*Pz_prev + B_comp*Pz_rejected 149 | 150 | Pz_prev = Pz 151 | Pz_prev_re = Pz_rejected 152 | altitude_px4_prev = altitude_px4 153 | 154 | 155 | ############# 156 | # MAIN LOOP # 157 | ############# 158 | 159 | def main(): 160 | # Declare variables 161 | global q0, q1, q2, q3 162 | global roll, pitch, yaw 163 | global Ax_IMU, Ay_IMU, Az_IMU, Vx_IMU, Vy_IMU 164 | global Gx, Gy, Gz 165 | global Vx_px4, Vy_px4, altitude_px4, quality_px4 166 | global altitude_tfmini 167 | global Vx_px4_LP, Vx_px4_LP_prev, Vy_px4_LP, Vy_px4_LP_prev 168 | global Vx, Vx_prev, Vy, Vy_prev 169 | global Px, Py, Pz, Pz_prev, altitude_px4_prev 170 | global Pz_prev_re 171 | global alt_counter 172 | global dt 173 | Vx_IMU = Vy_IMU = Vx_prev = Vy_prev = Vx_px4_LP = Vx_px4_LP_prev = Vy_px4_LP = Vy_px4_LP_prev = Gx = Gy = Gz = 0.0 174 | q1 = q2 = q3 = q0 = 0.0 175 | altitude_px4 = Vy_px4 = Vx_px4 = 0.0 176 | altitude_tfmini = 0.0 177 | Px = Py = Pz = altitude_px4_prev = 0 178 | Pz_prev = 0.3 179 | Pz_prev_re = 0.3 180 | Vx = Vy = 0 181 | quality_px4 = 0 182 | alt_counter = 0 183 | 184 | # Initialize node 185 | rospy.init_node('odom', anonymous=True) 186 | rate = rospy.Rate(100) # Hz 187 | dt = 1.0/100.0 188 | 189 | # Initialize topics to publish 190 | pub_pose = rospy.Publisher('/odom/pose', PoseStamped, queue_size=1) 191 | pub_Vx = rospy.Publisher('/odom/Vel_x', Float64, queue_size=1) 192 | pub_Vy = rospy.Publisher('/odom/Vel_y', Float64, queue_size=1) 193 | 194 | 195 | # Subscribe to topics with reference to callback functions 196 | rospy.Subscriber('/IMU/pose', PoseStamped, pose_feedback) 197 | rospy.Subscriber('/IMU/accel', AccelStamped, accel_feedback) 198 | rospy.Subscriber('/IMU/twist', TwistStamped, twist_feedback) 199 | rospy.Subscriber('/px4_data/quality', Int32, quality_feedback) 200 | rospy.Subscriber('/px4_data/x_vel', Float64, x_vel_feedback) 201 | rospy.Subscriber('/px4_data/y_vel', Float64, y_vel_feedback) 202 | rospy.Subscriber('/px4_data/altitude', Float64, altitude_feedback) 203 | rospy.Subscriber('/px4_data/quality', Int32, quality_feedback) 204 | rospy.Subscriber('/tfmini/altitude', Float64, tfmini_feedback) 205 | 206 | 207 | while not rospy.is_shutdown(): 208 | try: 209 | # Do stuff 210 | fuseData() 211 | 212 | # Publish to topics 213 | p = publishPose() #format data into posestamped struct 214 | pub_pose.publish(p) 215 | pub_Vx.publish(Vx) 216 | pub_Vy.publish(Vy) 217 | 218 | except Exception: 219 | traceback.print_exc() 220 | #rospy.loginfo('Some error ocurred in odometry.py') 221 | indent = 1 222 | 223 | rate.sleep() 224 | 225 | 226 | if __name__ == '__main__': 227 | try: 228 | main() 229 | except rospy.ROSInterruptException: 230 | pass 231 | 232 | 233 | -------------------------------------------------------------------------------- /AutoQuad/src/planning/planner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from apriltag_ros.msg import AprilTagDetectionArray 5 | from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped 6 | from std_msgs.msg import String, Float64, Empty, Int32 7 | from nav_msgs.msg import Odometry 8 | import math 9 | import traceback 10 | import time 11 | import numpy as np 12 | 13 | 14 | ################## 15 | # Initialization # 16 | ################## 17 | 18 | 19 | ############# 20 | # Functions # 21 | ############# 22 | 23 | def odom_pose_feedback(data): 24 | global Xpos, Ypos, alt 25 | Xpos = data.pose.pose.position.x 26 | Ypos = data.pose.pose.position.y 27 | alt = data.pose.pose.position.z 28 | 29 | Xpos = Xpos*1.33; 30 | Ypos = Ypos*1.33; 31 | 32 | 33 | 34 | def mission_planner_feedback(data): 35 | global mission_planner 36 | mission_planner = data.data 37 | 38 | def tag_feedback(data): 39 | global x_obs, y_obs 40 | global target_detected_0, target_detected_1, target_detected_2 41 | global detection_distance 42 | global flag_new_obstacle 43 | global XPos, YPos 44 | global mission_planner 45 | 46 | if(len(data.detections)==0): 47 | indent = 1 48 | #print("No tags detected...") 49 | else: 50 | #print("Tags Detected!") 51 | for i in range(0,len(data.detections)): 52 | if(data.detections[i].id[0] == 0 and target_detected_0 == 0 and flag_new_obstacle == 0 and mission_planner == 0): # first tag id 53 | Px = data.detections[i].pose.pose.pose.position.x 54 | Py = data.detections[i].pose.pose.pose.position.y 55 | Pz = data.detections[i].pose.pose.pose.position.z 56 | q1 = data.detections[i].pose.pose.pose.orientation.x 57 | q2 = data.detections[i].pose.pose.pose.orientation.y 58 | q3 = data.detections[i].pose.pose.pose.orientation.z 59 | q0 = data.detections[i].pose.pose.pose.orientation.w 60 | dist2camera = math.sqrt(Px*Px + Pz*Pz) #only care about z,x camera axes (x,y axes in inertial frame) 61 | print("tag with id '0' detected at distance: ", dist2camera) 62 | if (detection_distance > dist2camera): #within detection radius 63 | target_detected_0 = 1 #we've detected this target, dont detect it again 64 | #transform from camera frame to inertial frame 65 | x_obs = Xpos + Pz + r_offset #position in inertial frame 66 | y_obs = Ypos - Px 67 | print(x_obs,y_obs) 68 | flag_new_obstacle = 1 #used to tell in main loop that we've got a new obstacle to deal with 69 | if(data.detections[i].id[0] == 1 and target_detected_1 == 0 and flag_new_obstacle == 0 and mission_planner == 0): # second tag id 70 | Px = data.detections[i].pose.pose.pose.position.x 71 | Py = data.detections[i].pose.pose.pose.position.y 72 | Pz = data.detections[i].pose.pose.pose.position.z 73 | q1 = data.detections[i].pose.pose.pose.orientation.x 74 | q2 = data.detections[i].pose.pose.pose.orientation.y 75 | q3 = data.detections[i].pose.pose.pose.orientation.z 76 | q0 = data.detections[i].pose.pose.pose.orientation.w 77 | dist2camera = math.sqrt(Px*Px + Pz*Pz) #only care about z,x camera axes (x,y axes in inertial frame) 78 | print("tag with id '1' detected at distance: ", dist2camera) 79 | if (detection_distance > dist2camera): #within detection radius 80 | target_detected_1 = 1 #we've detected this target, dont detect it again 81 | #transform from camera frame to inertial frame 82 | x_obs = Xpos + Pz + r_offset #position in inertial frame 83 | y_obs = Ypos - Px 84 | print(x_obs,y_obs) 85 | flag_new_obstacle = 1 #used to tell in main loop that we've got a new obstacle to deal with 86 | if(data.detections[i].id[0] == 2 and target_detected_2 == 0 and flag_new_obstacle == 0 and mission_planner == 0): #third tag id 87 | Px = data.detections[i].pose.pose.pose.position.x 88 | Py = data.detections[i].pose.pose.pose.position.y 89 | Pz = data.detections[i].pose.pose.pose.position.z 90 | q1 = data.detections[i].pose.pose.pose.orientation.x 91 | q2 = data.detections[i].pose.pose.pose.orientation.y 92 | q3 = data.detections[i].pose.pose.pose.orientation.z 93 | q0 = data.detections[i].pose.pose.pose.orientation.w 94 | dist2camera = math.sqrt(Px*Px + Pz*Pz) #only care about z,x camera axes (x,y axes in inertial frame) 95 | print("tag with id '2' detected at distance: ", dist2camera) 96 | if (detection_distance > dist2camera): #within detection radius 97 | target_detected_2 = 1 #we've detected this target, dont detect it again 98 | #transform from camera frame to inertial frame 99 | x_obs = Xpos + Pz + r_offset #position in inertial frame 100 | y_obs = Ypos - Px 101 | print(x_obs,y_obs) 102 | flag_new_obstacle = 1 #used to tell in main loop that we've got a new obstacle to deal with 103 | 104 | else: 105 | indent = 1 106 | 107 | 108 | def publishPose(): 109 | p = PoseStamped() 110 | p.header.frame_id = "map" 111 | p.header.stamp = rospy.Time.now() 112 | p.pose.position.x = Px 113 | p.pose.position.y = Py 114 | p.pose.position.z = Pz 115 | p.pose.orientation.x = q1 116 | p.pose.orientation.y = q2 117 | p.pose.orientation.z = q3 118 | p.pose.orientation.w = q0 119 | return p 120 | 121 | ############# 122 | # MAIN LOOP # 123 | ############# 124 | 125 | def main(): 126 | # Declare variables 127 | global Px1, Py1, Pz1 128 | global q1_1, q2_1, q3_1, q0_1 129 | global Px, Py, Pz, q1, q2, q3, q0 130 | global target_detected_0, target_detected_1, target_detected_2 131 | global dt 132 | global x_des_pub, y_des_pub, NodeID_pub 133 | global mission_planner 134 | global Xpos, Ypos, alt 135 | global x_obs, y_obs, r_obs 136 | global detection_distance 137 | global flag_new_obstacle 138 | global r_offset 139 | flag_new_obstacle = 0 140 | Xpos = Ypos = alt = 0.0 141 | mission_planner = 1 #1 is stopped, 0 is allowed to traverse 142 | x_des_pub = y_des_pub = 0.0 143 | target_detected_0 = target_detected_1 = target_detected_2 = 0 144 | Px1 = Py1 = Pz1 = 0.0 145 | q1_1 = q2_1 = q3_1 = 0.0 146 | q0_1 = 1.0 147 | Px = Py = Pz = 0.0 148 | q1 = q2 = q3 = q0 = 0.0 149 | 150 | LogPos_arr = np.array([]) 151 | LogXPos_arr = np.array([]) 152 | LogYPos_arr = np.array([]) 153 | 154 | #important shit to change 155 | time_elapsed = 0.0 156 | time_interval = 0.4 #time between traversal from one pos to the next along path 157 | traverse_index = 0 158 | time_elapsed_log = 0.0 #for position data logging 159 | time_interval_log = 0.2 160 | 161 | detection_distance = 2.0 #minimum distance to april tag to detect it, meters 162 | r_obs = 1.0 #obstacle radius, meters 163 | r_offset = 0.15 #offset from front of obstacle to center (x-direction) 164 | 165 | x_des_pub = 0.0 #change to starting coordinates! 166 | y_des_pub = 2.0 #change to starting coordinates! 167 | NodeID_pub = 0 #doesnt matter 168 | 169 | 170 | # Initialize node 171 | rospy.init_node('planner', anonymous=True) 172 | rate = rospy.Rate(30) # Hz 173 | dt = 1/30.0 174 | 175 | 176 | 177 | # Initialize topics to publish 178 | pub_x_des = rospy.Publisher('/control/Xpos_des', Float64, queue_size=1) 179 | pub_y_des = rospy.Publisher('/control/Ypos_des', Float64, queue_size=1) 180 | pub_NodeID = rospy.Publisher('/control/NodeID', Int32, queue_size=1) 181 | pub_mission_planner = rospy.Publisher('/mission_planner', Int32, queue_size=1) 182 | pub_x_obs = rospy.Publisher('/obstacle/x_obs', Float64, queue_size=1) 183 | pub_y_obs = rospy.Publisher('/obstacle/y_obs', Float64, queue_size=1) 184 | pub_r_obs = rospy.Publisher('/obstacle/r_obs', Float64, queue_size=1) 185 | 186 | # Subscribe to topics with reference to callback functions 187 | rospy.Subscriber('/tag_detections', AprilTagDetectionArray, tag_feedback) 188 | rospy.Subscriber('/mission_planner', Int32, mission_planner_feedback) 189 | rospy.Subscriber('/camera/odom/sample', Odometry, odom_pose_feedback) 190 | 191 | 192 | # use these arrays as stand-in until we get read in from text files... 193 | global x_des_arr ,y_des_arr, NodeID_arr 194 | #x_des_arr = np.array([0.0, 1.0, 1.0, 0.0, 0.0]) #test square 195 | #y_des_arr = np.array([0.0, 0.0, 1.0, 1.0, 0.0]) 196 | 197 | #test-circle aka testicle 198 | #x_des_arr=[] 199 | #y_des_arr=[] 200 | #with open("CircleJerk.txt", "r") as filestream: 201 | # for line in filestream: 202 | # currentline = line.split(",") 203 | # x_des_arr = np.append(x_des_arr, float(currentline[1])) 204 | # y_des_arr = np.append(y_des_arr, float(currentline[2])) 205 | #print(x_des_arr) 206 | 207 | #MISSION PLANNER STATES: 208 | #0 = allow traversal along solution path 209 | #1 = stop traversal along solution path 210 | #2 = initial solution is available from D*Lite 211 | #3 = new solution (1st replan) is available from D*Lite 212 | #4 = new solution (2nd replan) is available from D*Lite 213 | #5 = new solution (3rd replan) is available from D*Lite 214 | #6 = brand new obstacle detected and (x,y,r) published 215 | 216 | pub_x_des.publish(x_des_pub) #publishes starting coordinates for the controller 217 | pub_y_des.publish(y_des_pub) 218 | pub_NodeID.publish(NodeID_pub) 219 | 220 | while not rospy.is_shutdown(): 221 | try: 222 | #print(Xpos,Ypos) 223 | 224 | #check if new solution is available...if it is, update x_des_arr & y_des_array from text file and set traverse_index = 0, set mission_planner topic to 0 to allow traversal 225 | if (mission_planner == 2): #initial solution is available from D*Lite node 226 | print("D*Lite returned initial solution path. Reading in...") 227 | x_des_arr=[] 228 | y_des_arr=[] 229 | NodeID_arr=[] 230 | with open("output_path1.txt", "r") as filestream: 231 | for line in filestream: 232 | currentline = line.split(",") 233 | NodeID_arr = np.append(NodeID_arr, float(currentline[0])) 234 | x_des_arr = np.append(x_des_arr, float(currentline[1])) 235 | y_des_arr = np.append(y_des_arr, float(currentline[2])) 236 | traverse_index = 0 237 | mission_planner = 0 #allow traversal over solution path 238 | pub_mission_planner.publish(mission_planner) 239 | 240 | 241 | if (mission_planner == 3): #1st replanned solution is available from D*Lite node 242 | print("D*Lite returned 1st replanned solution path. Reading in...") 243 | x_des_arr=[] 244 | y_des_arr=[] 245 | NodeID_arr=[] 246 | with open("output_path2.txt", "r") as filestream: 247 | for line in filestream: 248 | currentline = line.split(",") 249 | NodeID_arr = np.append(NodeID_arr, float(currentline[0])) 250 | x_des_arr = np.append(x_des_arr, float(currentline[1])) 251 | y_des_arr = np.append(y_des_arr, float(currentline[2])) 252 | traverse_index = 0 253 | mission_planner = 0 #allow traversal over solution path 254 | pub_mission_planner.publish(mission_planner) 255 | 256 | if (mission_planner == 4): #2nd replanned solution is available from D*Lite node 257 | print("D*Lite returned 2nd replanned solution path. Reading in...") 258 | x_des_arr=[] 259 | y_des_arr=[] 260 | NodeID_arr=[] 261 | with open("output_path3.txt", "r") as filestream: 262 | for line in filestream: 263 | currentline = line.split(",") 264 | NodeID_arr = np.append(NodeID_arr, float(currentline[0])) 265 | x_des_arr = np.append(x_des_arr, float(currentline[1])) 266 | y_des_arr = np.append(y_des_arr, float(currentline[2])) 267 | traverse_index = 0 268 | mission_planner = 0 #allow traversal over solution path 269 | pub_mission_planner.publish(mission_planner) 270 | 271 | if (mission_planner == 5): #3rd replanned solution is available from D*Lite node 272 | print("D*Lite returned 3rd replanned solution path. Reading in...") 273 | x_des_arr=[] 274 | y_des_arr=[] 275 | NodeID_arr=[] 276 | with open("output_path4.txt", "r") as filestream: 277 | for line in filestream: 278 | currentline = line.split(",") 279 | NodeID_arr = np.append(NodeID_arr, float(currentline[0])) 280 | x_des_arr = np.append(x_des_arr, float(currentline[1])) 281 | y_des_arr = np.append(y_des_arr, float(currentline[2])) 282 | traverse_index = 0 283 | mission_planner = 0 #allow traversal over solution path 284 | pub_mission_planner.publish(mission_planner) 285 | 286 | 287 | #check if obstacle detected, set mission_planner to 6 and publish x,y,r coordinates in inertial frame 288 | if (flag_new_obstacle == 1): 289 | print("New obstacle detected, publishing obstacle coordinates and updating mission_planner=6") 290 | 291 | pub_x_obs.publish(x_obs) 292 | pub_y_obs.publish(y_obs) 293 | pub_r_obs.publish(r_obs) 294 | flag_new_obstacle = 0 295 | mission_planner = 6 #new obstacle detected and coordinates published -> D* Lite node 296 | pub_mission_planner.publish(mission_planner) 297 | 298 | 299 | #traverse along solution until exhausted 300 | if (time_elapsed >= time_interval and mission_planner == 0 and traverse_index < len(x_des_arr)): #incriment along coord arrays in intervals of time_interval 301 | time_elapsed = 0.0 302 | 303 | #print(x_des_arr[traverse_index]) 304 | NodeID_pub = NodeID_arr[traverse_index] - 1; #converts to zero based indexing 305 | x_des_pub = x_des_arr[traverse_index] 306 | y_des_pub = y_des_arr[traverse_index] 307 | print("Traversing to Coordinates:",x_des_pub,y_des_pub) 308 | 309 | traverse_index = traverse_index + 1 310 | 311 | 312 | #log odometry data to text file 313 | if (time_elapsed_log >= time_interval_log): #log data every time_interval_log seconds 314 | time_elapsed_log = 0.0 315 | #Xpos, Ypos 316 | LogXPos_arr = np.append(LogXPos_arr, [Xpos]) 317 | LogYPos_arr = np.append(LogYPos_arr, [Ypos]) 318 | LogPos_arr = np.stack((LogXPos_arr,LogYPos_arr), axis=-1) 319 | np.savetxt("measured_path.txt", LogPos_arr) 320 | 321 | 322 | #publish desired coordinates 323 | pub_NodeID.publish(NodeID_pub) 324 | pub_x_des.publish(x_des_pub) 325 | pub_y_des.publish(y_des_pub) 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | time_elapsed = time_elapsed + dt 334 | time_elapsed_log = time_elapsed_log + dt 335 | 336 | 337 | except Exception: 338 | traceback.print_exc() 339 | #rospy.loginfo('Some error ocurred in target_planner.py') 340 | indent = 1 341 | 342 | rate.sleep() 343 | 344 | 345 | if __name__ == '__main__': 346 | try: 347 | main() 348 | except rospy.ROSInterruptException: 349 | pass 350 | 351 | 352 | -------------------------------------------------------------------------------- /AutoQuad/src/planning/target_planner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from apriltag_ros.msg import AprilTagDetectionArray 5 | from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped 6 | from std_msgs.msg import String, Float64, Empty, Int32 7 | import math 8 | import traceback 9 | import time 10 | 11 | 12 | ################## 13 | # Initialization # 14 | ################## 15 | 16 | 17 | ############# 18 | # Functions # 19 | ############# 20 | def tag_feedback(data): 21 | global Px1, Py1, Pz1 22 | global q1_1, q2_1, q3_1, q0_1 23 | global target_detected_1 24 | 25 | if(len(data.detections)==0): 26 | indent = 1 27 | target_detected_1 = 0 28 | else: 29 | for i in range(0,len(data.detections)): 30 | if(data.detections[i].id[0] == 10): # first tag id, etc... 31 | Px1 = data.detections[i].pose.pose.pose.position.x 32 | Py1 = data.detections[i].pose.pose.pose.position.y 33 | Pz1 = data.detections[i].pose.pose.pose.position.z 34 | q1_1 = data.detections[i].pose.pose.pose.orientation.x 35 | q2_1 = data.detections[i].pose.pose.pose.orientation.y 36 | q3_1 = data.detections[i].pose.pose.pose.orientation.z 37 | q0_1 = data.detections[i].pose.pose.pose.orientation.w 38 | target_detected_1 = 1 39 | else: 40 | target_detected_1 = 0 41 | 42 | 43 | def publishPose(): 44 | p = PoseStamped() 45 | p.header.frame_id = "map" 46 | p.header.stamp = rospy.Time.now() 47 | p.pose.position.x = Px 48 | p.pose.position.y = Py 49 | p.pose.position.z = Pz 50 | p.pose.orientation.x = q1 51 | p.pose.orientation.y = q2 52 | p.pose.orientation.z = q3 53 | p.pose.orientation.w = q0 54 | return p 55 | 56 | ############# 57 | # MAIN LOOP # 58 | ############# 59 | 60 | def main(): 61 | # Declare variables 62 | global Px1, Py1, Pz1 63 | global q1_1, q2_1, q3_1, q0_1 64 | global Px, Py, Pz, q1, q2, q3, q0 65 | global target_detected, target_detected_1 66 | target_detected = target_detected_1 = 0 67 | Px1 = Py1 = Pz1 = 0.0 68 | q1_1 = q2_1 = q3_1 = 0.0 69 | q0_1 = 1.0 70 | Px = Py = Pz = 0.0 71 | q1 = q2 = q3 = q0 = 0.0 72 | 73 | 74 | # Initialize node 75 | rospy.init_node('target_planner', anonymous=True) 76 | rate = rospy.Rate(30) # Hz 77 | 78 | # Initialize topics to publish 79 | pub_pose = rospy.Publisher('/target/pose', PoseStamped, queue_size=1) 80 | pub_target_detected = rospy.Publisher('/target/target_detected', Int32, queue_size=1) 81 | 82 | # Subscribe to topics with reference to callback functions 83 | rospy.Subscriber('/tag_detections', AprilTagDetectionArray, tag_feedback) 84 | 85 | while not rospy.is_shutdown(): 86 | try: 87 | # decide what target to be tracking here.....fixed on tag 1 for now 88 | Px = Px1 89 | Py = Py1 90 | Pz = Pz1 91 | q1 = q1_1 92 | q2 = q2_1 93 | q3 = q3_1 94 | q0 = q0_1 95 | target_detected = target_detected_1 96 | 97 | # Publish to topics 98 | p = publishPose() #format data into posestamped struct 99 | pub_pose.publish(p) 100 | pub_target_detected.publish(target_detected) 101 | 102 | 103 | except Exception: 104 | traceback.print_exc() 105 | #rospy.loginfo('Some error ocurred in target_planner.py') 106 | indent = 1 107 | 108 | rate.sleep() 109 | 110 | 111 | if __name__ == '__main__': 112 | try: 113 | main() 114 | except rospy.ROSInterruptException: 115 | pass 116 | 117 | 118 | -------------------------------------------------------------------------------- /AutoQuad/src/px4Flow/px4.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import String, Float64, Empty, Int32 5 | import math 6 | import traceback 7 | import time 8 | from px4Flow import PX4FLOW 9 | 10 | 11 | ################## 12 | # Initialization # 13 | ################## 14 | 15 | px4flow = PX4FLOW() 16 | 17 | 18 | ############# 19 | # Functions # 20 | ############# 21 | 22 | def printData(): 23 | global data, x_vel, y_vel, altitude, quality 24 | print"{:5.2f}".format(x_vel), 25 | print"{:5.2f}".format(y_vel), 26 | print"{:5.2f}".format(altitude), 27 | print"{:5.2f}".format(quality) 28 | #print('') 29 | 30 | 31 | ############# 32 | # MAIN LOOP # 33 | ############# 34 | 35 | def main(): 36 | # Declare variables 37 | global data, x_vel, y_vel, altitude, quality 38 | 39 | # Initialize node 40 | rospy.init_node('px4', anonymous=True) 41 | rate = rospy.Rate(100) # Hz 42 | 43 | # Initialize topics to publish 44 | pub_px4_x_vel = rospy.Publisher('/px4_data/x_vel', Float64, queue_size=1) 45 | pub_px4_y_vel = rospy.Publisher('/px4_data/y_vel', Float64, queue_size=1) 46 | pub_px4_alt = rospy.Publisher('/px4_data/altitude', Float64, queue_size=1) 47 | pub_px4_quality = rospy.Publisher('/px4_data/quality', Int32, queue_size=1) 48 | 49 | 50 | # Subscribe to topics with reference to callback functions 51 | 52 | 53 | while not rospy.is_shutdown(): 54 | try: 55 | # Get px4 data 56 | data = px4flow.read() 57 | x_vel = data[0]/1000.0 58 | y_vel = data[1]/1000.0 59 | altitude = data[2]/1000.0 60 | quality = data[3] 61 | #printData() 62 | 63 | # Publish to topics 64 | pub_px4_x_vel.publish(x_vel) 65 | pub_px4_y_vel.publish(y_vel) 66 | pub_px4_alt.publish(altitude) 67 | pub_px4_quality.publish(quality) 68 | 69 | except Exception: 70 | traceback.print_exc() 71 | #rospy.loginfo('Some error ocurred in px4.py') 72 | indent = 1 73 | 74 | rate.sleep() 75 | 76 | 77 | if __name__ == '__main__': 78 | try: 79 | main() 80 | except rospy.ROSInterruptException: 81 | pass 82 | 83 | 84 | -------------------------------------------------------------------------------- /AutoQuad/src/px4Flow/px4Flow.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #################################################################################################### 4 | #################################################################################################### 5 | ## ## 6 | ## Hove's Raspberry Pi Python Quadcopter Flight Controller. Open Source @ GitHub ## 7 | ## PiStuffing/Quadcopter under GPL for non-commercial application. Any code derived from ## 8 | ## this should retain this copyright comment. ## 9 | ## ## 10 | ## Copyright 2012 - 2016 Andy Baker (Hove) - andy@pistuffing.co.uk ## 11 | ## ## 12 | #################################################################################################### 13 | #################################################################################################### 14 | 15 | 16 | import time 17 | import string 18 | #import sys 19 | #import getopt 20 | import math 21 | import thread 22 | #from array import * 23 | import smbus 24 | 25 | 26 | 27 | #################################################################################################### 28 | # 29 | # Adafruit i2c interface enhanced with performance / error handling enhancements 30 | # 31 | #################################################################################################### 32 | class I2C: 33 | 34 | def __init__(self, address, bus=smbus.SMBus(1)): 35 | self.address = address 36 | self.bus = bus 37 | self.misses = 0 38 | 39 | def reverseByteOrder(self, data): 40 | "Reverses the byte order of an int (16-bit) or long (32-bit) value" 41 | # Courtesy Vishal Sapre 42 | dstr = hex(data)[2:].replace('L','') 43 | byteCount = len(dstr[::2]) 44 | val = 0 45 | for i, n in enumerate(range(byteCount)): 46 | d = data & 0xFF 47 | val |= (d << (8 * (byteCount - i - 1))) 48 | data >>= 8 49 | return val 50 | 51 | def writeByte(self, value): 52 | while True: 53 | try: 54 | self.bus.write_byte(self.address, value) 55 | break 56 | except IOError, err: 57 | self.misses += 1 58 | 59 | def write8(self, reg, value): 60 | "Writes an 8-bit value to the specified register/address" 61 | while True: 62 | try: 63 | self.bus.write_byte_data(self.address, reg, value) 64 | break 65 | except IOError, err: 66 | self.misses += 1 67 | 68 | def writeList(self, reg, list): 69 | "Writes an array of bytes using I2C format" 70 | while True: 71 | try: 72 | self.bus.write_i2c_block_data(self.address, reg, list) 73 | break 74 | except IOError, err: 75 | self.misses += 1 76 | 77 | def readU8(self, reg): 78 | "Read an unsigned byte from the I2C device" 79 | while True: 80 | try: 81 | result = self.bus.read_byte_data(self.address, reg) 82 | return result 83 | except IOError, err: 84 | self.misses += 1 85 | 86 | def readS8(self, reg): 87 | "Reads a signed byte from the I2C device" 88 | while True: 89 | try: 90 | result = self.bus.read_byte_data(self.address, reg) 91 | if (result > 127): 92 | return result - 256 93 | else: 94 | return result 95 | except IOError, err: 96 | self.misses += 1 97 | 98 | def readU16(self, reg): 99 | "Reads an unsigned 16-bit value from the I2C device" 100 | while True: 101 | try: 102 | hibyte = self.bus.read_byte_data(self.address, reg) 103 | result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg+1) 104 | return result 105 | except IOError, err: 106 | self.misses += 1 107 | 108 | def readS16(self, reg): 109 | "Reads a signed 16-bit value from the I2C device" 110 | while True: 111 | try: 112 | hibyte = self.bus.read_byte_data(self.address, reg) 113 | if (hibyte > 127): 114 | hibyte -= 256 115 | result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg+1) 116 | return result 117 | except IOError, err: 118 | self.misses += 1 119 | 120 | def readList(self, reg, length): 121 | "Reads a a byte array value from the I2C device" 122 | result = self.bus.read_i2c_block_data(self.address, reg, length) 123 | return result 124 | 125 | def getMisses(self): 126 | return self.misses 127 | 128 | 129 | 130 | #################################################################################################### 131 | # 132 | # Sensor Driver for PIX4FLOW sensor 133 | # 134 | #################################################################################################### 135 | class PX4FLOW: 136 | # i2c = None 137 | 138 | # Registers/etc. 139 | __PX4FLOW_REGISTER_BASE = 0x0 # 2 bytes unsigned - number 140 | __PX4FLOW_FRAME_COUNT = 0x0 # 2 bytes unsigned - number 141 | __PX4FLOW_PIXEL_FLOW_X = 0x2 # 2 bytes signed - latest x flow (pixels x 10) 142 | __PX4FLOW_PIXEL_FLOW_Y = 0x4 # 2 bytes signed - latest y flow (pixels x 10) 143 | __PX4FLOW_FLOW_COMP_M_X = 0x6 # 2 bytes signed - x velocity * 1000 (m/s) 144 | __PX4FLOW_FLOW_COMP_M_Y = 0x8 # 2 bytes signed - y velocity * 1000 (m/s) 145 | __PX4FLOW_QUAL_REG = 0x0A # 2 bytes signed - optical flow quality (0:bad 255:max) 146 | __PX4FLOW_GYRO_X_RATE = 0x0C # 2 bytes signed - gyro x rate (rad/sec) 147 | __PX4FLOW_GYRO_Y_RATE = 0x0E # 2 bytes signed - gyro y rate (rad/sec) 148 | __PX4FLOW_GYRO_Z_RATE = 0x10 # 2 bytes signed - gyro z rate (rad/sec) 149 | __PX4FLOW_GYRO_RANGE = 0x12 # 1 byte unsigned - 0 - 7 = 50 - 2000 (degrees / second) 150 | __PX4FLOW_SONAR_TIMESTAMP = 0x13 # 1 byte unsigned - time since last sonar sample (ms) 151 | __PX4FLOW_GROUND_DISTANCE = 0x14 # 2 bytes signed - ground distance (meters <0 = error) 152 | 153 | #-------------------------------------------------------------------------------------------------------- 154 | # All integrals since the previous I2C read 155 | #-------------------------------------------------------------------------------------------------------- 156 | __PX4FLOW_REGISTER_INTEGRAL = 0x16 # 2 unsigned - number of reads since last I2C read 157 | __PX4FLOW_FRAME_COUNT_INTEGRAL = 0x16 # 2 unsigned - number of reads since last I2C read 158 | __PX4FLOW_PIXEL_FLOW_X_INTEGRAL = 0x02 # 2 signed - integrated flow around x axis (rad * 1000) 159 | __PX4FLOW_PIXEL_FLOW_Y_INTEGRAL = 0x04 # 2 signed - integrated flow around y axis (rad * 1000) 160 | __PX4FLOW_GYRO_X_RATE_INTEGRAL = 0x06 # 2 signed - integrated gyro X axis roll (rad * 1000) # Would this be better for incremental roll rather than rate * dt 161 | __PX4FLOW_GYRO_Y_RATE_INTEGRAL = 0x08 # 2 signed - integrated gyro Y axis pitch (rad * 1000) # Would this be better for incremental roll rather than rate * dt 162 | __PX4FLOW_GYRO_Z_RATE_INTEGRAL = 0x0A # 2 signed - integrated gyro Z axis yaw (rad * 1000) # Would this be better for incremental roll rather than rate * dt 163 | __PX4FLOW_TIMESPAN_INTEGRAL = 0x0E # 4 unsigned - integrated time lapse (microseconds) 164 | __PX4FLOW_SONAR_TIMESTAMP_2 = 0x12 # 4 unsigned - time since last sonar update (microseconds) 165 | __PX4FLOW_GROUND_DISTANCE_2 = 0x14 # 2 signed - ground distance (meters * 1000) 166 | __PX4FLOW_GYRO_TEMPERATURE = 0x16 # 2 signed - temperature (celsius * 100) 167 | __PX4FLOW_QUALITY_AVERAGE = 0x18 # 1 signed - averaged quality (0:bad 255:max) 168 | 169 | def __init__(self, address=0x42): 170 | self.i2c = I2C(address) 171 | self.address = address 172 | 173 | def read(self): 174 | #AB! Isn't SMBUS limited to 32 bytes? 175 | sensor_data = self.i2c.readList(self.__PX4FLOW_REGISTER_BASE, 22) # 22 if not using the integral registers 176 | 177 | hibyte = sensor_data[self.__PX4FLOW_FLOW_COMP_M_X + 1] 178 | if (hibyte > 127): 179 | hibyte -= 256 180 | x_velocity = ((hibyte << 8) + sensor_data[self.__PX4FLOW_FLOW_COMP_M_X]) 181 | 182 | hibyte = sensor_data[self.__PX4FLOW_FLOW_COMP_M_Y + 1] 183 | if (hibyte > 127): 184 | hibyte -= 256 185 | y_velocity = ((hibyte << 8) + sensor_data[self.__PX4FLOW_FLOW_COMP_M_Y]) 186 | 187 | hibyte = sensor_data[self.__PX4FLOW_QUAL_REG + 1] 188 | if (hibyte > 127): 189 | hibyte -= 256 190 | qual = (hibyte << 8) + sensor_data[self.__PX4FLOW_QUAL_REG] 191 | 192 | #~ hibyte = sensor_data[self.__PX4FLOW_GYRO_X_RATE + 1] 193 | #~ if (hibyte > 127): 194 | #~ hibyte -= 256 195 | #~ pitch_rate = (hibyte << 8) + sensor_data[self.__PX4FLOW_GYRO_X_RATE] 196 | 197 | #~ hibyte = sensor_data[self.__PX4FLOW_GYRO_Y_RATE + 1] 198 | #~ if (hibyte > 127): 199 | #~ hibyte -= 256 200 | #~ roll_rate = (hibyte << 8) + sensor_data[self.__PX4FLOW_GYRO_Y_RATE] 201 | 202 | #~ hibyte = sensor_data[self.__PX4FLOW_GYRO_Z_RATE + 1] 203 | #~ if (hibyte > 127): 204 | #~ hibyte -= 256 205 | #~ yaw_rate = (hibyte << 8) + sensor_data[self.__PX4FLOW_GYRO_Z_RATE] 206 | 207 | #~ sonar_dt = (sensor_data[self.__PX4FLOW_SONAR_TIMESTAMP]) / 1000 208 | 209 | hibyte = sensor_data[self.__PX4FLOW_GROUND_DISTANCE + 1] 210 | if (hibyte > 127): 211 | hibyte -= 256 212 | ground_distance = ((hibyte << 8) + sensor_data[self.__PX4FLOW_GROUND_DISTANCE]) 213 | 214 | 215 | #return sensor_data 216 | return x_velocity, y_velocity, ground_distance, qual 217 | #return pitch_rate, roll_rate, yaw_rate 218 | -------------------------------------------------------------------------------- /AutoQuad/src/px4Flow/px4Flow.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nickrehm/AutoQuad/682f26b046a56ad1848a8b1ecad840ffe111ad34/AutoQuad/src/px4Flow/px4Flow.pyc -------------------------------------------------------------------------------- /AutoQuad/src/serialComm/serialComm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped 5 | from std_msgs.msg import String, Float64, Empty, Int32 6 | import math 7 | import traceback 8 | import serial 9 | import time 10 | 11 | 12 | ################## 13 | # Initialization # 14 | ################## 15 | 16 | # configure serial connection through GPIO pins 17 | ser = serial.Serial( 18 | port = '/dev/ttyAMA0', 19 | baudrate = 3000000, 20 | timeout=0.02) 21 | 22 | 23 | ############# 24 | # Functions # 25 | ############# 26 | 27 | def receiveData(): 28 | global q0, q1, q2, q3, GyroX, GyroY, GyroZ, AccX, AccY, AccZ, radio_command 29 | 30 | if(ser.read() == 'a'): 31 | q0 = ser.readline() 32 | q0 = float(ser.readline().strip()) 33 | if(ser.read()=='b'): 34 | q1 = ser.readline() 35 | q1 = float(ser.readline().strip()) 36 | if(ser.read()=='c'): 37 | q2 = ser.readline() 38 | q2 = float(ser.readline().strip()) 39 | if(ser.read()=='d'): 40 | q3 = ser.readline() 41 | q3 = float(ser.readline().strip()) 42 | if(ser.read()=='e'): 43 | GyroX = ser.readline() 44 | GyroX = float(ser.readline().strip()) 45 | if(ser.read()=='f'): 46 | GyroY = ser.readline() 47 | GyroY = float(ser.readline().strip()) 48 | if(ser.read()=='g'): 49 | GyroZ = ser.readline() 50 | GyroZ = float(ser.readline().strip()) 51 | if(ser.read()=='h'): 52 | AccX = ser.readline() 53 | AccX = float(ser.readline().strip()) 54 | if(ser.read()=='i'): 55 | AccY = ser.readline() 56 | AccY = float(ser.readline().strip()) 57 | if(ser.read()=='j'): 58 | AccZ = ser.readline() 59 | AccZ = float(ser.readline().strip()) 60 | if(ser.read()=='k'): 61 | radio_command = ser.readline() 62 | radio_command = int(ser.readline().strip()) 63 | 64 | def sendData(): 65 | ser.write('a') 66 | #ser.write('*') 67 | ser.write(bytes(auto_thro_pwm)) 68 | ser.write('b') 69 | 70 | ser.write('b') 71 | #ser.write('*') 72 | ser.write(bytes(auto_roll_pwm)) 73 | ser.write('c') 74 | 75 | ser.write('c') 76 | #ser.write('*') 77 | ser.write(bytes(auto_pitch_pwm)) 78 | ser.write('d') 79 | 80 | ser.write('d') 81 | #ser.write('*') 82 | ser.write(bytes(auto_yaw_pwm)) 83 | ser.write('e') 84 | 85 | def checkData(): 86 | global q0, q1, q2, q3, GyroX, GyroY, GyroZ, AccX, AccY, AccZ, radio_command 87 | global q0_prev, q1_prev, q2_prev, q3_prev, GyroX_prev, GyroY_prev, GyroZ_prev, AccX_prev, AccY_prev, AccZ_prev, radio_command_prev 88 | check = 0 89 | 90 | if(isinstance(q0, float) == False): 91 | check = check + 1 92 | if(isinstance(q1, float) == False): 93 | check = check + 1 94 | if(isinstance(q2, float) == False): 95 | check = check + 1 96 | if(isinstance(q3, float) == False): 97 | check = check + 1 98 | if(isinstance(GyroX, float) == False): 99 | check = check + 1 100 | if(isinstance(GyroY, float) == False): 101 | check = check + 1 102 | if(isinstance(GyroZ, float) == False): 103 | check = check + 1 104 | if(isinstance(AccX, float) == False): 105 | check = check + 1 106 | if(isinstance(AccY, float) == False): 107 | check = check + 1 108 | if(isinstance(AccZ, float) == False): 109 | check = check + 1 110 | if(isinstance(radio_command, int) == False): 111 | check = check + 1 112 | 113 | if(check > 0): 114 | # print("Bad data packet received") 115 | q0 = q0_prev 116 | q1 = q1_prev 117 | q2 = q2_prev 118 | q3 = q3_prev 119 | GyroX = GyroX_prev 120 | GyroY = GyroY_prev 121 | GyroZ = GyroZ_prev 122 | AccX = AccX_prev 123 | AccY = AccY_prev 124 | AccZ = AccZ_prev 125 | radio_command = radio_command_prev 126 | 127 | q0_prev = q0 128 | q1_prev = q1 129 | q2_prev = q2 130 | q3_prev = q3 131 | GyroX_prev = GyroX 132 | GyroY_prev = GyroY 133 | GyroZ_prev = GyroZ 134 | AccX_prev = AccX 135 | AccY_prev = AccY 136 | AccZ_prev = AccZ 137 | 138 | def printData(): 139 | print(' q0 '), 140 | print('q1 '), 141 | print('q2 '), 142 | print('q3 '), 143 | print('GX '), 144 | print('GY '), 145 | print('GZ '), 146 | print('AccX '), 147 | print('AccY '), 148 | print('AccZ '), 149 | print('radio_command') 150 | print"{:5.2f}".format(q0), 151 | print"{:5.2f}".format(q1), 152 | print"{:5.2f}".format(q2), 153 | print"{:5.2f}".format(q3), 154 | print"{:5.2f}".format(GyroX), 155 | print"{:5.2f}".format(GyroY), 156 | print"{:5.2f}".format(GyroZ), 157 | print"{:5.2f}".format(AccX), 158 | print"{:5.2f}".format(AccY), 159 | print"{:5.2f}".format(AccZ), 160 | print"{:7}".format(radio_command) 161 | print('') 162 | 163 | 164 | def publishPose(): 165 | p = PoseStamped() 166 | p.header.frame_id = "map" 167 | p.header.stamp = rospy.Time.now() 168 | p.pose.position.x = 0.0 169 | p.pose.position.y = 0.0 170 | p.pose.position.z = 0.0 171 | p.pose.orientation.x = q1 172 | p.pose.orientation.y = q2 173 | p.pose.orientation.z = q3 174 | p.pose.orientation.w = q0 175 | return p 176 | 177 | def publishTwist(): 178 | t = TwistStamped() 179 | t.header.frame_id = "map" 180 | t.header.stamp = rospy.Time.now() 181 | t.twist.linear.x = 0.0 182 | t.twist.linear.y = 0.0 183 | t.twist.linear.z = 0.0 184 | t.twist.angular.x = GyroX 185 | t.twist.angular.y = GyroY 186 | t.twist.angular.z = GyroZ 187 | return t 188 | 189 | def publishAccel(): 190 | a = AccelStamped() 191 | a.header.frame_id = "map" 192 | a.header.stamp = rospy.Time.now() 193 | a.accel.linear.x = AccX 194 | a.accel.linear.y = AccY 195 | a.accel.linear.z = AccZ 196 | return a 197 | 198 | def thro_pwm_feedback(data): 199 | global auto_thro_pwm 200 | auto_thro_pwm = data.data 201 | 202 | def roll_pwm_feedback(data): 203 | global auto_roll_pwm 204 | auto_roll_pwm = data.data 205 | 206 | def pitch_pwm_feedback(data): 207 | global auto_pitch_pwm 208 | auto_pitch_pwm = data.data 209 | 210 | def yaw_pwm_feedback(data): 211 | global auto_yaw_pwm 212 | auto_yaw_pwm = data.data 213 | 214 | ############# 215 | # MAIN LOOP # 216 | ############# 217 | 218 | def main(): 219 | # Declare variables 220 | global q0, q1, q2, q3, GyroX, GyroY, GyroZ, AccX, AccY, AccZ, radio_command 221 | global q0_prev, q1_prev, q2_prev, q3_prev, GyroX_prev, GyroY_prev, GyroZ_prev, AccX_prev, AccY_prev, AccZ_prev, radio_command_prev 222 | global auto_thro_pwm, auto_roll_pwm, auto_pitch_pwm, auto_yaw_pwm 223 | auto_thro_pwm = 1000 224 | auto_roll_pwm = auto_pitch_pwm = auto_yaw_pwm = 1500 225 | q0 = q1 = q2 = q3 = GyroX = GyroY = GyroZ = AccX = AccY = AccZ = 0 226 | radio_command = 1 227 | 228 | # Initialize node 229 | rospy.init_node('serialComm', anonymous=True) 230 | rate = rospy.Rate(50) # Hz 231 | 232 | # Initialize topics to publish 233 | pub_pose = rospy.Publisher('/IMU/pose', PoseStamped, queue_size=1) 234 | pub_twist = rospy.Publisher('/IMU/twist', TwistStamped, queue_size=1) 235 | pub_accel = rospy.Publisher('/IMU/accel', AccelStamped, queue_size=1) 236 | pub_radio_command = rospy.Publisher('/radio_command', Int32, queue_size=1) 237 | 238 | # Subscribe to topics with reference to callback functions 239 | rospy.Subscriber('/control/thro_pwm', Int32, thro_pwm_feedback) 240 | rospy.Subscriber('/control/roll_pwm', Int32, roll_pwm_feedback) 241 | rospy.Subscriber('/control/pitch_pwm', Int32, pitch_pwm_feedback) 242 | rospy.Subscriber('/control/yaw_pwm', Int32, yaw_pwm_feedback) 243 | 244 | while not rospy.is_shutdown(): 245 | try: 246 | # receive/send data 247 | ser.reset_input_buffer() 248 | ser.reset_output_buffer() 249 | receiveData() 250 | sendData() 251 | checkData() 252 | #printData() 253 | 254 | # Publish to topics 255 | p = publishPose() #format data into posestamped struct 256 | pub_pose.publish(p) 257 | t = publishTwist() #format data into twiststamped struct 258 | pub_twist.publish(t) 259 | a = publishAccel() #format data into accelstamped struct 260 | pub_accel.publish(a) 261 | pub_radio_command.publish(radio_command) 262 | 263 | 264 | except Exception: 265 | #traceback.print_exc() 266 | #rospy.loginfo('Some error ocurred in serialComm.py') 267 | indent = 1 268 | 269 | rate.sleep() 270 | 271 | 272 | if __name__ == '__main__': 273 | try: 274 | main() 275 | except rospy.ROSInterruptException: 276 | pass 277 | 278 | 279 | -------------------------------------------------------------------------------- /AutoQuad/src/serialComm/serial_testing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import serial 4 | import time 5 | import math 6 | 7 | # configure serial connection 8 | ser = serial.Serial( 9 | port = '/dev/ttyAMA0', 10 | baudrate = 3000000, 11 | timeout=1) 12 | 13 | 14 | # loop 15 | q0 = q1 = q2 = q3 = GyroX = GyroY = GyroZ = AccX = AccY = AccZ = 0 16 | auto_thro_pwm = 1010 17 | auto_roll_pwm = 1500 18 | auto_pitch_pwm = 1500 19 | auto_pitch_pwm = 1500 20 | auto_yaw_pwm = 1500 21 | dt = .002 22 | t = 0 23 | while 1: 24 | t = t + dt 25 | auto_roll_pwm = 1500 + int(100*math.sin(2.0*(t + dt))) 26 | 27 | try: 28 | # receive data 29 | if(ser.read() == 'a'): 30 | q0 = ser.readline() 31 | q0 = float(ser.readline()) 32 | if(ser.read()=='b'): 33 | q1 = ser.readline() 34 | q1 = float(ser.readline()) 35 | if(ser.read()=='c'): 36 | q2 = ser.readline() 37 | q2 = float(ser.readline()) 38 | if(ser.read()=='d'): 39 | q3 = ser.readline() 40 | q3 = float(ser.readline()) 41 | if(ser.read()=='e'): 42 | GyroX = ser.readline() 43 | GyroX = float(ser.readline()) 44 | if(ser.read()=='f'): 45 | GyroY = ser.readline() 46 | GyroY = float(ser.readline()) 47 | if(ser.read()=='g'): 48 | GyroZ = ser.readline() 49 | GyroZ = float(ser.readline()) 50 | if(ser.read()=='h'): 51 | AccX = ser.readline() 52 | AccX = float(ser.readline()) 53 | if(ser.read()=='i'): 54 | AccY = ser.readline() 55 | AccY = float(ser.readline()) 56 | if(ser.read()=='j'): 57 | AccZ = ser.readline() 58 | AccZ = float(ser.readline()) 59 | ser.reset_input_buffer() 60 | 61 | # print recieved data 62 | print(' q0 '), 63 | print('q1 '), 64 | print('q2 '), 65 | print('q3 '), 66 | print('GyroX '), 67 | print('GyroY '), 68 | print('GyroZ '), 69 | print('AccX '), 70 | print('AccY '), 71 | print('AccZ') 72 | print"{:6.2f}".format(q0), 73 | print"{:6.2f}".format(q1), 74 | print"{:6.2f}".format(q2), 75 | print"{:6.2f}".format(q3), 76 | print"{:6.2f}".format(GyroX), 77 | print"{:6.2f}".format(GyroY), 78 | print"{:6.2f}".format(GyroZ), 79 | print"{:6.2f}".format(AccX), 80 | print"{:6.2f}".format(AccY), 81 | print"{:6.2f}".format(AccZ) 82 | print('') 83 | 84 | # send data 85 | ser.write('a') 86 | ser.write(bytes(auto_thro_pwm)) 87 | ser.write('b') 88 | ser.write('b') 89 | ser.write(bytes(auto_roll_pwm)) 90 | ser.write('c') 91 | ser.write('c') 92 | ser.write(bytes(auto_pitch_pwm)) 93 | ser.write('d') 94 | ser.write('d') 95 | ser.write(bytes(auto_yaw_pwm)) 96 | ser.write('e') 97 | 98 | 99 | 100 | 101 | time.sleep(0.002) # 500Hz 102 | 103 | except Exception: 104 | pass 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | -------------------------------------------------------------------------------- /AutoQuad/src/servo/servo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped 5 | from std_msgs.msg import String, Float64, Empty, Int32 6 | import traceback 7 | import pigpio 8 | import time 9 | 10 | 11 | ################## 12 | # Initialization # 13 | ################## 14 | 15 | pi = pigpio.pi() 16 | 17 | 18 | ############# 19 | # Functions # 20 | ############# 21 | def servo_feedback(data): 22 | global servo_state 23 | servo_state = data.data 24 | 25 | ############# 26 | # MAIN LOOP # 27 | ############# 28 | 29 | def main(): 30 | # Declare variables 31 | global servo_state 32 | servo_state = 0 33 | 34 | # Initialize node 35 | rospy.init_node('servo', anonymous=True) 36 | rate = rospy.Rate(5) # Hz 37 | 38 | # Initialize topics to publish 39 | 40 | # Subscribe to topics with reference to callback functions 41 | rospy.Subscriber('/servo_command', Int32, servo_feedback) 42 | 43 | while not rospy.is_shutdown(): 44 | try: 45 | # actuate servo 46 | if(servo_state == 0): 47 | #open 48 | pi.set_servo_pulsewidth(12,1030) 49 | elif(servo_state == 1): 50 | #closed 51 | pi.set_servo_pulsewidth(12,1800) 52 | 53 | # Publish to topics 54 | 55 | 56 | 57 | except Exception: 58 | traceback.print_exc() 59 | #rospy.loginfo('Some error ocurred in servo.py') 60 | indent = 1 61 | 62 | rate.sleep() 63 | 64 | 65 | if __name__ == '__main__': 66 | try: 67 | main() 68 | except rospy.ROSInterruptException: 69 | pass 70 | 71 | 72 | -------------------------------------------------------------------------------- /AutoQuad/src/servo/servotesting.py: -------------------------------------------------------------------------------- 1 | import RPi.GPIO as GPIO 2 | import time 3 | 4 | GPIO.setwarnings(False) 5 | GPIO.setmode(GPIO.BCM) 6 | GPIO.setup(12, GPIO.OUT) 7 | servo1 = GPIO.PWM(12,50) #pin, frequency 8 | servo1.start(0) #initialize to 0 duty cycle 9 | 10 | 11 | angle_des = 60 12 | duty = angle_des/18 + 2 13 | 14 | while True: 15 | servo1.ChangeDutyCycle(duty) 16 | 17 | 18 | -------------------------------------------------------------------------------- /AutoQuad/src/tfmini/tfmini.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import PoseStamped, TwistStamped, AccelStamped 5 | from std_msgs.msg import String, Float64, Empty, Int32 6 | import math 7 | import traceback 8 | import serial 9 | import time 10 | 11 | 12 | ################## 13 | # Initialization # 14 | ################## 15 | 16 | # configure serial connection through GPIO pins 17 | ser = serial.Serial( 18 | port = '/dev/ttyAMA1', 19 | baudrate = 115200, 20 | timeout=0.1) 21 | 22 | 23 | ############# 24 | # Functions # 25 | ############# 26 | def get_data(): 27 | global distance 28 | ser.reset_input_buffer() 29 | recv = ser.read(9) 30 | if recv[0] == 'Y' and recv[1] == 'Y': # 0x59 is 'Y' 31 | low = int(recv[2].encode('hex'), 16) 32 | high = int(recv[3].encode('hex'), 16) 33 | distance = (low + high * 256)/100.0 34 | 35 | ############# 36 | # MAIN LOOP # 37 | ############# 38 | 39 | def main(): 40 | # Declare variables 41 | global distance 42 | distance = 0.0 43 | 44 | # Initialize node 45 | rospy.init_node('tfmini', anonymous=True) 46 | rate = rospy.Rate(100) # Hz 47 | 48 | # Initialize topics to publish 49 | pub_alt = rospy.Publisher('/tfmini/altitude', Float64, queue_size=1) 50 | 51 | 52 | # Subscribe to topics with reference to callback functions 53 | 54 | 55 | while not rospy.is_shutdown(): 56 | try: 57 | # receive data 58 | get_data() 59 | 60 | # Publish to topics 61 | pub_alt.publish(distance) 62 | 63 | 64 | except Exception: 65 | traceback.print_exc() 66 | #rospy.loginfo('Some error ocurred in tfmini.py') 67 | indent = 1 68 | 69 | rate.sleep() 70 | 71 | 72 | if __name__ == '__main__': 73 | try: 74 | main() 75 | except rospy.ROSInterruptException: 76 | pass 77 | 78 | 79 | -------------------------------------------------------------------------------- /AutoQuad/src/tfmini/tfmini_test.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -* 2 | import serial 3 | 4 | ser = serial.Serial( 5 | port = '/dev/ttyAMA1', 6 | baudrate = 115200, 7 | timeout=0.02) 8 | 9 | def getTFminiData(): 10 | while True: 11 | count = ser.in_waiting 12 | if count > 8: 13 | recv = ser.read(9) 14 | ser.reset_input_buffer() 15 | if recv[0] == 'Y' and recv[1] == 'Y': # 0x59 is 'Y' 16 | low = int(recv[2].encode('hex'), 16) 17 | high = int(recv[3].encode('hex'), 16) 18 | distance = (low + high * 256)/100.0 19 | print(distance) 20 | 21 | 22 | 23 | if __name__ == '__main__': 24 | try: 25 | if ser.is_open == False: 26 | ser.open() 27 | getTFminiData() 28 | except KeyboardInterrupt: # Ctrl+C 29 | if ser != None: 30 | ser.close() 31 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake --------------------------------------------------------------------------------