├── .gitignore ├── COPYING.md ├── README.md ├── mpc_local_planner ├── .clang-format ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ ├── mpc_collision.cfg │ ├── mpc_controller.cfg │ ├── mpc_footprint.cfg │ ├── rviz_test_mpc_optim.rviz │ └── test_mpc_optim_node.yaml ├── include │ └── mpc_local_planner │ │ ├── controller.h │ │ ├── mpc_local_planner_ros.h │ │ ├── optimal_control │ │ ├── fd_collocation_se2.h │ │ ├── final_state_conditions_se2.h │ │ ├── finite_differences_grid_se2.h │ │ ├── finite_differences_variable_grid_se2.h │ │ ├── full_discretization_grid_base_se2.h │ │ ├── min_time_via_points_cost.h │ │ ├── quadratic_cost_se2.h │ │ ├── stage_inequality_se2.h │ │ └── vector_vertex_se2.h │ │ ├── systems │ │ ├── base_robot_se2.h │ │ ├── kinematic_bicycle_model.h │ │ ├── robot_dynamics_interface.h │ │ ├── simple_car.h │ │ └── unicycle_robot.h │ │ └── utils │ │ ├── conversion.h │ │ ├── math_utils.h │ │ ├── publisher.h │ │ └── time_series_se2.h ├── launch │ └── test_mpc_optim_node.launch ├── mpc_local_planner_plugin.xml ├── package.xml ├── scripts │ └── plot_optimal_control_results.py └── src │ ├── controller.cpp │ ├── mpc_local_planner_ros.cpp │ ├── optimal_control │ ├── final_state_conditions_se2.cpp │ ├── finite_differences_grid_se2.cpp │ ├── finite_differences_variable_grid_se2.cpp │ ├── full_discretization_grid_base_se2.cpp │ ├── min_time_via_points_cost.cpp │ ├── quadratic_cost_se2.cpp │ └── stage_inequality_se2.cpp │ ├── test_mpc_optim_node.cpp │ └── utils │ ├── conversion.cpp │ ├── publisher.cpp │ └── time_series_se2.cpp ├── mpc_local_planner_examples ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg │ ├── amcl_params.yaml │ ├── carlike │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ └── mpc_local_planner_params.yaml │ ├── diff_drive │ │ ├── costmap_common_params.yaml │ │ ├── costmap_converter_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ ├── mpc_local_planner_params_minimum_time.yaml │ │ └── mpc_local_planner_params_quadratic_form.yaml │ ├── rviz_navigation.rviz │ └── rviz_navigation_cc.rviz ├── launch │ ├── carlike_minimum_time.launch │ ├── diff_drive_minimum_time.launch │ ├── diff_drive_minimum_time_costmap_conversion.launch │ └── diff_drive_quadratic_form.launch ├── maps │ ├── corridor.png │ ├── corridor.yaml │ ├── empty_box.png │ ├── empty_box.yaml │ ├── maze.png │ └── maze.yaml ├── package.xml └── stage │ ├── corridor.world │ ├── empty_box.world │ ├── maze_carlike.world │ ├── maze_diff_drive.world │ ├── maze_omnidir.world │ └── robots │ ├── carlike_robot.inc │ ├── diff_drive_robot.inc │ ├── diff_drive_robot_gps.inc │ ├── obstacle.inc │ └── omnidir_robot.inc └── mpc_local_planner_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg ├── OptimalControlResult.msg └── StateFeedback.msg └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Build files 2 | build 3 | 4 | # Autosave files 5 | *.autosave 6 | 7 | # MacOS temporary files 8 | .DS_Store 9 | 10 | # QT project files 11 | *.txt.user* 12 | 13 | # kDevelop project files 14 | .kdev4 15 | *.kdev4 16 | 17 | # Version control 18 | .svn 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | mpc_local_planner ROS Package 2 | ============================= 3 | 4 | The mpc_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack. 5 | It provides a generic and versatile model predictive control implementation with minimum-time and quadratic-form receding-horizon configurations. 6 | 7 | For custom build instructions (e.g. compilation with other third-party solvers) see [this wiki](https://github.com/rst-tu-dortmund/mpc_local_planner/wiki). 8 | 9 | Refer to http://wiki.ros.org/mpc_local_planner for more general information and tutorials. 10 | 11 | Build status: 12 | - ROS Melodic (*melodic-devel*): [![Melodic Status](http://build.ros.org/buildStatus/icon?job=Mdev__mpc_local_planner__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__mpc_local_planner__ubuntu_bionic_amd64/) 13 | 14 | ## Authors 15 | 16 | * Christoph Rösmann 17 | 18 | ## Citing the Software 19 | 20 | *Since a lot of time and effort has gone into the development, please cite at least one of the following publications if you are using the software for published work.* 21 | 22 | **Main Paper and Approach** 23 | 24 | - C. Rösmann, A. Makarow, and T. Bertram: Online Motion Planning based on Nonlinear Model Predictive Control with Non-Euclidean Rotation Groups, [arXiv:2006.03534 '[cs.RO]'](https://arxiv.org/abs/2006.03534), June 2020. 25 | 26 | **Standard MPC and Hypergraph** 27 | 28 | - C. Rösmann, M. Krämer, A. Makarow, F. Hoffmann and T. Bertram: Exploiting Sparse Structures in Nonlinear Model Predictive Control with Hypergraphs, IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), New Zealand, July 2018. 29 | 30 | **Time-Optimal MPC and Hypergraph** 31 | 32 | - C. Rösmann: [Time-optimal nonlinear model predictive control, Direct transcription methods with variable discretization and structural sparsity exploitation](http://dx.doi.org/10.17877/DE290R-20283). Dissertation, Technische Universität Dortmund, Oct. 2019. 33 | - C. Rösmann, F. Hoffmann und T. Bertram: Timed-Elastic-Bands for Time-Optimal Point-to-Point Nonlinear Model Predictive Control, European Control Conference (ECC), Austria, July 2015. 34 | 35 | 36 | Buy Me A Coffee 37 | 38 | ## License 39 | 40 | Copyright (c) 2020, Christoph Rösmann, All rights reserved. 41 | 42 | This program is free software: you can redistribute it and/or modify 43 | it under the terms of the GNU General Public License as published by 44 | the Free Software Foundation, either version 3 of the License, or 45 | (at your option) any later version. 46 | 47 | This program is distributed in the hope that it will be useful, 48 | but WITHOUT ANY WARRANTY; without even the implied warranty of 49 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 50 | GNU General Public License for more details. 51 | 52 | You should have received a copy of the GNU General Public License 53 | along with this program. If not, see . 54 | 55 | The package depends on third-party packages: 56 | - *control_box_rst*, GPLv3, https://github.com/rst-tu-dortmund/control_box_rst 57 | Please check also the dependencies of control_box_rst (not all modules 58 | are included at the moment) 59 | - *Ipopt*, EPL 1.0, https://github.com/coin-or/Ipopt 60 | 61 | 62 | It depends on other ROS packages, which are listed in the package.xml. They are licensed under BSD resp. Apache 2.0. 63 | 64 | 65 | ## Requirements 66 | 67 | Install dependencies (listed in the *package.xml* and *CMakeLists.txt* file) using *rosdep*: 68 | 69 | rosdep install mpc_local_planner 70 | -------------------------------------------------------------------------------- /mpc_local_planner/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | # Doc: https://clang.llvm.org/docs/ClangFormatStyleOptions.html 5 | AccessModifierOffset: -3 6 | AlignAfterOpenBracket: Align 7 | AlignConsecutiveAssignments: true 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlinesLeft: true 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllParametersOfDeclarationOnNextLine: true 13 | AllowShortBlocksOnASingleLine: false 14 | AllowShortCaseLabelsOnASingleLine: false 15 | AllowShortFunctionsOnASingleLine: All 16 | AllowShortIfStatementsOnASingleLine: true 17 | AllowShortLoopsOnASingleLine: true 18 | AlwaysBreakAfterDefinitionReturnType: None 19 | AlwaysBreakAfterReturnType: None 20 | AlwaysBreakBeforeMultilineStrings: true 21 | AlwaysBreakTemplateDeclarations: true 22 | BinPackArguments: true 23 | BinPackParameters: true 24 | BraceWrapping: 25 | AfterClass: true 26 | AfterControlStatement: true 27 | AfterEnum: false 28 | AfterFunction: true 29 | AfterNamespace: false 30 | AfterObjCDeclaration: false 31 | AfterStruct: true 32 | AfterUnion: true 33 | BeforeCatch: true 34 | BeforeElse: true 35 | IndentBraces: false 36 | BreakBeforeBinaryOperators: None 37 | # Custom: break according to "BraceWrapping" 38 | BreakBeforeBraces: Custom 39 | BreakBeforeTernaryOperators: true 40 | BreakConstructorInitializersBeforeComma: false 41 | ColumnLimit: 150 42 | CommentPragmas: '^ IWYU pragma:' 43 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 44 | ConstructorInitializerIndentWidth: 4 45 | ContinuationIndentWidth: 4 46 | Cpp11BracedListStyle: true 47 | DerivePointerAlignment: false 48 | DisableFormat: false 49 | ExperimentalAutoDetectBinPacking: false 50 | #FixNamespaceComments: true 51 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 52 | IncludeCategories: 53 | - Regex: '^<.*\.h>' 54 | Priority: 1 55 | - Regex: '^<.*' 56 | Priority: 2 57 | - Regex: '.*' 58 | Priority: 3 59 | IndentCaseLabels: true 60 | IndentWidth: 4 61 | IndentWrappedFunctionNames: false 62 | KeepEmptyLinesAtTheStartOfBlocks: true 63 | MacroBlockBegin: '' 64 | MacroBlockEnd: '' 65 | MaxEmptyLinesToKeep: 1 66 | NamespaceIndentation: None 67 | ObjCBlockIndentWidth: 2 68 | ObjCSpaceAfterProperty: false 69 | ObjCSpaceBeforeProtocolList: false 70 | PenaltyBreakBeforeFirstCallParameter: 1 71 | PenaltyBreakComment: 300 72 | PenaltyBreakFirstLessLess: 120 73 | PenaltyBreakString: 1000 74 | PenaltyExcessCharacter: 1000000 75 | PenaltyReturnTypeOnItsOwnLine: 200 76 | PointerAlignment: Left 77 | ReflowComments: true 78 | SortIncludes: true 79 | SpaceAfterCStyleCast: false 80 | #SpaceAfterTemplateKeyword: false 81 | SpaceBeforeAssignmentOperators: true 82 | SpaceBeforeParens: ControlStatements 83 | SpaceInEmptyParentheses: false 84 | SpacesBeforeTrailingComments: 2 85 | SpacesInAngles: false 86 | SpacesInContainerLiterals: true 87 | SpacesInCStyleCastParentheses: false 88 | SpacesInParentheses: false 89 | SpacesInSquareBrackets: false 90 | Standard: Cpp11 91 | #Standard: Auto 92 | TabWidth: 8 93 | UseTab: Never 94 | ... 95 | 96 | -------------------------------------------------------------------------------- /mpc_local_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mpc_local_planner 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.3 (2020-06-09) 6 | ------------------ 7 | * Added feasibility check with costmap robot model 8 | * Changed obstacle parameters `cutoff_factor` and `force_inclusion_factor` to `cutoff_dist` and `force_inclusion_dist` 9 | * Added check for obstacle pointer validity in StageInequalitySE2 10 | * Dynamic obstacles: the inequality constraints now use the actual time parameter rather than the time from the previous optimization 11 | * Added hybrid cost (time and control effort) 12 | * Added kinematic bicycle model with velocity input 13 | * Grid: The time difference is now initialized to dt_ref for reference trajectory caching 14 | * Fixed wrong start orientation in point-to-point grid initialization 15 | * Fixed wrong angular computation in midpoint differences 16 | * Added missing install files (thanks to marbosjo) 17 | * Changed minimum CMake version to 3.1 18 | * Contributors: Christoph Rösmann, marbosjo 19 | 20 | 0.0.2 (2020-03-12) 21 | ------------------ 22 | * Added option to inherit the robot footprint model from the costmap_2d footprint by setting footprint type to `costmap_2d` (thanks to Alexander Xydes). 23 | Note that using a polygon as a robot footprint model is computationally more expensive than using two circle or line models. 24 | * Fixed issues in fd discretization grid and graph consistency 25 | * Fixed initialization of linear velocity output commands in simple car and unicycle models (thanks to Thomas Denewiler). 26 | * Fixed wrong parameter namespaces for some grid parameters 27 | * Replaced non-ASCII characters in python script 28 | * Added find_package script for osqp in case control_box_rst is linked against it 29 | * Contributors: Christoph Rösmann, Alexander Xydes, Thomas Denewiler 30 | 31 | 0.0.1 (2020-02-20) 32 | ------------------ 33 | * First release 34 | * Contributors: Christoph Rösmann 35 | -------------------------------------------------------------------------------- /mpc_local_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(mpc_local_planner) 3 | 4 | # Set to Release in order to speed up the program significantly 5 | set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ## Find catkin macros and libraries 11 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 12 | ## is used, also find other catkin packages 13 | find_package(catkin REQUIRED COMPONENTS 14 | base_local_planner 15 | costmap_2d 16 | costmap_converter 17 | dynamic_reconfigure 18 | roscpp 19 | geometry_msgs 20 | interactive_markers 21 | nav_core 22 | nav_msgs 23 | mbf_costmap_core 24 | mbf_msgs 25 | mpc_local_planner_msgs 26 | pluginlib 27 | std_msgs 28 | teb_local_planner 29 | tf2 30 | tf2_eigen 31 | tf2_geometry_msgs 32 | tf2_ros 33 | visualization_msgs 34 | ) 35 | 36 | # Find control_box_rst 37 | # TODO: Currently, we use QUIET rather than REQUIRED to avoid warnings 38 | # as some components depend on each other (circularly) 39 | # which is currently not handled well by the exported cmake config 40 | find_package(control_box_rst QUIET COMPONENTS 41 | core 42 | communication 43 | controllers 44 | numerics 45 | systems 46 | optimization 47 | optimal_control 48 | systems 49 | PATH_SUFFIXES share/control_box_rst control_box_rst 50 | PATHS ${CMAKE_BINARY_DIR} /usr/local 51 | ) 52 | 53 | # Eigen3 FindScript Backward compatibility (ubuntu saucy) 54 | # Since FindEigen.cmake is deprecated starting from jade. 55 | # if (EXISTS "FindEigen3.cmake") 56 | find_package(Eigen3 REQUIRED) 57 | 58 | find_package(osqp QUIET) 59 | 60 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 61 | 62 | set(EXTERNAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 63 | 64 | ## Uncomment this if the package has a setup.py. This macro ensures 65 | ## modules and global scripts declared therein get installed 66 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 67 | # catkin_python_setup() 68 | 69 | 70 | ################################################ 71 | ## Declare ROS messages, services and actions ## 72 | ################################################ 73 | 74 | ## To declare and build messages, services or actions from within this 75 | ## package, follow these steps: 76 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 77 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 78 | ## * In the file package.xml: 79 | ## * add a build_depend tag for "message_generation" 80 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 81 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 82 | ## but can be declared for certainty nonetheless: 83 | ## * add a exec_depend tag for "message_runtime" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "message_generation" and every package in MSG_DEP_SET to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * add "message_runtime" and every package in MSG_DEP_SET to 88 | ## catkin_package(CATKIN_DEPENDS ...) 89 | ## * uncomment the add_*_files sections below as needed 90 | ## and list every .msg/.srv/.action file to be processed 91 | ## * uncomment the generate_messages entry below 92 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 93 | 94 | ## Generate messages in the 'msg' folder 95 | # add_message_files( 96 | # FILES 97 | # Message1.msg 98 | # Message2.msg 99 | # ) 100 | 101 | ## Generate services in the 'srv' folder 102 | # add_service_files( 103 | # FILES 104 | # Service1.srv 105 | # Service2.srv 106 | # ) 107 | 108 | ## Generate actions in the 'action' folder 109 | # add_action_files( 110 | # FILES 111 | # Action1.action 112 | # Action2.action 113 | # ) 114 | 115 | ## Generate added messages and services with any dependencies listed here 116 | # generate_messages( 117 | # DEPENDENCIES 118 | # std_msgs # Or other packages containing msgs 119 | # ) 120 | 121 | ################################################ 122 | ## Declare ROS dynamic reconfigure parameters ## 123 | ################################################ 124 | 125 | ## To declare and build dynamic reconfigure parameters within this 126 | ## package, follow these steps: 127 | ## * In the file package.xml: 128 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 129 | ## * In this file (CMakeLists.txt): 130 | ## * add "dynamic_reconfigure" to 131 | ## find_package(catkin REQUIRED COMPONENTS ...) 132 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 133 | ## and list every .cfg file to be processed 134 | 135 | #add dynamic reconfigure api 136 | #find_package(catkin REQUIRED dynamic_reconfigure) 137 | generate_dynamic_reconfigure_options( 138 | cfg/mpc_collision.cfg 139 | cfg/mpc_controller.cfg 140 | cfg/mpc_footprint.cfg 141 | ) 142 | 143 | ################################### 144 | ## catkin specific configuration ## 145 | ################################### 146 | ## The catkin_package macro generates cmake config files for your package 147 | ## Declare things to be passed to dependent projects 148 | ## INCLUDE_DIRS: uncomment this if your package contains header files 149 | ## LIBRARIES: libraries you create in this project that dependent projects also need 150 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 151 | ## DEPENDS: system dependencies of this project that dependent projects also need 152 | catkin_package( 153 | INCLUDE_DIRS include ${EXTERNAL_INCLUDE_DIRS} 154 | LIBRARIES mpc_local_planner_utils mpc_local_planner_optimal_control mpc_local_planner 155 | CATKIN_DEPENDS roscpp mpc_local_planner_msgs control_box_rst teb_local_planner dynamic_reconfigure 156 | # DEPENDS 157 | ) 158 | 159 | ########### 160 | ## Build ## 161 | ########### 162 | 163 | ## Specify additional locations of header files 164 | ## Your package locations should be listed before other locations 165 | include_directories( 166 | include 167 | ${EXTERNAL_INCLUDE_DIRS} 168 | ${catkin_INCLUDE_DIRS} 169 | ) 170 | 171 | ## Declare a C++ library 172 | add_library(mpc_local_planner_utils 173 | src/utils/publisher.cpp 174 | src/utils/conversion.cpp 175 | src/utils/time_series_se2.cpp 176 | ) 177 | 178 | add_library(mpc_local_planner_optimal_control 179 | src/optimal_control/full_discretization_grid_base_se2.cpp 180 | src/optimal_control/finite_differences_grid_se2.cpp 181 | src/optimal_control/finite_differences_variable_grid_se2.cpp 182 | src/optimal_control/stage_inequality_se2.cpp 183 | src/optimal_control/quadratic_cost_se2.cpp 184 | src/optimal_control/final_state_conditions_se2.cpp 185 | src/optimal_control/min_time_via_points_cost.cpp 186 | ) 187 | 188 | add_library(mpc_local_planner 189 | src/controller.cpp 190 | src/mpc_local_planner_ros.cpp 191 | ) 192 | 193 | # Dynamic reconfigure: make sure configure headers are built before any node using them 194 | add_dependencies(mpc_local_planner ${PROJECT_NAME}_gencfg) 195 | 196 | ## Declare a C++ executable 197 | ## With catkin_make all packages are built within a single CMake context 198 | ## The recommended prefix ensures that target names across packages don't collide 199 | add_executable(test_mpc_optim_node src/test_mpc_optim_node.cpp) 200 | 201 | ## Rename C++ executable without prefix 202 | ## The above recommended prefix causes long target names, the following renames the 203 | ## target back to the shorter version for ease of user use 204 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 205 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 206 | 207 | ## Add cmake target dependencies of the executable 208 | ## same as for the library above 209 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 210 | 211 | add_dependencies(mpc_local_planner_utils corbo_core corbo_systems) 212 | target_link_libraries(mpc_local_planner_utils 213 | corbo_core 214 | corbo_systems 215 | ) 216 | 217 | target_link_libraries(mpc_local_planner_optimal_control 218 | corbo_optimal_control 219 | ) 220 | 221 | target_link_libraries(mpc_local_planner 222 | mpc_local_planner_utils 223 | mpc_local_planner_optimal_control 224 | corbo_controllers 225 | ${catkin_LIBRARIES} 226 | ) 227 | 228 | target_link_libraries(test_mpc_optim_node 229 | mpc_local_planner 230 | ${catkin_LIBRARIES} 231 | ) 232 | 233 | ############# 234 | ## Install ## 235 | ############# 236 | 237 | # all install targets should use catkin DESTINATION variables 238 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 239 | 240 | ## Mark executable scripts (Python etc.) for installation 241 | ## in contrast to setup.py, you can choose the destination 242 | install(PROGRAMS 243 | scripts/plot_optimal_control_results.py 244 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 245 | ) 246 | 247 | ## Mark executables and/or libraries for installation 248 | install(TARGETS mpc_local_planner mpc_local_planner_utils mpc_local_planner_optimal_control 249 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 250 | ) 251 | install(TARGETS test_mpc_optim_node 252 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 253 | ) 254 | 255 | ## Mark cpp header files for installation 256 | install(DIRECTORY include/${PROJECT_NAME}/ 257 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 258 | #FILES_MATCHING PATTERN "*.h" 259 | PATTERN ".svn" EXCLUDE 260 | ) 261 | 262 | ## Mark other files for installation (e.g. launch and bag files, etc.) 263 | install(FILES 264 | mpc_local_planner_plugin.xml 265 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 266 | ) 267 | 268 | install(DIRECTORY 269 | launch cfg scripts 270 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 271 | PATTERN ".svn" EXCLUDE 272 | ) 273 | 274 | ############# 275 | ## Testing ## 276 | ############# 277 | 278 | ## Add gtest based cpp test target and link libraries 279 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_local_planner.cpp) 280 | # if(TARGET ${PROJECT_NAME}-test) 281 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 282 | # endif() 283 | 284 | ## Add folders to be run by python nosetests 285 | # catkin_add_nosetests(test) 286 | -------------------------------------------------------------------------------- /mpc_local_planner/cfg/mpc_collision.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import * 4 | 5 | gen = ParameterGenerator() 6 | 7 | # This unusual line allows to reuse existing parameter definitions 8 | # that concern all localplanners 9 | #add_generic_localplanner_params(gen) 10 | 11 | # For integers and doubles: 12 | # Name Type Reconfiguration level 13 | # Description 14 | # Default Min Max 15 | 16 | # Collision avoidance 17 | 18 | grp_collision = gen.add_group("Collision avoidance", type="tab") 19 | 20 | grp_collision.add("include_costmap_obstacles", bool_t, 0, "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", True) 21 | 22 | grp_collision.add("costmap_obstacles_behind_robot_dist", double_t, 0, "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", 1.5, 0, 10) 23 | 24 | grp_collision.add("collision_check_min_resolution_angular", double_t, 0, "", 3.1415, -3.1415, 3.1415) 25 | grp_collision.add("collision_check_no_poses", int_t, 0, "", -1, -1, 100) 26 | 27 | 28 | exit(gen.generate("mpc_local_planner", "mpc_local_planner", "CollisionReconfigure")) -------------------------------------------------------------------------------- /mpc_local_planner/cfg/mpc_controller.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import * 4 | 5 | gen = ParameterGenerator() 6 | 7 | # This unusual line allows to reuse existing parameter definitions 8 | # that concern all localplanners 9 | #add_generic_localplanner_params(gen) 10 | 11 | # For integers and doubles: 12 | # Name Type Reconfiguration level 13 | # Description 14 | # Default Min Max 15 | 16 | grp_controller = gen.add_group("Controller", type="tab") 17 | 18 | # Controller 19 | 20 | grp_controller.add("xy_goal_tolerance", double_t, 0, "Allowed final euclidean distance to the goal position", 0.2, 0, 1) 21 | 22 | grp_controller.add("yaw_goal_tolerance", double_t, 0, "Allowed final orientation error to the goal orientation", 0.1, 0, 1) 23 | 24 | grp_controller.add("global_plan_overwrite_orientation", bool_t, 0, "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", True) 25 | 26 | grp_controller.add("global_plan_prune_distance", double_t, 0, "", 1.0, 0, 10) 27 | 28 | grp_controller.add("max_global_plan_lookahead_dist", double_t, 0, "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", 1.5, 0, 10) 29 | 30 | grp_controller.add("global_plan_viapoint_sep", double_t, 0, "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", -1, -1, 10) 31 | 32 | 33 | exit(gen.generate("mpc_local_planner", "mpc_local_planner", "ControllerReconfigure")) -------------------------------------------------------------------------------- /mpc_local_planner/cfg/mpc_footprint.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import * 4 | 5 | gen = ParameterGenerator() 6 | 7 | # This unusual line allows to reuse existing parameter definitions 8 | # that concern all localplanners 9 | #add_generic_localplanner_params(gen) 10 | 11 | # For integers and doubles: 12 | # Name Type Reconfiguration level 13 | # Description 14 | # Default Min Max 15 | 16 | # Footprint model 17 | 18 | grp_footprint = gen.add_group("Footprint", type="tab") 19 | 20 | grp_footprint.add("is_footprint_dynamic", bool_t, 0, "If true, updated the footprint before checking trajectory feasibility", False) 21 | 22 | exit(gen.generate("mpc_local_planner", "mpc_local_planner", "FootprintReconfigure")) -------------------------------------------------------------------------------- /mpc_local_planner/cfg/rviz_test_mpc_optim.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Path1 9 | Splitter Ratio: 0.5 10 | Tree Height: 562 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 0.100000001 54 | Cell Size: 0.100000001 55 | Class: rviz/Grid 56 | Color: 160; 160; 164 57 | Enabled: true 58 | Line Style: 59 | Line Width: 0.0299999993 60 | Value: Lines 61 | Name: Fine Grid 62 | Normal Cell Count: 0 63 | Offset: 64 | X: 0 65 | Y: 0 66 | Z: 0 67 | Plane: XY 68 | Plane Cell Count: 100 69 | Reference Frame: 70 | Value: true 71 | - Alpha: 1 72 | Buffer Length: 1 73 | Class: rviz/Path 74 | Color: 25; 255; 0 75 | Enabled: true 76 | Head Diameter: 0.119999997 77 | Head Length: 0.200000003 78 | Length: 0.300000012 79 | Line Style: Lines 80 | Line Width: 0.0299999993 81 | Name: Path 82 | Offset: 83 | X: 0 84 | Y: 0 85 | Z: 0 86 | Pose Color: 255; 0; 0 87 | Pose Style: Arrows 88 | Radius: 0.0299999993 89 | Shaft Diameter: 0.0199999996 90 | Shaft Length: 0.100000001 91 | Topic: /test_mpc_optim_node/local_plan 92 | Unreliable: false 93 | Value: true 94 | - Alpha: 1 95 | Arrow Length: 0.300000012 96 | Axes Length: 0.300000012 97 | Axes Radius: 0.00999999978 98 | Class: rviz/PoseArray 99 | Color: 255; 25; 0 100 | Enabled: true 101 | Head Length: 0.0700000003 102 | Head Radius: 0.0299999993 103 | Name: PoseArray 104 | Shaft Length: 0.230000004 105 | Shaft Radius: 0.00999999978 106 | Shape: Arrow (Flat) 107 | Topic: /test_optim_node/teb_poses 108 | Unreliable: false 109 | Value: true 110 | - Class: rviz/Marker 111 | Enabled: true 112 | Marker Topic: /test_mpc_optim_node/mpc_markers 113 | Name: Marker 114 | Namespaces: 115 | PointObstacles: true 116 | Queue Size: 100 117 | Value: true 118 | - Class: rviz/InteractiveMarkers 119 | Enable Transparency: true 120 | Enabled: true 121 | Name: InteractiveMarkers 122 | Show Axes: false 123 | Show Descriptions: true 124 | Show Visual Aids: false 125 | Update Topic: /marker_obstacles/update 126 | Value: true 127 | Enabled: true 128 | Global Options: 129 | Background Color: 48; 48; 48 130 | Default Light: true 131 | Fixed Frame: map 132 | Frame Rate: 30 133 | Name: root 134 | Tools: 135 | - Class: rviz/Interact 136 | Hide Inactive Objects: true 137 | - Class: rviz/MoveCamera 138 | - Class: rviz/Select 139 | - Class: rviz/FocusCamera 140 | - Class: rviz/Measure 141 | - Class: rviz/SetInitialPose 142 | Topic: /initialpose 143 | - Class: rviz/SetGoal 144 | Topic: /move_base_simple/goal 145 | - Class: rviz/PublishPoint 146 | Single click: true 147 | Topic: /clicked_point 148 | Value: true 149 | Views: 150 | Current: 151 | Class: rviz/Orbit 152 | Distance: 9.30938148 153 | Enable Stereo Rendering: 154 | Stereo Eye Separation: 0.0599999987 155 | Stereo Focal Distance: 1 156 | Swap Stereo Eyes: false 157 | Value: false 158 | Focal Point: 159 | X: 2.73932815 160 | Y: 0.504589677 161 | Z: 0.0263484083 162 | Focal Shape Fixed Size: true 163 | Focal Shape Size: 0.0500000007 164 | Invert Z Axis: false 165 | Name: Current View 166 | Near Clip Distance: 0.00999999978 167 | Pitch: 1.56479633 168 | Target Frame: 169 | Value: Orbit (rviz) 170 | Yaw: 4.70042801 171 | Saved: ~ 172 | Window Geometry: 173 | Displays: 174 | collapsed: false 175 | Height: 846 176 | Hide Left Dock: false 177 | Hide Right Dock: true 178 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 179 | Selection: 180 | collapsed: false 181 | Time: 182 | collapsed: false 183 | Tool Properties: 184 | collapsed: false 185 | Views: 186 | collapsed: true 187 | Width: 1354 188 | X: 137 189 | Y: 50 190 | -------------------------------------------------------------------------------- /mpc_local_planner/cfg/test_mpc_optim_node.yaml: -------------------------------------------------------------------------------- 1 | ## Robot settings 2 | robot: 3 | type: "unicycle" 4 | unicycle: 5 | max_vel_x: 0.4 6 | max_vel_x_backwards: 0.2 7 | max_vel_theta: 0.3 8 | acc_lim_x: 0.0 # deactive bounds with zero 9 | dec_lim_x: 0.0 # deactive bounds with zero 10 | acc_lim_theta: 0.0 # deactivate bounds with zero 11 | simple_car: 12 | wheelbase: 0.4 13 | front_wheel_driving: False 14 | max_vel_x: 0.4 15 | max_vel_x_backwards: 0.2 16 | max_steering_angle: 1.4 17 | acc_lim_x: 0.0 # deactive bounds with zero 18 | dec_lim_x: 0.0 # deactive bounds with zero 19 | max_steering_rate: 0.5 # deactive bounds with zero 20 | kinematic_bicycle_vel_input: 21 | length_rear: 1.0 22 | length_front: 1.0 23 | max_vel_x: 0.4 24 | max_vel_x_backwards: 0.2 25 | max_steering_angle: 1.4 26 | acc_lim_x: 0.0 # deactive bounds with zero 27 | dec_lim_x: 0.0 # deactive bounds with zero 28 | max_steering_rate: 0.5 # deactive bounds with zero 29 | 30 | ## Footprint model for collision avoidance 31 | footprint_model: 32 | type: "point" 33 | 34 | collision_avoidance: 35 | min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model 36 | enable_dynamic_obstacles: False 37 | force_inclusion_dist: 0.5 38 | cutoff_dist: 2.5 39 | collision_check_no_poses: 5 40 | 41 | ## Planning grid 42 | grid: 43 | type: "fd_grid" 44 | grid_size_ref: 20 45 | dt_ref: 0.3 46 | xf_fixed: [True, True, True] # E.g., set last one to False in order to unfix the final orientation 47 | warm_start: True 48 | collocation_method: "forward_differences" 49 | cost_integration_method: "left_sum" 50 | variable_grid: 51 | enable: True 52 | min_dt: 0.0; 53 | max_dt: 10.0; 54 | grid_adaptation: 55 | enable: True 56 | dt_hyst_ratio: 0.1 57 | min_grid_size: 2 58 | max_grid_size: 50 59 | 60 | 61 | ## Planning options 62 | planning: 63 | objective: 64 | type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly 65 | quadratic_form: 66 | state_weights: [2.0, 2.0, 2.0] 67 | control_weights: [1.0, 1.0] 68 | integral_form: False 69 | hybrid_cost_minimum_time: False 70 | minimum_time_via_points: 71 | position_weight: 8.0 72 | orientation_weight: 0.0 73 | via_points_ordered: False 74 | terminal_cost: 75 | type: "quadratic" # can be "none" 76 | quadratic: 77 | final_state_weights: [2.0, 2.0, 2.0] 78 | terminal_constraint: 79 | type: "none" # can be "none" 80 | l2_ball: 81 | weight_matrix: [1.0, 1.0, 1.0] 82 | radius: 5 83 | 84 | 85 | ## Controller options 86 | controller: 87 | outer_ocp_iterations: 1 88 | xy_goal_tolerance: 0.2 89 | yaw_goal_tolerance: 0.1 90 | force_reinit_new_goal_dist: 1.0 91 | force_reinit_new_goal_angular: 1.57 92 | force_reinit_num_steps: 0 93 | global_plan_overwrite_orientation: True 94 | global_plan_viapoint_sep: -1 95 | allow_init_with_backward_motion: True 96 | publish_ocp_results: True 97 | print_cpu_time: False 98 | 99 | ## Solver settings 100 | solver: 101 | type: "ipopt" 102 | ipopt: 103 | iterations: 100 104 | max_cpu_time: -1.0 105 | ipopt_numeric_options: 106 | tol: 1e-4 107 | ipopt_string_options: 108 | linear_solver: "mumps" 109 | hessian_approximation: "limited-memory" # exact or limited-memory 110 | ipopt_integer_options: 111 | print_level: 2 112 | lsq_lm: 113 | iterations: 10 114 | weight_init_eq: 2 115 | weight_init_ineq: 2 116 | weight_init_bounds: 2 117 | weight_adapt_factor_eq: 1.5 118 | weight_adapt_factor_ineq: 1.5 119 | weight_adapt_factor_bounds: 1.5 120 | weight_adapt_max_eq: 500 121 | weight_adapt_max_ineq: 500 122 | weight_adapt_max_bounds: 500 123 | 124 | 125 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef CONTROLLER_H_ 24 | #define CONTROLLER_H_ 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | namespace mpc_local_planner { 45 | 46 | /** 47 | * @brief MPC controller for mobile robots 48 | * 49 | * @ingroup controllers 50 | * 51 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 52 | */ 53 | class Controller : public corbo::PredictiveController 54 | { 55 | public: 56 | using Ptr = std::shared_ptr; 57 | using PoseSE2 = teb_local_planner::PoseSE2; 58 | 59 | Controller() = default; 60 | 61 | bool configure(ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles, teb_local_planner::RobotFootprintModelPtr robot_model, 62 | const std::vector& via_points); 63 | bool step(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist& vel, double dt, ros::Time t, corbo::TimeSeries::Ptr u_seq, 64 | corbo::TimeSeries::Ptr x_seq); 65 | 66 | bool step(const std::vector& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t, 67 | corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq); 68 | 69 | // implements interface method 70 | corbo::ControllerInterface::Ptr getInstance() const override { return std::make_shared(); } 71 | static corbo::ControllerInterface::Ptr getInstanceStatic() { return std::make_shared(); } 72 | 73 | void setOptimalControlProblem(corbo::OptimalControlProblemInterface::Ptr ocp) = delete; 74 | 75 | RobotDynamicsInterface::Ptr getRobotDynamics() { return _dynamics; } 76 | StageInequalitySE2::Ptr getInequalityConstraint() { return _inequality_constraint; } 77 | 78 | void stateFeedbackCallback(const mpc_local_planner_msgs::StateFeedback::ConstPtr& msg); 79 | 80 | void publishOptimalControlResult(); 81 | 82 | void setInitialPlanEstimateOrientation(bool estimate) { _initial_plan_estimate_orientation = estimate; } 83 | 84 | /** 85 | * @brief Check whether the planned trajectory is feasible or not. 86 | * 87 | * This method currently checks only that the trajectory, or a part of the trajectory is collision free. 88 | * Obstacles are here represented as costmap instead of the internal ObstacleContainer. 89 | * @param costmap_model Pointer to the costmap model 90 | * @param footprint_spec The specification of the footprint of the robot in world coordinates 91 | * @param inscribed_radius The radius of the inscribed circle of the robot 92 | * @param circumscribed_radius The radius of the circumscribed circle of the robot 93 | * @param min_resolution_collision_check_angular Min angular resolution during the costmap collision check: 94 | * if not respected intermediate samples are added. [rad] 95 | * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked. 96 | * @return \c true, if the robot footprint along the first part of the trajectory intersects with 97 | * any obstacle in the costmap, \c false otherwise. 98 | */ 99 | virtual bool isPoseTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector& footprint_spec, 100 | double inscribed_radius = 0.0, double circumscribed_radius = 0.0, 101 | double min_resolution_collision_check_angular = M_PI, int look_ahead_idx = -1); 102 | 103 | // implements interface method 104 | void reset() override; 105 | 106 | protected: 107 | corbo::DiscretizationGridInterface::Ptr configureGrid(const ros::NodeHandle& nh); 108 | RobotDynamicsInterface::Ptr configureRobotDynamics(const ros::NodeHandle& nh); 109 | corbo::NlpSolverInterface::Ptr configureSolver(const ros::NodeHandle& nh); 110 | corbo::StructuredOptimalControlProblem::Ptr configureOcp(const ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles, 111 | teb_local_planner::RobotFootprintModelPtr robot_model, 112 | const std::vector& via_points); 113 | 114 | bool generateInitialStateTrajectory(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, 115 | const std::vector& initial_plan, bool backward); 116 | 117 | std::string _robot_type; 118 | corbo::DiscretizationGridInterface::Ptr _grid; 119 | RobotDynamicsInterface::Ptr _dynamics; 120 | corbo::NlpSolverInterface::Ptr _solver; 121 | StageInequalitySE2::Ptr _inequality_constraint; 122 | corbo::StructuredOptimalControlProblem::Ptr _structured_ocp; 123 | 124 | ros::Publisher _ocp_result_pub; 125 | bool _ocp_successful = false; 126 | std::size_t _ocp_seq = 0; 127 | bool _publish_ocp_results = false; 128 | bool _print_cpu_time = false; 129 | 130 | bool _prefer_x_feedback = false; // prefer state feedback over odometry feedback 131 | ros::Subscriber _x_feedback_sub; 132 | std::mutex _x_feedback_mutex; 133 | ros::Time _recent_x_time; 134 | Eigen::VectorXd _recent_x_feedback; 135 | 136 | teb_local_planner::PoseSE2 _last_goal; 137 | double _force_reinit_new_goal_dist = 1.0; 138 | double _force_reinit_new_goal_angular = 0.5 * M_PI; 139 | corbo::DiscreteTimeReferenceTrajectory _x_seq_init; 140 | bool _initial_plan_estimate_orientation = true; 141 | bool _guess_backwards_motion = true; 142 | int _force_reinit_num_steps = 0; 143 | }; 144 | 145 | } // namespace mpc_local_planner 146 | 147 | #endif // CONTROLLER_H_ 148 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/optimal_control/fd_collocation_se2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef FINITE_DIFFERENCES_COLLOCATION_H_ 24 | #define FINITE_DIFFERENCES_COLLOCATION_H_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | namespace mpc_local_planner { 34 | 35 | /** 36 | * @brief Collocation via forward differences (specialized for SE2) 37 | * 38 | * Forward differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner: 39 | * \f[ 40 | * \frac{x_{k+1} - x_k}{\delta T} = f(x_k, u_k) 41 | * \f] 42 | * 43 | * @see ForwardDiffCollocation FiniteDifferencesCollocationInterface 44 | * 45 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 46 | */ 47 | class ForwardDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface 48 | { 49 | public: 50 | // Implements interface method 51 | corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared(); } 52 | 53 | // Implements interface method 54 | void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, 55 | const corbo::SystemDynamicsInterface& system, Eigen::Ref error) override 56 | { 57 | assert(error.size() == x1.size()); 58 | assert(x1.size() >= 3); 59 | // assert(dt > 0 && "dt must be greater then zero!"); 60 | 61 | system.dynamics(x1, u1, error); 62 | error.head(2) -= (x2.head(2) - x1.head(2)) / dt; 63 | error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt; 64 | if (x1.size() > 3) 65 | { 66 | int n = x1.size() - 3; 67 | error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt; 68 | } 69 | } 70 | }; 71 | 72 | /** 73 | * @brief Collocation via midpoint differences (specialized for SE2) 74 | * 75 | * Midpoint differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner: 76 | * \f[ 77 | * \frac{x_{k+1} - x_k}{\delta T} = f(0.5*(x_k + x_{k+1}), u_k) 78 | * \f] 79 | * 80 | * @see FiniteDifferencesCollocationInterface 81 | * 82 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 83 | */ 84 | class MidpointDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface 85 | { 86 | public: 87 | // Implements interface method 88 | corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared(); } 89 | 90 | // Implements interface method 91 | void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, 92 | const corbo::SystemDynamicsInterface& system, Eigen::Ref error) override 93 | { 94 | assert(error.size() == x1.size()); 95 | assert(dt > 0 && "dt must be greater then zero!"); 96 | 97 | Eigen::VectorXd midpoint = 0.5 * (x1 + x2); 98 | // fix angular component 99 | midpoint.coeffRef(2) = interpolate_angle(x1.coeffRef(2), x2.coeffRef(2), 0.5); 100 | system.dynamics(midpoint, u1, error); 101 | error.head(2) -= (x2.head(2) - x1.head(2)) / dt; 102 | error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt; 103 | if (x1.size() > 3) 104 | { 105 | int n = x1.size() - 3; 106 | error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt; 107 | } 108 | } 109 | }; 110 | 111 | /** 112 | * @brief Collocation via Crank-Nicolson differences (specialized for SE2) 113 | * 114 | * Crank-Nicolson differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner: 115 | * \f[ 116 | * \frac{x_{k+1} - x_k}{\delta T} = 0.5 * ( f(x_k, u_k) + f(x_{k+1}, u_k)) 117 | * \f] 118 | * 119 | * @see FiniteDifferencesCollocationInterface 120 | * 121 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 122 | */ 123 | class CrankNicolsonDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface 124 | { 125 | public: 126 | // Implements interface method 127 | corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared(); } 128 | 129 | // Implements interface method 130 | void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, 131 | const corbo::SystemDynamicsInterface& system, Eigen::Ref error) override 132 | { 133 | assert(error.size() == x1.size()); 134 | assert(dt > 0 && "dt must be greater then zero!"); 135 | 136 | Eigen::VectorXd f1(x1.size()); 137 | system.dynamics(x1, u1, f1); 138 | system.dynamics(x2, u1, error); 139 | // error = (x2 - x1) / dt - 0.5 * (f1 + error); 140 | error.head(2) -= (x2.head(2) - x1.head(2)) / dt - 0.5 * (f1.head(2) + error.head(2)); 141 | error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt - 0.5 * (f1.coeffRef(2) + error.coeffRef(2)); 142 | if (x1.size() > 3) 143 | { 144 | int n = x1.size() - 3; 145 | error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt - 0.5 * (f1.tail(n) + error.tail(n)); 146 | } 147 | } 148 | }; 149 | 150 | } // namespace mpc_local_planner 151 | 152 | #endif // FINITE_DIFFERENCES_COLLOCATION_H_ 153 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/optimal_control/final_state_conditions_se2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef FINAL_STATE_CONDITIONS_SE2_H_ 24 | #define FINAL_STATE_CONDITIONS_SE2_H_ 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace mpc_local_planner { 33 | 34 | /** 35 | * @brief Quadratic final state cost (specialized for SE2) 36 | * 37 | * This class implements quadratic final costs 38 | * \f[ 39 | * J_f(x) = (x_{ref} - x)^T Q_f (x_{xref} - x) 40 | * \f] 41 | * However, this class ensures that the distance (x_{ref} - x) is 42 | * computed properly in SO(2) for the third state component. 43 | * 44 | * @see corbo::QuadraticFinalStateCost 45 | * 46 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 47 | */ 48 | class QuadraticFinalStateCostSE2 : public corbo::QuadraticFinalStateCost 49 | { 50 | public: 51 | using Ptr = std::shared_ptr; 52 | 53 | QuadraticFinalStateCostSE2() : corbo::QuadraticFinalStateCost() {} 54 | 55 | QuadraticFinalStateCostSE2(const Eigen::Ref& Qf, bool lsq_form = false) : corbo::QuadraticFinalStateCost(Qf, lsq_form) {} 56 | 57 | corbo::FinalStageCost::Ptr getInstance() const override { return std::make_shared(); } 58 | 59 | void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; 60 | }; 61 | 62 | /** 63 | * @brief Terminal ball constraint (specialized for SE2) 64 | * 65 | * This class ensures that the distance (x_{ref} - x) is 66 | * computed properly in SO(2) for the third state component. 67 | * 68 | * @see corbo::TerminalBall 69 | * 70 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 71 | */ 72 | class TerminalBallSE2 : public corbo::TerminalBall 73 | { 74 | public: 75 | using Ptr = std::shared_ptr; 76 | 77 | TerminalBallSE2() : corbo::TerminalBall() {} 78 | 79 | TerminalBallSE2(const Eigen::Ref& S, double gamma) : corbo::TerminalBall(S, gamma) {} 80 | 81 | corbo::FinalStageConstraint::Ptr getInstance() const override { return std::make_shared(); } 82 | 83 | void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; 84 | }; 85 | 86 | } // namespace mpc_local_planner 87 | 88 | #endif // FINAL_STATE_CONDITIONS_SE2_H_ 89 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_grid_se2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef FINITE_DIFFERENCES_GRID_H_ 24 | #define FINITE_DIFFERENCES_GRID_H_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace mpc_local_planner { 31 | 32 | /** 33 | * @brief Finite differences grid for SE2 34 | * 35 | * This class implements a full discretization grid with finite difference collocation. 36 | * The temporal resolution is fixed. 37 | * 38 | * @see corbo::FullDiscretizationGridBase corbo::DiscretizationGridInterface 39 | * FiniteDifferencesGridSE2 40 | * 41 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 42 | */ 43 | class FiniteDifferencesGridSE2 : public FullDiscretizationGridBaseSE2 44 | { 45 | public: 46 | using Ptr = std::shared_ptr; 47 | 48 | using BaseEdge = corbo::BaseEdge; 49 | using BaseMixedEdge = corbo::BaseMixedEdge; 50 | 51 | //! Default constructor 52 | FiniteDifferencesGridSE2() = default; 53 | //! Default destructor 54 | virtual ~FiniteDifferencesGridSE2() = default; 55 | 56 | //! Return a newly created shared instance of the implemented class 57 | corbo::DiscretizationGridInterface::Ptr getInstance() const override { return std::make_shared(); } 58 | 59 | //! Get access to the associated factory 60 | static corbo::Factory& getFactory() { return corbo::Factory::instance(); } 61 | 62 | protected: 63 | void createEdges(NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics) override; 64 | 65 | bool isDtFixedIntended() const override { return true; } 66 | }; 67 | 68 | } // namespace mpc_local_planner 69 | 70 | #endif // FINITE_DIFFERENCES_GRID_H_ 71 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_variable_grid_se2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef FINITE_DIFFERENCES_VARIABLE_GRID_H_ 24 | #define FINITE_DIFFERENCES_VARIABLE_GRID_H_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace mpc_local_planner { 31 | 32 | /** 33 | * @brief Finite differences grid with variable resolution for SE2 34 | * 35 | * This class implements a full discretization grid with finite difference collocation. 36 | * The temporal resolution is free. 37 | * This grid corresponds to the global uniform grid in time-optimal MPC as defined in [1,2]. 38 | * 39 | * [1] C. Rösmann, F. Hoffmann und T. Bertram: Timed-Elastic-Bands for Time-Optimal Point-to-Point Nonlinear Model Predictive Control, ECC 2015. 40 | * [2] C. Rösmann: Time-optimal nonlinear model predictive control - Direct transcription methods with variable discretization and structural sparsity 41 | * exploitation. Dissertation, TU Dortmund University, Oct. 2019. 42 | * 43 | * @see FiniteDifferencesGridSE2 44 | * 45 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 46 | */ 47 | class FiniteDifferencesVariableGridSE2 : public FiniteDifferencesGridSE2 48 | { 49 | public: 50 | using Ptr = std::shared_ptr; 51 | using UPtr = std::unique_ptr; 52 | 53 | enum class GridAdaptStrategy { NoGridAdapt, TimeBasedSingleStep, TimeBasedAggressiveEstimate, SimpleShrinkingHorizon }; 54 | 55 | FiniteDifferencesVariableGridSE2() = default; 56 | virtual ~FiniteDifferencesVariableGridSE2() = default; 57 | 58 | //! Return a newly created shared instance of the implemented class 59 | corbo::DiscretizationGridInterface::Ptr getInstance() const override { return std::make_shared(); } 60 | 61 | //! Get access to the associated factory 62 | static corbo::Factory& getFactory() { return corbo::Factory::instance(); } 63 | 64 | //! Set lower and upper bounds for the temporal resolution 65 | void setDtBounds(double dt_lb, double dt_ub); 66 | //! Set minium grid size for grid adaptation 67 | void setNmin(int n_min) { _n_min = n_min; } 68 | //! Disable grid adapation 69 | void disableGridAdaptation() { _grid_adapt = GridAdaptStrategy::NoGridAdapt; } 70 | //! Set grid adaptation strategy to 'single step' 71 | void setGridAdaptTimeBasedSingleStep(int n_max, double dt_hyst_ratio = 0.1, bool adapt_first_iter = false); 72 | //! Set grid adaptation strategy to 'aggressive' 73 | void setGridAdaptTimeBasedAggressiveEstimate(int n_max, double dt_hyst_ratio = 0.1, bool adapt_first_iter = false); 74 | //! Set grid adaptation strategy to 'shrinking horizon' 75 | void setGridAdaptSimpleShrinkingHorizon(bool adapt_first_iter = false); 76 | 77 | protected: 78 | bool isDtFixedIntended() const override { return false; } 79 | 80 | bool adaptGrid(bool new_run, NlpFunctions& nlp_fun) override; 81 | bool adaptGridTimeBasedSingleStep(NlpFunctions& nlp_fun); 82 | bool adaptGridTimeBasedAggressiveEstimate(NlpFunctions& nlp_fun); 83 | bool adaptGridSimpleShrinkingHorizon(NlpFunctions& nlp_fun); 84 | 85 | bool isMovingHorizonWarmStartActive() const override { return false; } 86 | bool isGridAdaptActive() const override { return true; } 87 | 88 | // auto resize stuff 89 | GridAdaptStrategy _grid_adapt = GridAdaptStrategy::NoGridAdapt; 90 | int _n_max = 1000; 91 | double _dt_hyst_ratio = 0.1; 92 | int _n_min = 2; 93 | bool _adapt_first_iter = false; 94 | }; 95 | 96 | } // namespace mpc_local_planner 97 | 98 | #endif // FINITE_DIFFERENCES_VARIABLE_GRID_H_ 99 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/optimal_control/min_time_via_points_cost.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef MIN_TIME_VIA_POINTS_COSTS_H_ 24 | #define MIN_TIME_VIA_POINTS_COSTS_H_ 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace mpc_local_planner { 37 | 38 | /** 39 | * @brief Hybrid cost function with minimum time and via-point objectives 40 | * 41 | * This class defines a joint objective of minimum time and via-point attraction. 42 | * A user defined weight controls the trade-off between both individual objectives. 43 | * 44 | * The current via-point strategy is borrowed from the teb_local_planner. 45 | * 46 | * @todo we can implement this class as LSQ version as well 47 | * 48 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 49 | */ 50 | class MinTimeViaPointsCost : public corbo::StageCost 51 | { 52 | public: 53 | using Ptr = std::shared_ptr; 54 | 55 | using ViaPointContainer = std::vector; 56 | 57 | //! Default constructor 58 | MinTimeViaPointsCost() = default; 59 | 60 | /** 61 | * @brief Construct with via-point container and weights 62 | * 63 | * @param[in] via_points Reference to the via-point container (must remain allocated) 64 | * @param[in] position_weight Via-point attraction weight for the position part 65 | * @param[in] orientation_weight Via-point attraction weight for the angular part 66 | * @param[in] via_points_orderedx True, if via-points should be associated w.r.t. the ordering in \c via_points 67 | */ 68 | MinTimeViaPointsCost(const ViaPointContainer& via_points, double position_weight, double orientation_weight, bool via_points_ordered) 69 | : _via_points_ordered(via_points_ordered) 70 | { 71 | setViaPointContainer(via_points); 72 | setViaPointWeights(position_weight, orientation_weight); 73 | } 74 | 75 | // implements interface method 76 | corbo::StageCost::Ptr getInstance() const override { return std::make_shared(); } 77 | 78 | // implements interface method 79 | bool hasNonIntegralTerms(int k) const override { return true; } 80 | // implements interface method 81 | bool hasIntegralTerms(int k) const override { return false; } 82 | 83 | // implements interface method 84 | int getNonIntegralDtTermDimension(int k) const override { return (k == 0 || !_single_dt) ? 1 : 0; } 85 | // implements interface method 86 | bool isLsqFormNonIntegralDtTerm(int k) const override { return false; } 87 | // implements interface method 88 | int getNonIntegralStateTermDimension(int k) const override; 89 | 90 | // implements interface method 91 | bool update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/, 92 | corbo::ReferenceTrajectoryInterface* /*sref*/, bool single_dt, const Eigen::VectorXd& x0, 93 | corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector& /*dts*/, 94 | const corbo::DiscretizationGridInterface* grid) override; 95 | 96 | // implements interface method 97 | void computeNonIntegralDtTerm(int k, double dt, Eigen::Ref cost) const override; 98 | // implements interface method 99 | void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; 100 | 101 | //! Set reference to via-point container (warning, object must remain allocated) 102 | void setViaPointContainer(const ViaPointContainer& via_points) { _via_points = &via_points; } 103 | //! Set if the optimzier should try to match the via-point ordering 104 | void setViaPointOrderedMode(bool ordered) { _via_points_ordered = ordered; } 105 | //! Set weights for via-point attraction 106 | void setViaPointWeights(double position_weight, double orientation_weight) 107 | { 108 | _vp_position_weight = position_weight; 109 | _vp_orientation_weight = orientation_weight; 110 | } 111 | 112 | protected: 113 | bool _single_dt = false; 114 | double _time_weight = 1; 115 | 116 | bool _via_points_ordered = false; 117 | double _vp_position_weight = 1e-3; 118 | double _vp_orientation_weight = 0; 119 | 120 | const ViaPointContainer* _via_points = nullptr; 121 | std::vector, std::size_t>> _vp_association; 122 | // also store previous number of associated points to detect structure changes 123 | }; 124 | 125 | } // namespace mpc_local_planner 126 | 127 | #endif // MIN_TIME_VIA_POINTS_COSTS_H_ 128 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/optimal_control/quadratic_cost_se2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef QUADRATIC_COST_SE2_H_ 24 | #define QUADRATIC_COST_SE2_H_ 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace mpc_local_planner { 33 | 34 | /** 35 | * @brief Quadratic form running cost (specialized for SE2) 36 | * 37 | * This class implements quadratic form running costs 38 | * \f[ 39 | * l(x, u) = (x_{ref} - x)^T Q (x_{xref} - x) + (u_{ref} - u)^T R (u_{ref} - u) 40 | * \f] 41 | * However, this class ensures that the state distance (x_{ref} - x) is 42 | * computed properly in SO(2) for the third state component. 43 | * 44 | * Note, the class also provides a least squares form if not in integral form which is: 45 | * \f$ l_{lsq} = \begin{bmatrix} \sqrt{Q} (x_{ref} - x) \\ \sqrt{R} (u_{ref} - u) \end{bmatrix} \f$ 46 | * \f$ \sqrt{Q} \f$ \f$ \sqrt{R} \f$ are computed once using the cholesky decomposition. 47 | * This form is later squared by the solver. 48 | * 49 | * @see corbo::QuadraticFinalStateCost 50 | * 51 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 52 | */ 53 | class QuadraticFormCostSE2 : public corbo::QuadraticFormCost 54 | { 55 | public: 56 | using Ptr = std::shared_ptr; 57 | 58 | //! Default constructor 59 | QuadraticFormCostSE2() : corbo::QuadraticFormCost() {} 60 | 61 | /** 62 | * @brief Construct with weight matrices 63 | * 64 | * @param[in] Q Positive definite state weighting matrix (must match dimensions!) 65 | * @param[in] R Positive definite control weighting matrix (must match dimensions!) 66 | * @param[in] integral_form If true, the running costs are later integrated over the grid interval, 67 | * otherwise, the values are just summed up as in sampled-data MPC. 68 | * @param[in] lsq_form Set to true in order to prefer the least-squares form 69 | */ 70 | QuadraticFormCostSE2(const Eigen::Ref& Q, const Eigen::Ref& R, bool integral_form = false, 71 | bool lsq_form = false) 72 | : corbo::QuadraticFormCost(Q, R, integral_form, lsq_form) 73 | { 74 | } 75 | 76 | // implements interface method 77 | corbo::StageCost::Ptr getInstance() const override { return std::make_shared(); } 78 | 79 | // implements interface method 80 | void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; 81 | 82 | // implements interface method 83 | void computeIntegralStateControlTerm(int k, const Eigen::Ref& x_k, const Eigen::Ref& u_k, 84 | Eigen::Ref cost) const override; 85 | }; 86 | 87 | /** 88 | * @brief Quadratic state running cost (specialized for SE2) 89 | * 90 | * This class implements quadratic state running costs 91 | * \f[ 92 | * l(x) = (x_{ref} - x)^T Q (x_{xref} - x) 93 | * \f] 94 | * However, this class ensures that the state distance (x_{ref} - x) is 95 | * computed properly in SO(2) for the third state component. 96 | * 97 | * Note, the class also provides a least squares form if not in integral form which is: 98 | * \f$ l_{lsq} = \sqrt{Q} (x_{ref} - x) \f$ 99 | * \f$ \sqrt{Q} \f$ is computed once using the cholesky decomposition. 100 | * This form is later squared by the solver. 101 | * 102 | * @see corbo::QuadraticFinalStateCost 103 | * 104 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 105 | */ 106 | class QuadraticStateCostSE2 : public corbo::QuadraticStateCost 107 | { 108 | public: 109 | using Ptr = std::shared_ptr; 110 | 111 | //! Default constructor 112 | QuadraticStateCostSE2() : corbo::QuadraticStateCost() {} 113 | 114 | /** 115 | * @brief Construct with weight matrices 116 | * 117 | * @param[in] Q Positive definite state weighting matrix (must match dimensions!) 118 | * @param[in] integral_form If true, the running costs are later integrated over the grid interval, 119 | * otherwise, the values are just summed up as in sampled-data MPC. 120 | * @param[in] lsq_form Set to true in order to prefer the least-squares form 121 | */ 122 | QuadraticStateCostSE2(const Eigen::Ref& Q, bool integral_form = false, bool lsq_form = false) 123 | : corbo::QuadraticStateCost(Q, integral_form, lsq_form) 124 | { 125 | } 126 | 127 | // implements interface method 128 | corbo::StageCost::Ptr getInstance() const override { return std::make_shared(); } 129 | 130 | // implements interface method 131 | void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; 132 | 133 | // implements interface method 134 | void computeIntegralStateControlTerm(int k, const Eigen::Ref& x_k, const Eigen::Ref& u_k, 135 | Eigen::Ref cost) const override; 136 | }; 137 | 138 | } // namespace mpc_local_planner 139 | 140 | #endif // QUADRATIC_COST_SE2_H_ 141 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef STAGE_INEQUALITY_SE2_H_ 24 | #define STAGE_INEQUALITY_SE2_H_ 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | namespace mpc_local_planner { 37 | 38 | /** 39 | * @brief Stage inequality constraint for obstacle avoidance and control deviation limits 40 | * 41 | * This class defines the inequality constraint for obstacle avoidance and control 42 | * deviation limits (in case limits are active). 43 | * 44 | * The current obstacle association strategy is borrowed from the teb_local_planner. 45 | * It takes the robot footprint model and the geometric obstacle shapes into account. 46 | * 47 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 48 | */ 49 | class StageInequalitySE2 : public corbo::StageInequalityConstraint 50 | { 51 | public: 52 | using Ptr = std::shared_ptr; 53 | 54 | using ObstaclePtr = teb_local_planner::ObstaclePtr; 55 | 56 | // implements interface method 57 | corbo::StageInequalityConstraint::Ptr getInstance() const override { return std::make_shared(); } 58 | 59 | // implements interface method 60 | bool hasNonIntegralTerms(int k) const override { return true; } 61 | // implements interface method 62 | bool hasIntegralTerms(int k) const override { return false; } 63 | 64 | // implements interface method 65 | int getNonIntegralStateTermDimension(int k) const override; 66 | // implements interface method 67 | int getNonIntegralStateDtTermDimension(int k) const override; 68 | // implements interface method 69 | int getNonIntegralControlDeviationTermDimension(int k) const override; 70 | 71 | // implements interface method 72 | bool update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/, 73 | corbo::ReferenceTrajectoryInterface* /*sref*/, bool /* single_dt*/, const Eigen::VectorXd& x0, 74 | corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector& /*dts*/, 75 | const corbo::DiscretizationGridInterface* grid) override; 76 | 77 | // implements interface method 78 | void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; 79 | // implements interface method 80 | void computeNonIntegralStateDtTerm(int k, const Eigen::Ref& x_k, double dt_k, 81 | Eigen::Ref cost) const override; 82 | // implements interface method 83 | void computeNonIntegralControlDeviationTerm(int k, const Eigen::Ref& u_k, const Eigen::Ref& u_prev, 84 | double dt, Eigen::Ref cost) const override; 85 | 86 | //! Set reference to obstacle vector (must remain allocated) 87 | void setObstacleVector(const teb_local_planner::ObstContainer& obstacles) { _obstacles = &obstacles; } 88 | //! Set robot footprint model 89 | void setRobotFootprintModel(teb_local_planner::RobotFootprintModelPtr robot_model) { _robot_model = robot_model; } 90 | 91 | //! Set minimum distance allowed 92 | void setMinimumDistance(double min_dist) { _min_obstacle_dist = min_dist; } 93 | //! Set parameters for prior filtering of obstacles 94 | void setObstacleFilterParameters(double force_inclusion_dist, double cutoff_dist) 95 | { 96 | _obstacle_filter_force_inclusion_dist = force_inclusion_dist; 97 | _obstacle_filter_cutoff_dist = cutoff_dist; 98 | } 99 | //! Set to true to enable dynamic obstacle (constant-velocity prediction) 100 | void setEnableDynamicObstacles(bool enable_dyn_obst) { _enable_dynamic_obstacles = enable_dyn_obst; } 101 | //! Specify lower and upper bounds for limiting the control deviation 102 | void setControlDeviationBounds(const Eigen::VectorXd& du_lb, const Eigen::VectorXd& du_ub); 103 | 104 | //! Get current minimum distance parameter value 105 | double getMinimumDistance() const { return _min_obstacle_dist; } 106 | 107 | protected: 108 | const teb_local_planner::ObstContainer* _obstacles = nullptr; 109 | std::vector _relevant_obstacles; // TODO: we can also store raw pointers as _via_points is locked by mutex 110 | std::vector _relevant_dyn_obstacles; 111 | 112 | teb_local_planner::RobotFootprintModelPtr _robot_model; 113 | 114 | double _current_dt = 0.1; 115 | int _num_du_lb_finite = 0; 116 | int _num_du_ub_finite = 0; 117 | 118 | double _min_obstacle_dist = 0.1; 119 | double _obstacle_filter_force_inclusion_dist = 1.5; 120 | double _obstacle_filter_cutoff_dist = 5; 121 | bool _enable_dynamic_obstacles = false; 122 | 123 | Eigen::VectorXd _du_lb; 124 | Eigen::VectorXd _du_ub; 125 | }; 126 | 127 | } // namespace mpc_local_planner 128 | 129 | #endif // STAGE_INEQUALITY_SE2_H_ 130 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/systems/base_robot_se2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef SYSTEMS_BASE_ROBOT_SE2_H_ 24 | #define SYSTEMS_BASE_ROBOT_SE2_H_ 25 | 26 | #include 27 | 28 | namespace mpc_local_planner { 29 | 30 | /** 31 | * @brief Specialization of RobotDynamicsInterface for mobile robots operating in SE2 32 | * 33 | * This abstract class defines a base class for robots in which the full state vector x 34 | * is embedded in SE2, i.e. x = [pos_x, pos_y, theta] with pos_x, pos_y as real numbers 35 | * and theta in [-pi, pi). 36 | * 37 | * Note, these models allow for simple conversion between state vectors and poses. 38 | * 39 | * @see RobotDynamicsInterface corbo::SystemDynamicsInterface 40 | * 41 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 42 | */ 43 | class BaseRobotSE2 : public RobotDynamicsInterface 44 | { 45 | public: 46 | using Ptr = std::shared_ptr; 47 | 48 | //! Default constructor 49 | BaseRobotSE2() = default; 50 | 51 | // implements interface method 52 | corbo::SystemDynamicsInterface::Ptr getInstance() const override = 0; 53 | 54 | // implements interface method 55 | int getInputDimension() const override = 0; 56 | // implements interface method 57 | int getStateDimension() const override { return 3; } 58 | 59 | // implements interface method 60 | bool isContinuousTime() const override { return true; } 61 | 62 | // implements interface method 63 | bool isLinear() const override { return false; } 64 | 65 | // implements interface method 66 | void getPositionFromState(const Eigen::Ref& x, double& pos_x, double& pos_y) const override 67 | { 68 | assert(x.size() == getStateDimension()); 69 | pos_x = x[0]; 70 | pos_y = x[1]; 71 | } 72 | 73 | // implements interface method 74 | void getPoseSE2FromState(const Eigen::Ref& x, double& pos_x, double& pos_y, double& theta) const override 75 | { 76 | assert(x.size() == getStateDimension()); 77 | pos_x = x[0]; 78 | pos_y = x[1]; 79 | theta = x[2]; 80 | } 81 | 82 | // implements interface method 83 | void getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref x) const override 84 | { 85 | assert(x.size() == getStateDimension()); 86 | x[0] = pos_x; 87 | x[1] = pos_y; 88 | x[2] = theta; 89 | if (x.size() > 3) x.tail(x.size() - 3).setZero(); 90 | } 91 | 92 | // implements interface method 93 | virtual bool mergeStateFeedbackAndOdomFeedback(const teb_local_planner::PoseSE2& odom_pose, const geometry_msgs::Twist& odom_twist, 94 | Eigen::Ref x) const override 95 | { 96 | assert(x.size() == getStateDimension()); 97 | x[0] = odom_pose.x(); 98 | x[1] = odom_pose.y(); 99 | x[2] = odom_pose.theta(); 100 | return true; 101 | } 102 | 103 | // implements interface method 104 | void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override = 0; 105 | }; 106 | 107 | } // namespace mpc_local_planner 108 | 109 | #endif // SYSTEMS_BASE_ROBOT_SE2_H_ 110 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/systems/kinematic_bicycle_model.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_ 24 | #define SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace mpc_local_planner { 31 | 32 | /** 33 | * @brief Kinematic Bicycle Model with Velocity Input 34 | * 35 | * This class implements the dynamics for a kinematic bicycle model. 36 | * Note, in this model the robot coordinate system is located at the center of gravity 37 | * which is between the rear and the front axle. 38 | * In case you want to define the coordinate system at the rear axle, 39 | * please refer to the simplified equations simple_car.h (SimpleCarModel). 40 | * 41 | * [1] R. Rajamani, Vehicle Dynamics and Control, Springer, 2012. 42 | * [2] J. Kong et al., Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design, IV, 2015. 43 | * 44 | * @see SimpleCarModel SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface 45 | * corbo::SystemDynamicsInterface 46 | * 47 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 48 | */ 49 | class KinematicBicycleModelVelocityInput : public BaseRobotSE2 50 | { 51 | public: 52 | //! Default constructor 53 | KinematicBicycleModelVelocityInput() = default; 54 | 55 | //! Constructs model with given wheelbase 56 | KinematicBicycleModelVelocityInput(double lr, double lf) : _lr(lr), _lf(lf) {} 57 | 58 | // implements interface method 59 | SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared(); } 60 | 61 | // implements interface method 62 | int getInputDimension() const override { return 2; } 63 | 64 | // implements interface method 65 | void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override 66 | { 67 | assert(x.size() == getStateDimension()); 68 | assert(u.size() == getInputDimension()); 69 | assert(x.size() == f.size() && 70 | "KinematicBicycleModelVelocityInput::dynamics(): x and f are not of the same size, do not forget to pre-allocate f."); 71 | 72 | double beta = std::atan(_lr / (_lf + _lr) * std::tan(u[1])); 73 | 74 | f[0] = u[0] * std::cos(x[2] + beta); 75 | f[1] = u[0] * std::sin(x[2] + beta); 76 | f[2] = u[0] * std::sin(beta) / _lr; 77 | } 78 | 79 | // implements interface method 80 | bool getTwistFromControl(const Eigen::Ref& u, geometry_msgs::Twist& twist) const override 81 | { 82 | assert(u.size() == getInputDimension()); 83 | twist.linear.x = u[0]; 84 | twist.linear.y = twist.linear.z = 0; 85 | 86 | twist.angular.z = u[1]; // warning, this is the angle and not the angular vel 87 | twist.angular.x = twist.angular.y = 0; 88 | 89 | return true; 90 | } 91 | 92 | //! Set parameters 93 | void setParameters(double lr, double lf) 94 | { 95 | _lr = lr; 96 | _lf = lf; 97 | } 98 | //! Get length between COG and front axle 99 | double getLengthFront() const { return _lf; } 100 | //! Get length between COG and rear axle 101 | double getLengthRear() const { return _lr; } 102 | 103 | protected: 104 | double _lr = 1.0; 105 | double _lf = 1.0; 106 | }; 107 | 108 | } // namespace mpc_local_planner 109 | 110 | #endif // SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_ 111 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/systems/robot_dynamics_interface.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_ 24 | #define SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | namespace mpc_local_planner { 34 | 35 | /** 36 | * @brief Specialization of SystemDynamicsInterface for mobile robots 37 | * 38 | * This abstract class extends cobro::SystemDynamicsInterface 39 | * by (abstract) conversion methods between states, poses embedded in SE2, 40 | * controls and twist. 41 | * 42 | * @see BaseRobotSE2 corbo::SystemDynamicsInterface 43 | * 44 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 45 | */ 46 | class RobotDynamicsInterface : public corbo::SystemDynamicsInterface 47 | { 48 | public: 49 | using Ptr = std::shared_ptr; 50 | 51 | /** 52 | * @brief Convert state vector to position (x,y) 53 | * 54 | * @param[in] x Reference to the state vector [getStateDimension() x 1] 55 | * @param[out] pos_x X-component of the position 56 | * @param[out] pos_y Y-component of the position 57 | */ 58 | virtual void getPositionFromState(const Eigen::Ref& x, double& pos_x, double& pos_y) const = 0; 59 | 60 | /** 61 | * @brief Convert state vector to pose (x,y,theta) 62 | * 63 | * @param[in] x Reference to the state vector [getStateDimension() x 1] 64 | * @param[out] pos_x X-component of the position part 65 | * @param[out] pos_y Y-component of the position part 66 | * @param[out] theta Orientation (yaw angle) in [-pi, pi) 67 | */ 68 | virtual void getPoseSE2FromState(const Eigen::Ref& x, double& pos_x, double& pos_y, double& theta) const = 0; 69 | 70 | /** 71 | * @brief Convert state vector to PoseSE2 object 72 | * 73 | * @param[in] x Reference to the state vector [getStateDimension() x 1] 74 | * @param[out] pose PoseSE2 object 75 | */ 76 | virtual void getPoseSE2FromState(const Eigen::Ref& x, teb_local_planner::PoseSE2& pose) const 77 | { 78 | getPoseSE2FromState(x, pose.x(), pose.y(), pose.theta()); 79 | } 80 | 81 | /** 82 | * @brief Convert pose (x,y,theta) to steady state 83 | * 84 | * Converts a pose to state vector with dimensions [getStateDimension() x 1]. 85 | * In case getStateDimension() > 3, the state information is incomplete 86 | * and hence it is assumed that a steady state for this particular pose is avaiable, 87 | * which means that there exist a control u such that f(x,u) = 0 (continuous time) or 88 | * f(x,u) = x (discrete time). 89 | * 90 | * For example, in case the remaining state components correspond to just integrators, 91 | * they can be set to zero. 92 | * 93 | * @param[in] pos_x X-component of the position part 94 | * @param[in] pos_y Y-component of the position part 95 | * @param[in] theta Orientation (yaw angle) in [-pi, pi) 96 | * @param[out] x Reference to the state vector [getStateDimension() x 1] 97 | */ 98 | virtual void getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref x) const = 0; 99 | 100 | /** 101 | * @brief Convert PoseSE2 to steady state 102 | * 103 | * See description of getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref x) 104 | * 105 | * @param[in] pose PoseSE2 object 106 | * @param[out] x Reference to the state vector [getStateDimension() x 1] 107 | */ 108 | virtual void getSteadyStateFromPoseSE2(const teb_local_planner::PoseSE2& pose, Eigen::Ref x) const 109 | { 110 | getSteadyStateFromPoseSE2(pose.x(), pose.y(), pose.theta(), x); 111 | } 112 | 113 | /** 114 | * @brief Convert control vector to twist message 115 | * 116 | * Convert the control vector to a twist message (containing velocity information) 117 | * if possible. 118 | * 119 | * @todo Maybe add current state x as optional input to allow for computing a twist out of the first state and control 120 | * 121 | * @param[in] u Reference to the control vector [getInputDimension() x 1] 122 | * @param[out] twist Reference to the twist message 123 | * @return true, if conversion was successful, false otherwise 124 | */ 125 | virtual bool getTwistFromControl(const Eigen::Ref& u, geometry_msgs::Twist& twist) const = 0; 126 | 127 | /** 128 | * @brief Merge custom state feedback with pose and twist feedback 129 | * 130 | * Many robots in ROS publish their state information (pose, linear and angular velocity)e.g. via an odometry message 131 | * (potentially corrected by localization). 132 | * However, for more complex robot models, it might not be possible to obtain the full state information without any observer 133 | * or further measurements. 134 | * For this purpose, custom state feedback can be provided. But as the complete navigation stack relies on the information obtained by 135 | * odometry, this method allows for merging of both sources. 136 | * In particular, it overrides only state components related to the pose and velocity. 137 | * 138 | * @param[in] odom_pose PoseSE2 object obtained from odometry 139 | * @param[in] odom_twist geometry::Twist message obtained from odometry 140 | * @param[in,out] x Custom state feedback in which related will be overriden [getStateDimension() x 1] 141 | * @return true, if merging was successful, false otherwise 142 | */ 143 | virtual bool mergeStateFeedbackAndOdomFeedback(const teb_local_planner::PoseSE2& odom_pose, const geometry_msgs::Twist& odom_twist, 144 | Eigen::Ref x) const = 0; 145 | 146 | virtual void reset() {} 147 | }; 148 | 149 | } // namespace mpc_local_planner 150 | 151 | #endif // SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_ 152 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/systems/simple_car.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef SYSTEMS_SIMPLE_CAR_H_ 24 | #define SYSTEMS_SIMPLE_CAR_H_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace mpc_local_planner { 31 | 32 | /** 33 | * @brief Simple car model 34 | * 35 | * This class implements the dynamics for a simple car model 36 | * in which the rear wheels are actuated. 37 | * The front wheels are the steering wheels (for wheelbase > 0). 38 | * The state vector [x, y, theta] is defined at the center of the rear axle. 39 | * See [1,2] for a mathematical description and a figure. 40 | * 41 | * [1] S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006. 42 | * (Chapter 13, http://planning.cs.uiuc.edu/) 43 | * [2] A. De Luca et al., Feedback Control of a Nonholonomic Car-like Robot, 44 | * in Robot Motion Planning and Control (Ed. J.-P. Laumond), Springer, 1998. 45 | * (https://homepages.laas.fr/jpl/promotion/chap4.pdf) 46 | * 47 | * @see SimpleCarFrontWheelDrivingModel KinematicBicycleModelVelocityInput 48 | * BaseRobotSE2 RobotDynamicsInterface corbo::SystemDynamicsInterface 49 | * 50 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 51 | */ 52 | class SimpleCarModel : public BaseRobotSE2 53 | { 54 | public: 55 | //! Default constructor 56 | SimpleCarModel() = default; 57 | 58 | //! Constructs model with given wheelbase 59 | SimpleCarModel(double wheelbase) : _wheelbase(wheelbase) {} 60 | 61 | // implements interface method 62 | SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared(); } 63 | 64 | // implements interface method 65 | int getInputDimension() const override { return 2; } 66 | 67 | // implements interface method 68 | void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override 69 | { 70 | assert(x.size() == getStateDimension()); 71 | assert(u.size() == getInputDimension()); 72 | assert(x.size() == f.size() && "SimpleCarModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f."); 73 | 74 | f[0] = u[0] * std::cos(x[2]); 75 | f[1] = u[0] * std::sin(x[2]); 76 | f[2] = u[0] * std::tan(u[1]) / _wheelbase; 77 | } 78 | 79 | // implements interface method 80 | bool getTwistFromControl(const Eigen::Ref& u, geometry_msgs::Twist& twist) const override 81 | { 82 | assert(u.size() == getInputDimension()); 83 | twist.linear.x = u[0]; 84 | twist.linear.y = twist.linear.z = 0; 85 | 86 | twist.angular.z = u[1]; // warning, this is the angle and not the angular vel 87 | twist.angular.x = twist.angular.y = 0; 88 | 89 | return true; 90 | } 91 | 92 | //! Set wheelbase 93 | void setWheelbase(double wheelbase) { _wheelbase = wheelbase; } 94 | //! Get wheelbase 95 | double getWheelbase() const { return _wheelbase; } 96 | 97 | protected: 98 | double _wheelbase = 1.0; 99 | }; 100 | 101 | /** 102 | * @brief Simple car model with front wheel actuation 103 | * 104 | * This class implements the dynamics for a simple car model 105 | * in which the front wheels are actuated and steered (for wheelbase > 0). 106 | * The state vector [x, y, theta] is defined at the center of the rear axle. 107 | * See [1] for a mathematical description and a figure. 108 | * 109 | * [1] A. De Luca et al., Feedback Control of a Nonholonomic Car-like Robot, 110 | * in Robot Motion Planning and Control (Ed. J.-P. Laumond), Springer, 1998. 111 | * (https://homepages.laas.fr/jpl/promotion/chap4.pdf) 112 | * 113 | * @see SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface 114 | * corbo::SystemDynamicsInterface 115 | * 116 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 117 | */ 118 | class SimpleCarFrontWheelDrivingModel : public SimpleCarModel 119 | { 120 | public: 121 | //! Default constructor 122 | SimpleCarFrontWheelDrivingModel() = default; 123 | 124 | //! Constructs model with given wheelbase 125 | SimpleCarFrontWheelDrivingModel(double wheelbase) : SimpleCarModel(wheelbase) {} 126 | 127 | // implements interface method 128 | SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared(); } 129 | 130 | // implements interface method 131 | void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override 132 | { 133 | assert(x.size() == getStateDimension()); 134 | assert(u.size() == getInputDimension()); 135 | assert(x.size() == f.size() && 136 | "SimpleCarFrontWheelDrivingModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f."); 137 | 138 | f[0] = u[0] * std::cos(x[2]); 139 | f[1] = u[0] * std::sin(x[2]); 140 | f[2] = u[0] * std::sin(u[1]) / _wheelbase; 141 | } 142 | }; 143 | 144 | } // namespace mpc_local_planner 145 | 146 | #endif // SYSTEMS_SIMPLE_CAR_H_ 147 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/systems/unicycle_robot.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef SYSTEMS_UNICYCLE_ROBOT_H_ 24 | #define SYSTEMS_UNICYCLE_ROBOT_H_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace mpc_local_planner { 31 | 32 | /** 33 | * @brief Unicycle model 34 | * 35 | * This class implements the dynamics for a unicycle model 36 | * which can be used e.g., for differential-drive robots. 37 | * See [1] for a mathematical description and a figure. 38 | * 39 | * [1] S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006. 40 | * (Chapter 13, http://planning.cs.uiuc.edu/) 41 | * 42 | * @see BaseRobotSE2 RobotDynamicsInterface corbo::SystemDynamicsInterface 43 | * 44 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 45 | */ 46 | class UnicycleModel : public BaseRobotSE2 47 | { 48 | public: 49 | //! Default constructor 50 | UnicycleModel() = default; 51 | 52 | // implements interface method 53 | SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared(); } 54 | 55 | // implements interface method 56 | int getInputDimension() const override { return 2; } 57 | 58 | // implements interface method 59 | void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override 60 | { 61 | assert(x.size() == getStateDimension()); 62 | assert(u.size() == getInputDimension()); 63 | assert(x.size() == f.size() && "UnicycleModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f."); 64 | 65 | f[0] = u[0] * std::cos(x[2]); 66 | f[1] = u[0] * std::sin(x[2]); 67 | f[2] = u[1]; 68 | } 69 | 70 | // implements interface method 71 | bool getTwistFromControl(const Eigen::Ref& u, geometry_msgs::Twist& twist) const override 72 | { 73 | assert(u.size() == getInputDimension()); 74 | twist.linear.x = u[0]; 75 | twist.linear.y = twist.linear.z = 0; 76 | 77 | twist.angular.z = u[1]; 78 | twist.angular.x = twist.angular.y = 0; 79 | return true; 80 | } 81 | }; 82 | 83 | } // namespace mpc_local_planner 84 | 85 | #endif // SYSTEMS_UNICYCLE_ROBOT_H_ 86 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/utils/conversion.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef UTILS_CONVERSION_H_ 24 | #define UTILS_CONVERSION_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace mpc_local_planner { 36 | 37 | /** 38 | * @brief Convert TimeSeries to pose array 39 | * 40 | * Converts TimeSeries to std::vector. 41 | * 42 | * @remarks Note, the actual data of the TimeSeries object is copied without interpolation. 43 | * 44 | * @todo We could avoid the system dynamics dependency by specifying a generic getter function for the SE2 poses 45 | * 46 | * @param[in] time_series corbo::TimeSeries object or any child class 47 | * @param[in] dynamics Reference to the robot dynamics interface (to access state-to-SE2 conversion methods) 48 | * @param[out] poses_stamped The resulting pose array (note, the incoming vector will be cleared) 49 | * @param[in] frame_id The planning frame id that is added to the message header 50 | */ 51 | void convert(const corbo::TimeSeries& time_series, const RobotDynamicsInterface& dynamics, std::vector& poses_stamped, 52 | const std::string& frame_id); 53 | 54 | } // namespace mpc_local_planner 55 | 56 | #endif // UTILS_CONVERSION_H_ 57 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/utils/math_utils.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef UTILS_MATH_UTILS_H_ 24 | #define UTILS_MATH_UTILS_H_ 25 | 26 | #include 27 | 28 | namespace mpc_local_planner { 29 | 30 | /** 31 | * @brief Return the average angle of an arbitrary number of given angles [rad] 32 | * @param angles vector containing all angles 33 | * @return average / mean angle, that is normalized to [-pi, pi] 34 | */ 35 | inline double average_angles(const std::vector& angles) 36 | { 37 | double x = 0, y = 0; 38 | for (std::vector::const_iterator it = angles.begin(); it != angles.end(); ++it) 39 | { 40 | x += std::cos(*it); 41 | y += std::sin(*it); 42 | } 43 | if (x == 0 && y == 0) 44 | return 0; 45 | else 46 | return std::atan2(y, x); 47 | } 48 | 49 | /** 50 | * @brief Calculate Euclidean distance between two 2D point datatypes 51 | * @param point1 object containing fields x and y 52 | * @param point2 object containing fields x and y 53 | * @return Euclidean distance: ||point2-point1|| 54 | */ 55 | template 56 | inline double distance_points2d(const P1& point1, const P2& point2) 57 | { 58 | return std::sqrt(std::pow(point2.x - point1.x, 2) + std::pow(point2.y - point1.y, 2)); 59 | } 60 | 61 | //! Calculate Euclidean distance between two 2D points 62 | inline double distance_points2d(double x1, double y1, double x2, double y2) { return std::sqrt(std::pow(x2 - x1, 2) + std::pow(y2 - y1, 2)); } 63 | 64 | /** 65 | * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) 66 | * @param v1 object containing public methods x() and y() 67 | * @param v2 object containing fields x() and y() 68 | * @return magnitude that would result in the 3D case (along the z-axis) 69 | */ 70 | template 71 | inline double cross2d(const V1& v1, const V2& v2) 72 | { 73 | return v1.x() * v2.y() - v2.x() * v1.y(); 74 | } 75 | 76 | /** 77 | * @brief normalize angle to interval [-pi, pi) 78 | * @remark This function is based on normalize_theta from g2o 79 | * see: https://github.com/RainerKuemmerle/g2o/blob/master/g2o/stuff/misc.h 80 | */ 81 | inline double normalize_theta(double theta) 82 | { 83 | if (theta >= -M_PI && theta < M_PI) return theta; 84 | 85 | double multiplier = std::floor(theta / (2.0 * M_PI)); 86 | theta = theta - multiplier * 2.0 * M_PI; 87 | if (theta >= M_PI) theta -= 2.0 * M_PI; 88 | if (theta < -M_PI) theta += 2.0 * M_PI; 89 | 90 | return theta; 91 | } 92 | 93 | /** 94 | * @brief Return the interpolated angle between two angles [rad] 95 | * @param angle1 96 | * @param angle2 97 | * @param factor in [0,1], or (1,inf) for extrapolation 98 | * @return average / mean angle, that is normalized to [-pi, pi] 99 | */ 100 | inline double interpolate_angle(double angle1, double angle2, double factor) 101 | { 102 | return normalize_theta(angle1 + factor * normalize_theta(angle2 - angle1)); 103 | } 104 | 105 | } // namespace mpc_local_planner 106 | 107 | #endif // UTILS_MATH_UTILS_H_ 108 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/utils/publisher.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef UTILS_PUBLISHER_H_ 24 | #define UTILS_PUBLISHER_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | namespace mpc_local_planner { 40 | 41 | /** 42 | * @brief This class provides publishing methods for common messages 43 | * 44 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 45 | */ 46 | class Publisher 47 | { 48 | using ObstaclePtr = teb_local_planner::ObstaclePtr; 49 | using ObstContainer = teb_local_planner::ObstContainer; 50 | using PointObstacle = teb_local_planner::PointObstacle; 51 | using CircularObstacle = teb_local_planner::CircularObstacle; 52 | using LineObstacle = teb_local_planner::LineObstacle; 53 | using PolygonObstacle = teb_local_planner::PolygonObstacle; 54 | using Point2dContainer = teb_local_planner::Point2dContainer; 55 | 56 | public: 57 | /** 58 | * @brief Default constructor 59 | * @remarks do not forget to call initialize() 60 | */ 61 | Publisher() = default; 62 | 63 | /** 64 | * @brief Constructor that initializes the class and registers topics 65 | * @param[in] nh local ros::NodeHandle 66 | * @param[in] map_frame the planning frame name 67 | */ 68 | Publisher(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame); 69 | 70 | /** 71 | * @brief Initializes the class and registers topics. 72 | * 73 | * Call this function if only the default constructor has been called before. 74 | * @param[in] nh local ros::NodeHandle 75 | * @param[in] map_frame the planning frame name 76 | */ 77 | void initialize(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame); 78 | 79 | /** 80 | * @brief Publish a given local plan to the ros topic \e ../../local_plan 81 | * @param[in] local_plan Pose array describing the local plan 82 | */ 83 | void publishLocalPlan(const std::vector& local_plan) const; 84 | 85 | /** 86 | * @brief Publish a given local plan to the ros topic \e ../../local_plan 87 | * @param[in] local_plan Pose array describing the local plan 88 | */ 89 | void publishLocalPlan(const corbo::TimeSeries& ts) const; 90 | 91 | /** 92 | * @brief Publish a given global plan to the ros topic \e ../../global_plan 93 | * @param[in] global_plan Pose array describing the global plan 94 | */ 95 | void publishGlobalPlan(const std::vector& global_plan) const; 96 | 97 | /** 98 | * @brief Publish the visualization of the robot model 99 | * 100 | * @param[in] current_pose Current pose of the robot 101 | * @param[in] robot_model Subclass of BaseRobotFootprintModel 102 | * @param[in] map_frame frame name for the msg header 103 | * @param[in] ns Namespace for the marker objects 104 | * @param[in] color Color of the footprint 105 | */ 106 | void publishRobotFootprintModel(const teb_local_planner::PoseSE2& current_pose, const teb_local_planner::BaseRobotFootprintModel& robot_model, 107 | const std::string& ns = "RobotFootprintModel", const std_msgs::ColorRGBA& color = toColorMsg(0.5, 0.0, 0.8, 0.0)); 108 | 109 | /** 110 | * @brief Publish obstacle positions to the ros topic \e ../../mpc_markers 111 | * @todo Move filling of the marker message to polygon class in order to avoid checking types. 112 | * @param[in] obstacles Obstacle container 113 | */ 114 | void publishObstacles(const teb_local_planner::ObstContainer& obstacles) const; 115 | 116 | /** 117 | * @brief Publish via-points to the ros topic \e ../../teb_markers 118 | * 119 | * @todo add option to switch between points and poses (including orientation) to be published 120 | * 121 | * @param[in] via_points via-point container 122 | * @param[in] ns marker namespace 123 | */ 124 | void publishViaPoints(const std::vector& via_points, const std::string& ns = "ViaPoints") const; 125 | 126 | /** 127 | * @brief Helper function to generate a color message from single values 128 | * @param[in] a Alpha value 129 | * @param[in] r Red value 130 | * @param[in] g Green value 131 | * @param[in] b Blue value 132 | * @return Color message 133 | */ 134 | static std_msgs::ColorRGBA toColorMsg(float a, float r, float g, float b); 135 | 136 | private: 137 | bool _initialized = false; 138 | 139 | std::string _map_frame = "map"; 140 | 141 | RobotDynamicsInterface::Ptr _system; 142 | 143 | ros::Publisher _local_plan_pub; 144 | ros::Publisher _global_plan_pub; 145 | ros::Publisher _mpc_marker_pub; 146 | }; 147 | 148 | } // namespace mpc_local_planner 149 | 150 | #endif // UTILS_PUBLISHER_H_ 151 | -------------------------------------------------------------------------------- /mpc_local_planner/include/mpc_local_planner/utils/time_series_se2.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #ifndef UTILS_TIME_SERIES_SE2_H_ 24 | #define UTILS_TIME_SERIES_SE2_H_ 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace mpc_local_planner { 31 | 32 | /** 33 | * @brief Time Series with SE2 support 34 | * 35 | * A time series represents a sequence of vectors of floating point numbers 36 | * associated with time stamps. A time series might also be interpreted as a 37 | * discrete-time trajectory. 38 | * This class extends corbo::TimeSeries to support SE2 poses. 39 | * Hereby, the third state component is always treated as orientation in [-pi, pi). 40 | * In particular, whenever values are retrieved by interpolation 41 | * (getValuesInterpolate()), the third state is ensured to remain properly in [-pi, pi). 42 | * 43 | * Note, as the first three state components define the SE2, any further auxiliary 44 | * state component is treated as ordinary real number. 45 | * 46 | * @see corbo::TimeSeries 47 | * 48 | * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) 49 | */ 50 | class TimeSeriesSE2 : public corbo::TimeSeries 51 | { 52 | public: 53 | using Ptr = std::shared_ptr; 54 | using ConstPtr = std::shared_ptr; 55 | 56 | //! Default constructor 57 | TimeSeriesSE2() = default; 58 | //! Construct empty time series with a dresired value vector dimension 59 | explicit TimeSeriesSE2(int value_dim) : TimeSeries(value_dim) {} 60 | 61 | /** 62 | * @brief Retrieve value vector at a desired time stamp (seconds) via interpolation 63 | * 64 | * This method interpolates the underlying time series object at a desired 65 | * time stamp (given in seconds). Interpolation and extrapolation settings 66 | * can be passed as optional arguments. 67 | * 68 | * @warning This method currently assumes that the values / time pairs are 69 | * stored with monotonically increasing time stamps (in time()). 70 | * 71 | * @param[in] time desired time stamp 72 | * @param[out] values interpolated value vector w.r.t. \c time 73 | * @param[in] interpolation interpolation method according to Interpolation enum 74 | * @param[in] extrapolate specify whether exrapliation should be applied with a similar 75 | * method like interpolation 76 | * @param[in] tolerance specify a tolerance for which two time stamps are treated equally 77 | * @returns true if successfull, false otherwise (e.g. if extrapolate is false but \c time > max(time) 78 | */ 79 | bool getValuesInterpolate(double time, Eigen::Ref values, Interpolation interpolation = Interpolation::Linear, 80 | Extrapolation extrapolate = Extrapolation::NoExtrapolation, double tolerance = 1e-6) const override; 81 | 82 | //! Compute and return the mean value of all values among all dimensions 83 | double computeMeanOverall() override; 84 | /** 85 | * @brief Compute and return the component-wise mean values 86 | * @param[out] mean_values Resulting cwise mean values [getValueDimension() x 1] (vector size must be preallocated) 87 | */ 88 | void computeMeanCwise(Eigen::Ref mean_values) override; 89 | }; 90 | 91 | } // namespace mpc_local_planner 92 | 93 | #endif // UTILS_TIME_SERIES_SE2_H_ 94 | -------------------------------------------------------------------------------- /mpc_local_planner/launch/test_mpc_optim_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /mpc_local_planner/mpc_local_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The mpc_local_planner package implements a plugin 5 | to the base_local_planner of the 2D navigation stack. 6 | 7 | 8 | 9 | 10 | Same plugin implemented MBF CostmapController's extended interface. 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /mpc_local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mpc_local_planner 4 | 0.0.3 5 | The mpc_local_planner package implements a plugin 6 | to the base_local_planner of the 2D navigation stack. 7 | It provides a generic and versatile model predictive control implementation 8 | with minimum-time and quadratic-form receding-horizon configurations. 9 | 10 | 11 | Christoph Rösmann 12 | Christoph Rösmann 13 | 14 | GPLv3 15 | 16 | http://wiki.ros.org/mpc_local_planner 17 | 18 | 19 | catkin 20 | 21 | roscpp 22 | tf2_eigen 23 | tf2_geometry_msgs 24 | 25 | roscpp 26 | 27 | roscpp 28 | 29 | control_box_rst 30 | 31 | base_local_planner 32 | costmap_2d 33 | costmap_converter 34 | dynamic_reconfigure 35 | eigen 36 | geometry_msgs 37 | interactive_markers 38 | nav_core 39 | nav_msgs 40 | mbf_costmap_core 41 | mbf_msgs 42 | mpc_local_planner_msgs 43 | pluginlib 44 | std_msgs 45 | teb_local_planner 46 | tf2 47 | tf2_ros 48 | visualization_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /mpc_local_planner/scripts/plot_optimal_control_results.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # 4 | # Software License Agreement 5 | # 6 | # Copyright (c) 2020, Christoph Roesmann, All rights reserved. 7 | # 8 | # This program is free software: you can redistribute it and/or modify 9 | # it under the terms of the GNU General Public License as published by 10 | # the Free Software Foundation, either version 3 of the License, or 11 | # (at your option) any later version. 12 | # 13 | # This program is distributed in the hope that it will be useful, 14 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | # GNU General Public License for more details. 17 | # 18 | # You should have received a copy of the GNU General Public License 19 | # along with this program. If not, see . 20 | # 21 | # Authors: Christoph Roesmann 22 | 23 | import rospy, math 24 | from mpc_local_planner_msgs.msg import OptimalControlResult 25 | import numpy as np 26 | import matplotlib.pyplot as plt 27 | import matplotlib.ticker as ticker 28 | from threading import Lock 29 | 30 | class OcpResultPlotter: 31 | 32 | def __init__(self, plot_states, topic_name): 33 | 34 | self.initialized = False 35 | self.plot_states = plot_states 36 | self.dim_states = 0 37 | self.dim_controls = 0 38 | self.x_fig = plt.Figure() 39 | self.x_axes = [] 40 | self.u_fig = plt.Figure() 41 | self.u_axes = [] 42 | self.tx = [] 43 | self.x = [] 44 | self.tu = [] 45 | self.u = [] 46 | self.mutex = Lock() 47 | 48 | self.sub = rospy.Subscriber(topic_name, OptimalControlResult, self.ocp_result_callback, queue_size = 1) 49 | rospy.loginfo("Plotting OCP results published on '%s'.",topic_name) 50 | rospy.loginfo("Make sure to enable rosparam 'controller/publish_ocp_results' in the mpc_local_planner.") 51 | 52 | def ocp_result_callback(self, data): 53 | rospy.loginfo_once("First message received.") 54 | if not self.initialized: 55 | self.dim_states = data.dim_states 56 | self.dim_controls = data.dim_controls 57 | 58 | # Read data 59 | self.mutex.acquire() 60 | if self.plot_states: 61 | self.tx = np.array(data.time_states) 62 | self.x = np.matrix(data.states) 63 | self.x = np.reshape(self.x, (self.tx.size, int(self.dim_states))) 64 | self.tu = np.array(data.time_controls) 65 | self.u = np.matrix(data.controls) 66 | self.u = np.reshape(self.u, (self.tu.size, int(self.dim_controls))) 67 | self.mutex.release() 68 | 69 | def initializePlotWindows(self): 70 | if self.plot_states: 71 | self.x_fig, self.x_axes = plt.subplots(self.dim_states, sharex=True) 72 | self.x_axes[0].set_title('States') 73 | for idx, ax in enumerate(self.x_axes): 74 | ax.set_ylabel("x" + str(idx)) 75 | self.x_axes[-1].set_xlabel('Time [s]') 76 | 77 | self.u_fig, self.u_axes = plt.subplots(self.dim_controls, sharex=True) 78 | self.u_axes[0].set_title('Controls') 79 | for idx, ax in enumerate(self.u_axes): 80 | ax.set_ylabel("u" + str(idx)) 81 | self.u_axes[-1].set_xlabel('Time [s]') 82 | plt.ion() 83 | plt.show() 84 | 85 | 86 | def plot(self): 87 | # We recreate the plot every time, not fast, but okay for now.... 88 | self.mutex.acquire() 89 | if self.plot_states: 90 | for idx, ax in enumerate(self.x_axes): 91 | ax.cla() 92 | ax.grid() 93 | ax.plot(self.tx, self.x[:,idx]) 94 | ax.get_yaxis().set_major_formatter(ticker.FuncFormatter(lambda x, p: "%.2f" % x)) 95 | ax.set_ylabel("x" + str(idx)) 96 | self.x_axes[0].set_title('States') 97 | self.x_axes[-1].set_xlabel('Time [s]') 98 | self.x_fig.canvas.draw() 99 | 100 | for idx, ax in enumerate(self.u_axes): 101 | ax.cla() 102 | ax.grid() 103 | ax.step(self.tu, self.u[:, idx], where='post') 104 | ax.get_yaxis().set_major_formatter(ticker.FuncFormatter(lambda x, p: "%.2f" % x)) 105 | ax.set_ylabel("u" + str(idx)) 106 | self.u_axes[0].set_title('Controls') 107 | self.u_axes[-1].set_xlabel('Time [s]') 108 | self.u_fig.canvas.draw() 109 | self.mutex.release() 110 | 111 | def start(self, rate): 112 | r = rospy.Rate(rate) # define rate here 113 | while not rospy.is_shutdown(): 114 | if not self.initialized and (self.dim_states > 0 or self.dim_controls > 0): 115 | self.initializePlotWindows() 116 | self.initialized = True 117 | if self.initialized: 118 | self.plot() 119 | r.sleep() 120 | 121 | 122 | 123 | if __name__ == '__main__': 124 | try: 125 | 126 | rospy.init_node("ocp_result_plotter", anonymous=True) 127 | 128 | topic_name = "/test_mpc_optim_node/ocp_result" 129 | topic_name = rospy.get_param('~ocp_result_topic', topic_name) 130 | 131 | plot_states = rospy.get_param('~plot_states', False) 132 | 133 | result_plotter = OcpResultPlotter(plot_states, topic_name) 134 | rate = 2 135 | topic_name = rospy.get_param('~plot_rate', rate) 136 | result_plotter.start(rate) 137 | except rospy.ROSInterruptException: 138 | pass 139 | -------------------------------------------------------------------------------- /mpc_local_planner/src/optimal_control/final_state_conditions_se2.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | namespace mpc_local_planner { 30 | 31 | void QuadraticFinalStateCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, 32 | Eigen::Ref cost) const 33 | { 34 | assert(cost.size() == getNonIntegralStateTermDimension(k)); 35 | 36 | Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); 37 | xd[2] = normalize_theta(xd[2]); 38 | if (_lsq_form) 39 | { 40 | if (_diagonal_mode) 41 | cost.noalias() = _Qf_diag_sqrt * xd; 42 | else 43 | cost.noalias() = _Qf_sqrt * xd; 44 | } 45 | else 46 | { 47 | if (_diagonal_mode) 48 | cost.noalias() = xd.transpose() * _Qf_diag * xd; 49 | else 50 | cost.noalias() = xd.transpose() * _Qf * xd; 51 | } 52 | } 53 | 54 | void TerminalBallSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const 55 | { 56 | assert(cost.size() == getNonIntegralStateTermDimension(k)); 57 | 58 | Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); 59 | xd[2] = normalize_theta(xd[2]); 60 | if (_diagonal_mode) 61 | cost[0] = xd.transpose() * _S_diag * xd - _gamma; 62 | else 63 | cost[0] = xd.transpose() * _S * xd - _gamma; 64 | } 65 | 66 | } // namespace mpc_local_planner 67 | -------------------------------------------------------------------------------- /mpc_local_planner/src/optimal_control/finite_differences_grid_se2.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace mpc_local_planner { 35 | 36 | void FiniteDifferencesGridSE2::createEdges(NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics) 37 | { 38 | assert(isValid()); 39 | 40 | // clear edges first 41 | // TODO(roesmann): we could implement a more efficient strategy without recreating the whole edgeset everytime 42 | edges.clear(); 43 | 44 | int n = getN(); 45 | 46 | std::vector cost_terms, eq_terms, ineq_terms; 47 | for (int k = 0; k < n - 1; ++k) 48 | { 49 | VectorVertexSE2& x_next = (k < n - 2) ? _x_seq[k + 1] : _xf; 50 | VectorVertex& u_prev = (k > 0) ? _u_seq[k - 1] : _u_prev; 51 | ScalarVertex& dt_prev = (k > 0) ? _dt : _u_prev_dt; 52 | 53 | cost_terms.clear(); 54 | eq_terms.clear(); 55 | ineq_terms.clear(); 56 | nlp_fun.getNonIntegralStageFunctionEdges(k, _x_seq[k], _u_seq[k], _dt, u_prev, dt_prev, cost_terms, eq_terms, ineq_terms); 57 | for (BaseEdge::Ptr& edge : cost_terms) edges.addObjectiveEdge(edge); 58 | for (BaseEdge::Ptr& edge : eq_terms) edges.addEqualityEdge(edge); 59 | for (BaseEdge::Ptr& edge : ineq_terms) edges.addInequalityEdge(edge); 60 | 61 | if (nlp_fun.stage_cost && nlp_fun.stage_cost->hasIntegralTerms(k)) 62 | { 63 | if (_cost_integration == CostIntegrationRule::TrapezoidalRule) 64 | { 65 | corbo::TrapezoidalIntegralCostEdge::Ptr edge = 66 | std::make_shared(_x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_cost, k); 67 | edges.addObjectiveEdge(edge); 68 | } 69 | else if (_cost_integration == CostIntegrationRule::LeftSum) 70 | { 71 | corbo::LeftSumCostEdge::Ptr edge = std::make_shared(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_cost, k); 72 | edges.addObjectiveEdge(edge); 73 | } 74 | else 75 | PRINT_ERROR_NAMED("Cost integration rule not implemented"); 76 | } 77 | 78 | if (nlp_fun.stage_equalities && nlp_fun.stage_equalities->hasIntegralTerms(k)) 79 | { 80 | if (_cost_integration == CostIntegrationRule::TrapezoidalRule) 81 | { 82 | corbo::TrapezoidalIntegralEqualityDynamicsEdge::Ptr edge = std::make_shared( 83 | dynamics, _x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_equalities, k); 84 | edge->setFiniteDifferencesCollocationMethod(_fd_eval); 85 | edges.addEqualityEdge(edge); 86 | } 87 | else if (_cost_integration == CostIntegrationRule::LeftSum) 88 | { 89 | corbo::LeftSumEqualityEdge::Ptr edge = 90 | std::make_shared(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_equalities, k); 91 | edges.addEqualityEdge(edge); 92 | 93 | // system dynamics edge 94 | corbo::FDCollocationEdge::Ptr sys_edge = std::make_shared(dynamics, _x_seq[k], _u_seq[k], x_next, _dt); 95 | sys_edge->setFiniteDifferencesCollocationMethod(_fd_eval); 96 | edges.addEqualityEdge(sys_edge); 97 | } 98 | else 99 | PRINT_ERROR_NAMED("Cost integration rule not implemented"); 100 | } 101 | else 102 | { 103 | // just the system dynamics edge 104 | corbo::FDCollocationEdge::Ptr edge = std::make_shared(dynamics, _x_seq[k], _u_seq[k], x_next, _dt); 105 | edge->setFiniteDifferencesCollocationMethod(_fd_eval); 106 | edges.addEqualityEdge(edge); 107 | } 108 | 109 | if (nlp_fun.stage_inequalities && nlp_fun.stage_inequalities->hasIntegralTerms(k)) 110 | { 111 | if (_cost_integration == CostIntegrationRule::TrapezoidalRule) 112 | { 113 | corbo::TrapezoidalIntegralInequalityEdge::Ptr edge = 114 | std::make_shared(_x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_inequalities, k); 115 | edges.addInequalityEdge(edge); 116 | } 117 | else if (_cost_integration == CostIntegrationRule::LeftSum) 118 | { 119 | corbo::LeftSumInequalityEdge::Ptr edge = 120 | std::make_shared(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_inequalities, k); 121 | edges.addInequalityEdge(edge); 122 | } 123 | else 124 | PRINT_ERROR_NAMED("Cost integration rule not implemented"); 125 | } 126 | } 127 | 128 | // check if we have a separate unfixed final state 129 | if (!_xf.isFixed()) 130 | { 131 | // set final state cost 132 | BaseEdge::Ptr cost_edge = nlp_fun.getFinalStateCostEdge(n - 1, _xf); 133 | if (cost_edge) edges.addObjectiveEdge(cost_edge); 134 | 135 | // set final state constraint 136 | BaseEdge::Ptr constr_edge = nlp_fun.getFinalStateConstraintEdge(n - 1, _xf); 137 | if (constr_edge) 138 | { 139 | if (nlp_fun.final_stage_constraints->isEqualityConstraint()) 140 | edges.addEqualityEdge(constr_edge); 141 | else 142 | edges.addInequalityEdge(constr_edge); 143 | } 144 | } 145 | 146 | // add control deviation edges for last control 147 | cost_terms.clear(); 148 | eq_terms.clear(); 149 | ineq_terms.clear(); 150 | nlp_fun.getFinalControlDeviationEdges(n, _u_ref, _u_seq.back(), _dt, cost_terms, eq_terms, ineq_terms); 151 | for (BaseEdge::Ptr& edge : cost_terms) edges.addObjectiveEdge(edge); 152 | for (BaseEdge::Ptr& edge : eq_terms) edges.addEqualityEdge(edge); 153 | for (BaseEdge::Ptr& edge : ineq_terms) edges.addInequalityEdge(edge); 154 | } 155 | 156 | } // namespace mpc_local_planner 157 | -------------------------------------------------------------------------------- /mpc_local_planner/src/optimal_control/finite_differences_variable_grid_se2.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace mpc_local_planner { 35 | 36 | void FiniteDifferencesVariableGridSE2::setDtBounds(double dt_lb, double dt_ub) 37 | { 38 | _dt_lb = dt_lb; 39 | _dt_ub = dt_ub; 40 | } 41 | 42 | void FiniteDifferencesVariableGridSE2::setGridAdaptTimeBasedSingleStep(int n_max, double dt_hyst_ratio, bool adapt_first_iter) 43 | { 44 | _grid_adapt = GridAdaptStrategy::TimeBasedSingleStep; 45 | _n_max = n_max; 46 | _dt_hyst_ratio = dt_hyst_ratio; 47 | _adapt_first_iter = adapt_first_iter; 48 | } 49 | 50 | void FiniteDifferencesVariableGridSE2::setGridAdaptTimeBasedAggressiveEstimate(int n_max, double dt_hyst_ratio, bool adapt_first_iter) 51 | { 52 | _grid_adapt = GridAdaptStrategy::TimeBasedAggressiveEstimate; 53 | _n_max = n_max; 54 | _dt_hyst_ratio = dt_hyst_ratio; 55 | _adapt_first_iter = adapt_first_iter; 56 | } 57 | 58 | void FiniteDifferencesVariableGridSE2::setGridAdaptSimpleShrinkingHorizon(bool adapt_first_iter) 59 | { 60 | _grid_adapt = GridAdaptStrategy::SimpleShrinkingHorizon; 61 | _adapt_first_iter = adapt_first_iter; 62 | } 63 | 64 | bool FiniteDifferencesVariableGridSE2::adaptGrid(bool new_run, NlpFunctions& nlp_fun) 65 | { 66 | // do not adapt grid in a new run 67 | if (new_run && !_adapt_first_iter) return false; 68 | 69 | bool changed = false; 70 | switch (_grid_adapt) 71 | { 72 | case GridAdaptStrategy::NoGridAdapt: 73 | { 74 | break; 75 | } 76 | case GridAdaptStrategy::TimeBasedSingleStep: 77 | { 78 | changed = adaptGridTimeBasedSingleStep(nlp_fun); 79 | break; 80 | } 81 | case GridAdaptStrategy::TimeBasedAggressiveEstimate: 82 | { 83 | changed = adaptGridTimeBasedAggressiveEstimate(nlp_fun); 84 | break; 85 | } 86 | case GridAdaptStrategy::SimpleShrinkingHorizon: 87 | { 88 | changed = adaptGridSimpleShrinkingHorizon(nlp_fun); 89 | break; 90 | } 91 | default: 92 | { 93 | PRINT_ERROR_NAMED("selected grid adaptation strategy not implemented."); 94 | } 95 | } 96 | return changed; 97 | } 98 | 99 | bool FiniteDifferencesVariableGridSE2::adaptGridTimeBasedSingleStep(NlpFunctions& nlp_fun) 100 | { 101 | PRINT_WARNING_COND_NAMED(!isTimeVariableGrid(), "time based adaptation might only be used with a fixed dt."); 102 | 103 | _nlp_fun = &nlp_fun; 104 | 105 | int n = getN(); 106 | 107 | double dt = getDt(); 108 | if (dt > _dt_ref * (1.0 + _dt_hyst_ratio) && n < _n_max) 109 | { 110 | resampleTrajectory(n + 1); 111 | _n_adapt = n + 1; 112 | return true; 113 | } 114 | else if (dt < _dt_ref * (1.0 - _dt_hyst_ratio) && n > _n_min) 115 | { 116 | resampleTrajectory(n - 1); 117 | _n_adapt = n - 1; 118 | return true; 119 | } 120 | return false; 121 | } 122 | 123 | bool FiniteDifferencesVariableGridSE2::adaptGridTimeBasedAggressiveEstimate(NlpFunctions& nlp_fun) 124 | { 125 | PRINT_WARNING_COND_NAMED(!isTimeVariableGrid(), "time based adaptation might only be used with a fixed dt."); 126 | 127 | _nlp_fun = &nlp_fun; 128 | int n = getN(); 129 | double dt = getDt(); 130 | 131 | // check if hysteresis is satisfied 132 | if (dt >= _dt_ref * (1.0 - _dt_hyst_ratio) && dt <= _dt_ref * (1.0 + _dt_hyst_ratio)) return false; 133 | 134 | // estimate number of samples based on the fraction dt/dt_ref. 135 | // dt is the time difference obtained in a previous solution (with a coarser resp. finer trajectory resolution) 136 | int new_n = std::round((double)n * (dt / _dt_ref)); 137 | 138 | // bound value 139 | if (new_n > _n_max) 140 | new_n = _n_max; 141 | else if (new_n < _n_min) 142 | new_n = _n_min; 143 | 144 | if (new_n == n) return false; 145 | 146 | // and now resample 147 | resampleTrajectory(new_n); 148 | _n_adapt = new_n; 149 | return true; 150 | } 151 | 152 | bool FiniteDifferencesVariableGridSE2::adaptGridSimpleShrinkingHorizon(NlpFunctions& nlp_fun) 153 | { 154 | int n = getN(); 155 | if (n > _n_min) 156 | { 157 | resampleTrajectory(n - 1); 158 | _n_adapt = n - 1; 159 | } 160 | return false; 161 | } 162 | 163 | } // namespace mpc_local_planner 164 | -------------------------------------------------------------------------------- /mpc_local_planner/src/optimal_control/min_time_via_points_cost.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | namespace mpc_local_planner { 32 | 33 | int MinTimeViaPointsCost::getNonIntegralStateTermDimension(int k) const 34 | { 35 | assert(k < _vp_association.size()); 36 | assert(_vp_association[k].first.size() == _vp_association[k].second); 37 | return (int)_vp_association[k].second; 38 | } 39 | 40 | bool MinTimeViaPointsCost::update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/, 41 | corbo::ReferenceTrajectoryInterface* /*sref*/, bool single_dt, const Eigen::VectorXd& x0, 42 | corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector& /*dts*/, 43 | const corbo::DiscretizationGridInterface* grid) 44 | { 45 | if (!_via_points) 46 | { 47 | PRINT_WARNING("MinTimeViaPointsCost::update(): no via_point container specified"); 48 | return !_vp_association.empty(); 49 | } 50 | 51 | // setup minimum time cost 52 | _single_dt = single_dt; 53 | if (single_dt) 54 | _time_weight = n - 1; 55 | else 56 | _time_weight = 1.0; 57 | 58 | // Setup via points 59 | 60 | // we currently require a full discretization grid as we want to have fast access to 61 | // individual states without requiring any new simulation. 62 | // Alternatively, other grids could be used in combination with method getStateAndControlTimeSeries() 63 | const FullDiscretizationGridBaseSE2* fd_grid = static_cast(grid); 64 | 65 | assert(n == fd_grid->getN()); 66 | 67 | bool new_structure = (n != _vp_association.size()); 68 | if (new_structure) 69 | { 70 | _vp_association.resize(n, std::make_pair, std::size_t>({}, 0)); 71 | } 72 | 73 | // clear previous association 74 | for (auto& item : _vp_association) 75 | { 76 | item.first.clear(); 77 | } 78 | 79 | int start_pose_idx = 0; 80 | 81 | for (const teb_local_planner::PoseSE2& pose : *_via_points) 82 | { 83 | int idx = fd_grid->findClosestPose(pose.x(), pose.y(), start_pose_idx); 84 | 85 | if (_via_points_ordered) start_pose_idx = idx + 2; // skip a point to have a DOF inbetween for further via-points 86 | 87 | // check if point coincides with final state or is located behind it 88 | if (idx > n - 2) idx = n - 2; // set to a pose before the goal, since it is never fixed 89 | 90 | // check if point coincides with start or is located before it 91 | if (idx < 1) 92 | { 93 | if (_via_points_ordered) 94 | idx = 1; // try to connect the via point with the second pose. Grid adaptation might add new poses inbetween later if enabled. 95 | else 96 | { 97 | PRINT_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose."); 98 | continue; // skip via points really close or behind the current robot pose 99 | } 100 | } 101 | 102 | _vp_association[idx].first.push_back(&pose); 103 | } 104 | 105 | // check for structure changes 106 | for (auto& item : _vp_association) 107 | { 108 | if (item.first.size() != item.second) 109 | { 110 | item.second = item.first.size(); 111 | new_structure = true; 112 | // but continue until end for next time 113 | } 114 | } 115 | 116 | // return true if structure requires a changed 117 | return new_structure; 118 | } 119 | 120 | void MinTimeViaPointsCost::computeNonIntegralDtTerm(int k, double dt, Eigen::Ref cost) const 121 | { 122 | if (!_single_dt || k == 0) 123 | cost[0] = _time_weight * dt; 124 | else 125 | { 126 | PRINT_DEBUG_NAMED("this method should not be called in single_dt mode and k>0"); 127 | } 128 | } 129 | 130 | void MinTimeViaPointsCost::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const 131 | { 132 | assert(_via_points); 133 | assert(k < _vp_association.size()); 134 | assert(cost.size() == _vp_association[k].first.size()); 135 | assert(_vp_association[k].second == _vp_association[k].first.size()); 136 | 137 | for (int i = 0; i < (int)_vp_association[k].second; ++i) 138 | { 139 | cost[i] = _vp_position_weight * (_vp_association[k].first[i]->position() - x_k.head(2)).squaredNorm(); 140 | if (_vp_orientation_weight > 0) 141 | { 142 | cost[i] += _vp_orientation_weight * normalize_theta(_vp_association[k].first[i]->theta() - x_k[2]); 143 | } 144 | } 145 | } 146 | 147 | } // namespace mpc_local_planner 148 | -------------------------------------------------------------------------------- /mpc_local_planner/src/optimal_control/quadratic_cost_se2.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | namespace mpc_local_planner { 30 | 31 | void QuadraticFormCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const 32 | { 33 | assert(!_integral_form); 34 | assert(cost.size() == getNonIntegralStateTermDimension(k)); 35 | 36 | Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); 37 | xd[2] = normalize_theta(xd[2]); 38 | if (_lsq_form) 39 | { 40 | if (_Q_diagonal_mode) 41 | cost.noalias() = _Q_diag_sqrt * xd; 42 | else 43 | cost.noalias() = _Q_sqrt * xd; 44 | } 45 | else 46 | { 47 | if (_Q_diagonal_mode) 48 | cost.noalias() = xd.transpose() * _Q_diag * xd; 49 | else 50 | cost.noalias() = xd.transpose() * _Q * xd; 51 | } 52 | } 53 | 54 | void QuadraticFormCostSE2::computeIntegralStateControlTerm(int k, const Eigen::Ref& x_k, 55 | const Eigen::Ref& u_k, Eigen::Ref cost) const 56 | { 57 | assert(_integral_form); 58 | assert(cost.size() == 1); 59 | 60 | cost[0] = 0; 61 | 62 | Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); 63 | xd[2] = normalize_theta(xd[2]); 64 | if (_Q_diagonal_mode) 65 | cost[0] += xd.transpose() * _Q_diag * xd; 66 | else 67 | cost[0] += xd.transpose() * _Q * xd; 68 | 69 | if (_zero_u_ref) 70 | { 71 | if (_R_diagonal_mode) 72 | cost[0] += u_k.transpose() * _R_diag * u_k; 73 | else 74 | cost[0] += u_k.transpose() * _R * u_k; 75 | } 76 | else 77 | { 78 | Eigen::VectorXd ud = u_k - _u_ref->getReferenceCached(k); 79 | if (_R_diagonal_mode) 80 | cost[0] += ud.transpose() * _R_diag * ud; 81 | else 82 | cost[0] += ud.transpose() * _R * ud; 83 | } 84 | } 85 | 86 | void QuadraticStateCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const 87 | { 88 | assert(!_integral_form); 89 | assert(cost.size() == getNonIntegralStateTermDimension(k)); 90 | 91 | Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); 92 | xd[2] = normalize_theta(xd[2]); 93 | if (_lsq_form) 94 | { 95 | if (_diagonal_mode) 96 | cost.noalias() = _Q_diag_sqrt * xd; 97 | else 98 | cost.noalias() = _Q_sqrt * xd; 99 | } 100 | else 101 | { 102 | if (_diagonal_mode) 103 | cost.noalias() = xd.transpose() * _Q_diag * xd; 104 | else 105 | cost.noalias() = xd.transpose() * _Q * xd; 106 | } 107 | } 108 | 109 | void QuadraticStateCostSE2::computeIntegralStateControlTerm(int k, const Eigen::Ref& x_k, 110 | const Eigen::Ref& u_k, Eigen::Ref cost) const 111 | { 112 | assert(_integral_form); 113 | assert(cost.size() == 1); 114 | 115 | cost[0] = 0; 116 | 117 | Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); 118 | xd[2] = normalize_theta(xd[2]); 119 | if (_diagonal_mode) 120 | cost[0] += xd.transpose() * _Q_diag * xd; 121 | else 122 | cost[0] += xd.transpose() * _Q * xd; 123 | } 124 | 125 | } // namespace mpc_local_planner 126 | -------------------------------------------------------------------------------- /mpc_local_planner/src/utils/conversion.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace mpc_local_planner { 28 | 29 | void convert(const corbo::TimeSeries& time_series, const RobotDynamicsInterface& dynamics, std::vector& poses_stamped, 30 | const std::string& frame_id) 31 | { 32 | poses_stamped.clear(); 33 | 34 | if (time_series.isEmpty()) return; 35 | 36 | for (int i = 0; i < time_series.getTimeDimension(); ++i) 37 | { 38 | poses_stamped.emplace_back(); 39 | 40 | double theta = 0; 41 | dynamics.getPoseSE2FromState(time_series.getValuesMap(i), poses_stamped.back().pose.position.x, poses_stamped.back().pose.position.y, theta); 42 | poses_stamped.back().pose.orientation = tf::createQuaternionMsgFromYaw(theta); 43 | poses_stamped.back().header.frame_id = frame_id; 44 | poses_stamped.back().header.stamp = ros::Time::now(); // TODO(roesmann) should we use now()? 45 | } 46 | } 47 | 48 | } // namespace mpc_local_planner 49 | -------------------------------------------------------------------------------- /mpc_local_planner/src/utils/time_series_se2.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement 4 | * 5 | * Copyright (c) 2020, Christoph Rösmann, All rights reserved. 6 | * 7 | * This program is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * This program is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with this program. If not, see . 19 | * 20 | * Authors: Christoph Rösmann 21 | *********************************************************************/ 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace mpc_local_planner { 33 | 34 | bool TimeSeriesSE2::getValuesInterpolate(double time, Eigen::Ref values, Interpolation interpolation, Extrapolation extrapolation, 35 | double tolerance) const 36 | { 37 | if (_time.empty()) return false; 38 | 39 | auto it = std::find_if(_time.begin(), _time.end(), [time](double val) { return val >= time; }); // find next time interval 40 | if (it == _time.end()) 41 | { 42 | switch (extrapolation) 43 | { 44 | case Extrapolation::NoExtrapolation: 45 | { 46 | break; 47 | } 48 | case Extrapolation::ZeroOrderHold: 49 | { 50 | values = getValuesMap(getTimeDimension() - 1); 51 | return true; 52 | } 53 | default: 54 | { 55 | PRINT_ERROR("TimeSeries::valuesInterpolate(): desired extrapolation method not implemented."); 56 | return false; 57 | } 58 | } 59 | } 60 | // interpolate 61 | // int n = (int)_time.size(); 62 | int idx = (int)std::distance(_time.begin(), it); 63 | if (idx >= getTimeDimension()) idx = getTimeDimension() - 1; // this might occure with floating point comparison 64 | if (std::abs(time - _time[idx]) < tolerance) 65 | { 66 | // we are close to the next value, in particular idx 67 | values = getValuesMap(idx); 68 | return true; 69 | } 70 | if (idx < 1) 71 | { 72 | PRINT_ERROR("accessing a time idx in the past which is not this time series"); 73 | } 74 | // okay, so now we have the above case val > time -> so idx corresponds to the next sample. 75 | double dt = time - _time[idx - 1]; 76 | PRINT_ERROR_COND_NAMED(dt < 0, "dt < 0 in interpolation. This cannot be correct."); 77 | 78 | switch (interpolation) 79 | { 80 | case Interpolation::ZeroOrderHold: 81 | { 82 | if (idx < 1) idx = 1; 83 | values = getValuesMap(idx - 1); // since dt > 0 -> we use the previous value 84 | break; 85 | } 86 | case Interpolation::Linear: 87 | { 88 | if (idx < 1) 89 | { 90 | values = getValuesMap(0); 91 | } 92 | else 93 | { 94 | double dt_data = _time[idx] - _time[idx - 1]; 95 | double dt_frac = dt / dt_data; 96 | values.noalias() = getValuesMap(idx - 1) + dt_frac * (getValuesMap(idx) - getValuesMap(idx - 1)); 97 | // overwrite third component with is an angle in [-pi, pi) 98 | if (values.size() > 2) 99 | { 100 | values[2] = interpolate_angle(getValuesMap(idx - 1)[2], getValuesMap(idx)[2], dt_frac); 101 | } 102 | } 103 | break; 104 | } 105 | default: 106 | { 107 | PRINT_ERROR("TimeSeries::valuesInterpolate(): desired interpolation method not implemented."); 108 | return false; 109 | } 110 | } 111 | return true; 112 | } 113 | 114 | double TimeSeriesSE2::computeMeanOverall() 115 | { 116 | PRINT_ERROR_NAMED("SE2 version not yet implemented."); 117 | return getValuesMatrixView().mean(); 118 | } 119 | 120 | void TimeSeriesSE2::computeMeanCwise(Eigen::Ref mean_values) 121 | { 122 | PRINT_ERROR_NAMED("SE2 version Not yet implemented."); 123 | if (mean_values.size() != getValueDimension()) 124 | { 125 | PRINT_ERROR("TimeSeries::computeMeanCwise(): provided mean_values vector does not match value dimension"); 126 | return; 127 | } 128 | mean_values = getValuesMatrixView().rowwise().mean(); 129 | } 130 | 131 | } // namespace mpc_local_planner 132 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mpc_local_planner_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.3 (2020-06-09) 6 | ------------------ 7 | * Added feasibility check with costmap robot model 8 | * Changed obstacle parameters `cutoff_factor` and `force_inclusion_factor` to `cutoff_dist` and `force_inclusion_dist` 9 | * Dynamic obstacles: the inequality constraints now use the actual time parameter rather than the time from the previous optimization 10 | * Changed minimum CMake version to 3.1 11 | * Contributors: Christoph Rösmann 12 | 13 | 0.0.2 (2020-03-12) 14 | ------------------ 15 | * Added dependency on mpc_local_planner_msgs package 16 | * Default parameter set changed 17 | * Contributors: Christoph Rösmann 18 | 19 | 0.0.1 (2020-02-20) 20 | ------------------ 21 | * First release 22 | * Contributors: Christoph Rösmann 23 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(mpc_local_planner_examples) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES teb_local_planner_tutorials 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(teb_local_planner_tutorials 115 | # src/${PROJECT_NAME}/teb_local_planner_tutorials.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(teb_local_planner_tutorials ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(teb_local_planner_tutorials_node src/teb_local_planner_tutorials_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(teb_local_planner_tutorials_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(teb_local_planner_tutorials_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS teb_local_planner_tutorials teb_local_planner_tutorials_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | install(DIRECTORY 157 | launch cfg maps stage 158 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 159 | ) 160 | 161 | ## Mark other files for installation (e.g. launch and bag files, etc.) 162 | # install(FILES 163 | # # myfile1 164 | # # myfile2 165 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 166 | # ) 167 | 168 | ############# 169 | ## Testing ## 170 | ############# 171 | 172 | ## Add gtest based cpp test target and link libraries 173 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_teb_local_planner_tutorials.cpp) 174 | # if(TARGET ${PROJECT_NAME}-test) 175 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 176 | # endif() 177 | 178 | ## Add folders to be run by python nosetests 179 | # catkin_add_nosetests(test) 180 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/README.md: -------------------------------------------------------------------------------- 1 | # mpc_local_planner_tutorials 2 | This package contains supplementary material and examples for the [mpc_local_planner](http://wiki.ros.org/mpc_local_planner). 3 | 4 | The examples package mainly contains fully working robot navigation configurations in combination with the *mpc_local_planner*. 5 | 6 | **Dependencies:** 7 | 8 | * *navigation stack* and *mpc_local_planner* package 9 | * *stage*: `sudo apt-get install ros-melodic-stage-ros` 10 | 11 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/amcl_params.yaml: -------------------------------------------------------------------------------- 1 | use_map_topic: true 2 | 3 | odom_frame_id: "odom" 4 | base_frame_id: "base_footprint" 5 | global_frame_id: "map" 6 | 7 | ## Publish scans from best pose at a max of 10 Hz 8 | odom_model_type: "diff" 9 | odom_alpha5: 0.1 10 | gui_publish_rate: 10.0 11 | laser_max_beams: 60 12 | laser_max_range: 12.0 13 | min_particles: 500 14 | max_particles: 2000 15 | kld_err: 0.05 16 | kld_z: 0.99 17 | odom_alpha1: 0.2 18 | odom_alpha2: 0.2 19 | ## translation std dev, m 20 | odom_alpha3: 0.2 21 | odom_alpha4: 0.2 22 | laser_z_hit: 0.5 23 | aser_z_short: 0.05 24 | laser_z_max: 0.05 25 | laser_z_rand: 0.5 26 | laser_sigma_hit: 0.2 27 | laser_lambda_short: 0.1 28 | laser_model_type: "likelihood_field" # "likelihood_field" or "beam" 29 | laser_likelihood_max_dist: 2.0 30 | update_min_d: 0.25 31 | update_min_a: 0.2 32 | 33 | resample_interval: 1 34 | 35 | ## Increase tolerance because the computer can get quite busy 36 | transform_tolerance: 1.0 37 | recovery_alpha_slow: 0.001 38 | recovery_alpha_fast: 0.1 39 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/carlike/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | #---standard pioneer footprint--- 3 | #---(in meters)--- 4 | #footprint: [ [0.254, -0.0508], [0.1778, -0.0508], [0.1778, -0.1778], [-0.1905, -0.1778], [-0.254, 0], [-0.1905, 0.1778], [0.1778, 0.1778], [0.1778, 0.0508], [0.254, 0.0508] ] 5 | footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ] 6 | 7 | transform_tolerance: 0.2 8 | 9 | obstacle_layer: 10 | enabled: true 11 | obstacle_range: 3.0 12 | raytrace_range: 3.5 13 | inflation_radius: 0.2 14 | track_unknown_space: false 15 | combination_method: 1 16 | 17 | observation_sources: laser_scan_sensor 18 | laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true} 19 | 20 | 21 | inflation_layer: 22 | enabled: true 23 | cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) 24 | inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. 25 | 26 | static_layer: 27 | enabled: true 28 | map_topic: "/map" 29 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/carlike/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | 7 | transform_tolerance: 0.5 8 | plugins: 9 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 10 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 11 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/carlike/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | rolling_window: true 7 | width: 5.5 8 | height: 5.5 9 | resolution: 0.1 10 | transform_tolerance: 0.5 11 | 12 | plugins: 13 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 14 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 15 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | MpcLocalPlannerROS: 2 | 3 | odom_topic: odom 4 | 5 | ## Robot settings 6 | robot: 7 | type: "simple_car" 8 | simple_car: 9 | wheelbase: 0.4 10 | front_wheel_driving: False 11 | max_vel_x: 0.4 12 | max_vel_x_backwards: 0.2 13 | max_steering_angle: 1.4 14 | acc_lim_x: 0.5 # deactive bounds with zero 15 | dec_lim_x: 0.5 # deactive bounds with zero 16 | max_steering_rate: 0.5 # deactive bounds with zero 17 | 18 | ## Footprint model for collision avoidance 19 | footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 20 | type: "line" 21 | radius: 0.2 # for type "circular" 22 | line_start: [0.0, 0.0] # for type "line" 23 | line_end: [0.4, 0.0] # for type "line" 24 | front_offset: 0.2 # for type "two_circles" 25 | front_radius: 0.2 # for type "two_circles" 26 | rear_offset: 0.2 # for type "two_circles" 27 | rear_radius: 0.2 # for type "two_circles" 28 | vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon" 29 | is_footprint_dynamic: False 30 | 31 | 32 | ## Collision avoidance 33 | collision_avoidance: 34 | min_obstacle_dist: 0.27 # Note, this parameter must be chosen w.r.t. the footprint_model 35 | enable_dynamic_obstacles: False 36 | force_inclusion_dist: 0.5 37 | cutoff_dist: 2.5 38 | include_costmap_obstacles: True 39 | costmap_obstacles_behind_robot_dist: 1.0 40 | collision_check_no_poses: 5 41 | 42 | 43 | ## Planning grid 44 | grid: 45 | type: "fd_grid" 46 | grid_size_ref: 20 47 | dt_ref: 0.3 48 | xf_fixed: [True, True, True] 49 | warm_start: True 50 | collocation_method: "forward_differences" 51 | cost_integration_method: "left_sum" 52 | variable_grid: 53 | enable: True 54 | min_dt: 0.0; 55 | max_dt: 10.0; 56 | grid_adaptation: 57 | enable: True 58 | dt_hyst_ratio: 0.1 59 | min_grid_size: 2 60 | max_grid_size: 50 61 | 62 | ## Planning options 63 | planning: 64 | objective: 65 | type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly 66 | terminal_cost: 67 | type: "none" 68 | terminal_constraint: 69 | type: "none" 70 | 71 | ## Controller options 72 | controller: 73 | outer_ocp_iterations: 1 74 | xy_goal_tolerance: 0.2 75 | yaw_goal_tolerance: 0.1 76 | global_plan_overwrite_orientation: True 77 | global_plan_prune_distance: 1.0 78 | allow_init_with_backward_motion: True 79 | max_global_plan_lookahead_dist: 1.5 80 | force_reinit_new_goal_dist: 1.0 81 | force_reinit_new_goal_angular: 1.57 82 | prefer_x_feedback: False 83 | publish_ocp_results: False 84 | 85 | ## Solver settings 86 | solver: 87 | type: "ipopt" 88 | ipopt: 89 | iterations: 100 90 | max_cpu_time: -1.0 91 | ipopt_numeric_options: 92 | tol: 1e-4 93 | ipopt_string_options: 94 | linear_solver: "mumps" 95 | hessian_approximation: "limited-memory" # exact/limited-memory, WARNING 'exact' does currently not work well with the carlike model 96 | lsq_lm: 97 | iterations: 10 98 | weight_init_eq: 2 99 | weight_init_ineq: 2 100 | weight_init_bounds: 2 101 | weight_adapt_factor_eq: 1.5 102 | weight_adapt_factor_ineq: 1.5 103 | weight_adapt_factor_bounds: 1.5 104 | weight_adapt_max_eq: 500 105 | weight_adapt_max_ineq: 500 106 | weight_adapt_max_bounds: 500 107 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/diff_drive/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | #---standard pioneer footprint--- 3 | #---(in meters)--- 4 | robot_radius: 0.17 5 | footprint_padding: 0.00 6 | 7 | transform_tolerance: 0.2 8 | 9 | always_send_full_costmap: true 10 | 11 | obstacle_layer: 12 | enabled: true 13 | obstacle_range: 3.0 14 | raytrace_range: 4.0 15 | inflation_radius: 0.2 16 | track_unknown_space: true 17 | combination_method: 1 18 | 19 | observation_sources: laser_scan_sensor 20 | laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true} 21 | 22 | 23 | inflation_layer: 24 | enabled: true 25 | cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) 26 | inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. 27 | 28 | static_layer: 29 | enabled: true 30 | map_topic: "/map" 31 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/diff_drive/costmap_converter_params.yaml: -------------------------------------------------------------------------------- 1 | ########################################################################################### 2 | ## NOTE: Costmap conversion is experimental. Its purpose is to combine many point ## 3 | ## obstales into clusters, computed in a separate thread in order to improve the overall ## 4 | ## efficiency of local planning. However, the implemented conversion algorithms are in a ## 5 | ## very early stage of development. Contributions are welcome! ## 6 | ########################################################################################### 7 | 8 | MpcLocalPlannerROS: 9 | 10 | ## Costmap converter plugin 11 | #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" 12 | costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC" 13 | #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH" 14 | #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull" 15 | costmap_converter_spin_thread: True 16 | costmap_converter_rate: 5 17 | 18 | 19 | ## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons) 20 | ## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH 21 | costmap_converter/CostmapToLinesDBSRANSAC: 22 | cluster_max_distance: 0.4 23 | cluster_min_pts: 2 24 | ransac_inlier_distance: 0.15 25 | ransac_min_inliers: 10 26 | ransac_no_iterations: 1500 27 | ransac_remainig_outliers: 3 28 | ransac_convert_outlier_pts: True 29 | ransac_filter_remaining_outlier_pts: False 30 | convex_hull_min_pt_separation: 0.1 31 | 32 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/diff_drive/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | 7 | transform_tolerance: 0.5 8 | plugins: 9 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 10 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 11 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/diff_drive/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | rolling_window: true 7 | width: 5.5 8 | height: 5.5 9 | resolution: 0.1 10 | transform_tolerance: 0.5 11 | 12 | plugins: 13 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 14 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 15 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml: -------------------------------------------------------------------------------- 1 | MpcLocalPlannerROS: 2 | 3 | odom_topic: odom 4 | 5 | ## Robot settings 6 | robot: 7 | type: "unicycle" 8 | unicycle: 9 | max_vel_x: 0.4 10 | max_vel_x_backwards: 0.2 11 | max_vel_theta: 0.3 12 | acc_lim_x: 0.2 # deactive bounds with zero 13 | dec_lim_x: 0.2 # deactive bounds with zero 14 | acc_lim_theta: 0.2 # deactivate bounds with zero 15 | 16 | ## Footprint model for collision avoidance 17 | footprint_model: 18 | type: "point" 19 | is_footprint_dynamic: False 20 | 21 | ## Collision avoidance 22 | collision_avoidance: 23 | min_obstacle_dist: 0.2 # Note, this parameter must be chosen w.r.t. the footprint_model 24 | enable_dynamic_obstacles: False 25 | force_inclusion_dist: 0.5 26 | cutoff_dist: 2.5 27 | include_costmap_obstacles: True 28 | costmap_obstacles_behind_robot_dist: 1.5 29 | 30 | ## Planning grid 31 | grid: 32 | type: "fd_grid" 33 | grid_size_ref: 20 34 | dt_ref: 0.3 35 | xf_fixed: [True, True, True] 36 | warm_start: True 37 | collocation_method: "forward_differences" 38 | cost_integration_method: "left_sum" 39 | variable_grid: 40 | enable: True 41 | min_dt: 0.0; 42 | max_dt: 10.0; 43 | grid_adaptation: 44 | enable: True 45 | dt_hyst_ratio: 0.1 46 | min_grid_size: 2 47 | max_grid_size: 50 48 | 49 | ## Planning options 50 | planning: 51 | objective: 52 | type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly 53 | quadratic_form: 54 | state_weights: [2.0, 2.0, 2.0] 55 | control_weights: [1.0, 1.0] 56 | integral_form: False 57 | minimum_time_via_points: 58 | position_weight: 10.5 59 | orientation_weight: 0.0 60 | via_points_ordered: False 61 | terminal_cost: 62 | type: "none" # can be "none" 63 | quadratic: 64 | final_state_weights: [2.0, 2.0, 2.0] 65 | terminal_constraint: 66 | type: "none" # can be "none" 67 | l2_ball: 68 | weight_matrix: [1.0, 1.0, 1.0] 69 | radius: 5 70 | 71 | ## Controller options 72 | controller: 73 | outer_ocp_iterations: 5 74 | xy_goal_tolerance: 0.2 75 | yaw_goal_tolerance: 0.1 76 | global_plan_overwrite_orientation: true 77 | global_plan_prune_distance: 1.0 78 | allow_init_with_backward_motion: True 79 | max_global_plan_lookahead_dist: 1.5 80 | global_plan_viapoint_sep: 5.5 81 | force_reinit_new_goal_dist: 1.0 82 | force_reinit_new_goal_angular: 1.57 83 | force_reinit_num_steps: 0 84 | prefer_x_feedback: False 85 | publish_ocp_results: False 86 | 87 | ## Solver settings 88 | solver: 89 | type: "ipopt" 90 | ipopt: 91 | iterations: 100 92 | max_cpu_time: -1.0 93 | ipopt_numeric_options: 94 | tol: 1e-4 95 | ipopt_string_options: 96 | linear_solver: "mumps" 97 | hessian_approximation: "exact" # exact or limited-memory 98 | lsq_lm: 99 | iterations: 10 100 | weight_init_eq: 2 101 | weight_init_ineq: 2 102 | weight_init_bounds: 2 103 | weight_adapt_factor_eq: 1.5 104 | weight_adapt_factor_ineq: 1.5 105 | weight_adapt_factor_bounds: 1.5 106 | weight_adapt_max_eq: 500 107 | weight_adapt_max_ineq: 500 108 | weight_adapt_max_bounds: 500 109 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml: -------------------------------------------------------------------------------- 1 | MpcLocalPlannerROS: 2 | 3 | odom_topic: odom 4 | 5 | ## Robot settings 6 | robot: 7 | type: "unicycle" 8 | unicycle: 9 | max_vel_x: 0.4 10 | max_vel_x_backwards: 0.2 11 | max_vel_theta: 0.3 12 | acc_lim_x: 0.2 # deactive bounds with zero 13 | dec_lim_x: 0.2 # deactive bounds with zero 14 | acc_lim_theta: 0.2 # deactivate bounds with zero 15 | 16 | ## Footprint model for collision avoidance 17 | footprint_model: 18 | type: "point" 19 | is_footprint_dynamic: False 20 | 21 | ## Collision avoidance 22 | collision_avoidance: 23 | min_obstacle_dist: 0.2 # Note, this parameter must be chosen w.r.t. the footprint_model 24 | enable_dynamic_obstacles: False 25 | force_inclusion_dist: 0.5 26 | cutoff_dist: 2.5 27 | include_costmap_obstacles: True 28 | costmap_obstacles_behind_robot_dist: 1.5 29 | collision_check_no_poses: 5 30 | 31 | ## Planning grid 32 | grid: 33 | type: "fd_grid" 34 | grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist 35 | dt_ref: 0.3 # and here the corresponding temporal resolution 36 | xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below 37 | warm_start: True 38 | collocation_method: "forward_differences" 39 | cost_integration_method: "left_sum" 40 | variable_grid: 41 | enable: False # We want a fixed grid 42 | min_dt: 0.0; 43 | max_dt: 10.0; 44 | grid_adaptation: 45 | enable: True 46 | dt_hyst_ratio: 0.1 47 | min_grid_size: 2 48 | max_grid_size: 50 49 | 50 | ## Planning options 51 | planning: 52 | objective: 53 | type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly 54 | quadratic_form: 55 | state_weights: [2.0, 2.0, 0.25] 56 | control_weights: [0.1, 0.05] 57 | integral_form: False 58 | terminal_cost: 59 | type: "quadratic" # can be "none" 60 | quadratic: 61 | final_state_weights: [10.0, 10.0, 0.5] 62 | terminal_constraint: 63 | type: "none" # can be "none" 64 | l2_ball: 65 | weight_matrix: [1.0, 1.0, 1.0] 66 | radius: 5 67 | 68 | ## Controller options 69 | controller: 70 | outer_ocp_iterations: 1 71 | xy_goal_tolerance: 0.2 72 | yaw_goal_tolerance: 0.1 73 | global_plan_overwrite_orientation: true 74 | global_plan_prune_distance: 1.0 75 | allow_init_with_backward_motion: True 76 | max_global_plan_lookahead_dist: 1.0 # Check horizon length 77 | force_reinit_new_goal_dist: 1.0 78 | force_reinit_new_goal_angular: 1.57 79 | force_reinit_num_steps: 0 80 | prefer_x_feedback: False 81 | publish_ocp_results: False 82 | 83 | ## Solver settings 84 | solver: 85 | type: "ipopt" 86 | ipopt: 87 | iterations: 100 88 | max_cpu_time: -1.0 89 | ipopt_numeric_options: 90 | tol: 1e-4 91 | ipopt_string_options: 92 | linear_solver: "mumps" 93 | hessian_approximation: "exact" # exact or limited-memory 94 | lsq_lm: 95 | iterations: 10 96 | weight_init_eq: 2 97 | weight_init_ineq: 2 98 | weight_init_bounds: 2 99 | weight_adapt_factor_eq: 1.5 100 | weight_adapt_factor_ineq: 1.5 101 | weight_adapt_factor_bounds: 1.5 102 | weight_adapt_max_eq: 500 103 | weight_adapt_max_ineq: 500 104 | weight_adapt_max_bounds: 500 105 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/launch/carlike_minimum_time.launch: -------------------------------------------------------------------------------- 1 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/launch/diff_drive_minimum_time.launch: -------------------------------------------------------------------------------- 1 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/launch/diff_drive_minimum_time_costmap_conversion.launch: -------------------------------------------------------------------------------- 1 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/launch/diff_drive_quadratic_form.launch: -------------------------------------------------------------------------------- 1 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/maps/corridor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rst-tu-dortmund/mpc_local_planner/5b4e465fec718245484e8668e6dd45539ca37983/mpc_local_planner_examples/maps/corridor.png -------------------------------------------------------------------------------- /mpc_local_planner_examples/maps/corridor.yaml: -------------------------------------------------------------------------------- 1 | image: corridor.png 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/maps/empty_box.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rst-tu-dortmund/mpc_local_planner/5b4e465fec718245484e8668e6dd45539ca37983/mpc_local_planner_examples/maps/empty_box.png -------------------------------------------------------------------------------- /mpc_local_planner_examples/maps/empty_box.yaml: -------------------------------------------------------------------------------- 1 | image: empty_box.png 2 | resolution: 0.050000 3 | origin: [-3.0, -3.0, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/maps/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rst-tu-dortmund/mpc_local_planner/5b4e465fec718245484e8668e6dd45539ca37983/mpc_local_planner_examples/maps/maze.png -------------------------------------------------------------------------------- /mpc_local_planner_examples/maps/maze.yaml: -------------------------------------------------------------------------------- 1 | image: maze.png 2 | resolution: 0.05 3 | origin: [0.0, 0.0, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mpc_local_planner_examples 4 | 0.0.3 5 | The mpc_local_planner_examples package 6 | 7 | Christoph Rösmann 8 | Christoph Rösmann 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/mpc_local_planner_examples 13 | 14 | 15 | catkin 16 | 17 | stage_ros 18 | mpc_local_planner 19 | mpc_local_planner_msgs 20 | move_base 21 | map_server 22 | amcl 23 | global_planner 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/corridor.world: -------------------------------------------------------------------------------- 1 | include "robots/diff_drive_robot_gps.inc" 2 | include "robots/obstacle.inc" 3 | 4 | define floorplan model 5 | ( 6 | # sombre, sensible, artistic 7 | color "gray30" 8 | 9 | # most maps will need a bounding box 10 | boundary 0 11 | 12 | gui_nose 0 13 | gui_grid 0 14 | gui_outline 0 15 | gripper_return 0 16 | fiducial_return 0 17 | laser_return 1 18 | ) 19 | 20 | resolution 0.02 21 | interval_sim 100 # simulation timestep in milliseconds 22 | 23 | window 24 | ( 25 | size [ 600.0 700.0 ] 26 | center [ 0.0 0.0 ] 27 | rotate [ 0.0 0.0 ] 28 | scale 60 29 | ) 30 | 31 | floorplan 32 | ( 33 | name "corridor" 34 | bitmap "../maps/corridor.png" 35 | size [ 25.000 6.000 2.000 ] 36 | pose [ 12.5 3.0 0.000 0.000 ] 37 | ) 38 | 39 | # throw in a robot 40 | myrobot 41 | ( 42 | pose [ 2.0 3.0 0.0 0.0 ] 43 | name "turtlebot" 44 | ) 45 | 46 | 47 | myobstacle 48 | ( 49 | pose [ 4.0 3.77 0.0 0.0 ] 50 | name "obstacle0" 51 | ) 52 | 53 | myobstacle 54 | ( 55 | pose [ 5.5 4.09 0.0 0.0 ] 56 | name "obstacle1" 57 | ) 58 | 59 | myobstacle 60 | ( 61 | pose [ 7.0 5.38 0.0 0.0 ] 62 | name "obstacle2" 63 | ) 64 | 65 | myobstacle 66 | ( 67 | pose [ 8.5 1.47 0.0 0.0 ] 68 | name "obstacle3" 69 | ) 70 | 71 | myobstacle 72 | ( 73 | pose [ 10.0 2.14 0.0 0.0 ] 74 | name "obstacle4" 75 | ) 76 | 77 | myobstacle 78 | ( 79 | pose [ 11.5 3.58 0.0 0.0 ] 80 | name "obstacle5" 81 | ) 82 | 83 | myobstacle 84 | ( 85 | pose [ 13.0 0.26 0.0 0.0 ] 86 | name "obstacle6" 87 | ) 88 | 89 | myobstacle 90 | ( 91 | pose [ 14.5 1.09 0.0 0.0 ] 92 | name "obstacle7" 93 | ) 94 | 95 | myobstacle 96 | ( 97 | pose [ 16 2.94 0.0 0.0 ] 98 | name "obstacle8" 99 | ) 100 | 101 | myobstacle 102 | ( 103 | pose [ 17.5 2.76 0.0 0.0 ] 104 | name "obstacle9" 105 | ) 106 | 107 | myobstacle 108 | ( 109 | pose [ 19 0.79 0.0 0.0 ] 110 | name "obstacle10" 111 | ) 112 | 113 | myobstacle 114 | ( 115 | pose [ 20.5 5.16 0.0 0.0 ] 116 | name "obstacle11" 117 | ) 118 | 119 | myobstacle 120 | ( 121 | pose [ 22 3.62 0.0 0.0 ] 122 | name "obstacle12" 123 | ) 124 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/empty_box.world: -------------------------------------------------------------------------------- 1 | # include our robot and obstacle definitions 2 | include "robots/diff_drive_robot_gps.inc" 3 | include "robots/obstacle.inc" 4 | 5 | ## Simulation settings 6 | resolution 0.02 7 | interval_sim 100 # simulation timestep in milliseconds 8 | 9 | ## Load a static map 10 | model 11 | ( 12 | # most maps will need a bounding box 13 | name "emptyBox" 14 | bitmap "../maps/empty_box.png" 15 | size [ 6.0 6.0 2.0 ] 16 | pose [ 0.0 0.0 0.0 0.0 ] 17 | laser_return 1 18 | color "gray30" 19 | ) 20 | 21 | # throw in a robot 22 | myrobot 23 | ( 24 | pose [ -2.0 0.0 0.0 -90.0 ] 25 | name "myRobot" 26 | ) 27 | 28 | myobstacle 29 | ( 30 | pose [ 0.0 1.0 0.0 0.0 ] 31 | name "obstacle" 32 | ) 33 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/maze_carlike.world: -------------------------------------------------------------------------------- 1 | include "robots/carlike_robot.inc" 2 | 3 | 4 | define floorplan model 5 | ( 6 | # sombre, sensible, artistic 7 | color "gray30" 8 | 9 | # most maps will need a bounding box 10 | boundary 1 11 | 12 | gui_nose 0 13 | gui_grid 0 14 | gui_outline 0 15 | gripper_return 0 16 | fiducial_return 0 17 | laser_return 1 18 | ) 19 | 20 | resolution 0.02 21 | interval_sim 100 # simulation timestep in milliseconds 22 | 23 | window 24 | ( 25 | size [ 600.0 700.0 ] 26 | center [ 0.0 0.0 ] 27 | rotate [ 0.0 0.0 ] 28 | scale 60 29 | ) 30 | 31 | floorplan 32 | ( 33 | name "maze" 34 | bitmap "../maps/maze.png" 35 | size [ 10.0 10.0 2.0 ] 36 | pose [ 5.0 5.0 0.0 0.0 ] 37 | ) 38 | 39 | # throw in a robot 40 | carlike_robot 41 | ( 42 | pose [ 2.0 2.0 0.0 0.0 ] 43 | name "robot" 44 | ) 45 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/maze_diff_drive.world: -------------------------------------------------------------------------------- 1 | include "robots/diff_drive_robot.inc" 2 | 3 | 4 | define floorplan model 5 | ( 6 | # sombre, sensible, artistic 7 | color "gray30" 8 | 9 | # most maps will need a bounding box 10 | boundary 1 11 | 12 | gui_nose 0 13 | gui_grid 0 14 | gui_outline 0 15 | gripper_return 0 16 | fiducial_return 0 17 | laser_return 1 18 | ) 19 | 20 | resolution 0.02 21 | interval_sim 100 # simulation timestep in milliseconds 22 | 23 | window 24 | ( 25 | size [ 600.0 700.0 ] 26 | center [ 0.0 0.0 ] 27 | rotate [ 0.0 0.0 ] 28 | scale 60 29 | ) 30 | 31 | floorplan 32 | ( 33 | name "maze" 34 | bitmap "../maps/maze.png" 35 | size [ 10.0 10.0 2.0 ] 36 | pose [ 5.0 5.0 0.0 0.0 ] 37 | ) 38 | 39 | # throw in a robot 40 | diff_drive_robot 41 | ( 42 | pose [ 2.0 2.0 0.0 0.0 ] 43 | name "robot" 44 | ) 45 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/maze_omnidir.world: -------------------------------------------------------------------------------- 1 | include "robots/omnidir_robot.inc" 2 | 3 | 4 | define floorplan model 5 | ( 6 | # sombre, sensible, artistic 7 | color "gray30" 8 | 9 | # most maps will need a bounding box 10 | boundary 1 11 | 12 | gui_nose 0 13 | gui_grid 0 14 | gui_outline 0 15 | gripper_return 0 16 | fiducial_return 0 17 | laser_return 1 18 | ) 19 | 20 | resolution 0.02 21 | interval_sim 100 # simulation timestep in milliseconds 22 | 23 | window 24 | ( 25 | size [ 600.0 700.0 ] 26 | center [ 0.0 0.0 ] 27 | rotate [ 0.0 0.0 ] 28 | scale 60 29 | ) 30 | 31 | floorplan 32 | ( 33 | name "maze" 34 | bitmap "../maps/maze.png" 35 | size [ 10.0 10.0 2.0 ] 36 | pose [ 5.0 5.0 0.0 0.0 ] 37 | ) 38 | 39 | # throw in a robot 40 | omnidir_robot 41 | ( 42 | pose [ 2.0 2.0 0.0 0.0 ] 43 | name "robot" 44 | ) 45 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/robots/carlike_robot.inc: -------------------------------------------------------------------------------- 1 | define laser ranger 2 | ( 3 | sensor 4 | ( 5 | range_max 6.5 6 | fov 58.0 7 | samples 640 8 | ) 9 | # generic model properties 10 | color "black" 11 | size [ 0.06 0.15 0.03 ] 12 | ) 13 | 14 | # 15 | # Robot model: 16 | # footprint (counter-clockwise): [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] 17 | # center of rotation: [0,0] 18 | # wheelbase: 0.4 19 | 20 | define carlike_robot position 21 | ( 22 | pose [ 0.0 0.0 0.0 0.0 ] 23 | 24 | odom_error [0.03 0.03 999999 999999 999999 0.02] 25 | 26 | size [ 0.6 0.25 0.40 ] # this models the footprint (rectangular), but shifted such that the bottom-left corner is in [0,0]. The center of rotation now here at [0.3, 0.125, 0.2] 27 | # correct center of rotation: 28 | origin [ 0.2 0.0 0.0 0.0] 29 | gui_nose 1 30 | color "red" 31 | 32 | # kinematics 33 | drive "car" 34 | wheelbase 0.4 # distance between both axles 35 | 36 | # spawn sensors 37 | laser(pose [ -0.1 0.0 -0.11 0.0 ]) 38 | ) 39 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/robots/diff_drive_robot.inc: -------------------------------------------------------------------------------- 1 | define laser ranger 2 | ( 3 | sensor 4 | ( 5 | range_max 6.5 6 | fov 58.0 7 | samples 640 8 | ) 9 | # generic model properties 10 | color "black" 11 | size [ 0.06 0.15 0.03 ] 12 | ) 13 | 14 | define diff_drive_robot position 15 | ( 16 | pose [ 0.0 0.0 0.0 0.0 ] 17 | 18 | odom_error [0.03 0.03 999999 999999 999999 0.02] 19 | 20 | size [ 0.25 0.25 0.40 ] 21 | origin [ 0.0 0.0 0.0 0.0 ] 22 | gui_nose 1 23 | color "red" 24 | 25 | # kinematics 26 | drive "diff" 27 | 28 | # spawn sensors 29 | laser(pose [ -0.1 0.0 -0.11 0.0 ]) 30 | ) 31 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/robots/diff_drive_robot_gps.inc: -------------------------------------------------------------------------------- 1 | define mylaser ranger 2 | ( 3 | sensor 4 | ( 5 | # just for demonstration purposes 6 | range [0.1 25] # minimum and maximum range 7 | fov 360.0 # field of view 8 | samples 1920 # number of samples 9 | ) 10 | size [ 0.06 0.15 0.03 ] 11 | ) 12 | 13 | define myrobot position 14 | ( 15 | size [ 0.25 0.25 0.40 ] # (x,y,z) 16 | localization "gps" # exact localization 17 | gui_nose 1 # draw nose showing heading 18 | drive "diff" # diff-drive 19 | color "red" # red model 20 | mylaser(pose [ -0.1 0.0 -0.11 0.0 ]) # has mylaser sensor 21 | ) 22 | -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/robots/obstacle.inc: -------------------------------------------------------------------------------- 1 | define myobstacle position 2 | ( 3 | localization "gps" 4 | size [ 0.25 0.25 0.40 ] 5 | gui_nose 1 6 | drive "omni" 7 | color "blue" 8 | ) -------------------------------------------------------------------------------- /mpc_local_planner_examples/stage/robots/omnidir_robot.inc: -------------------------------------------------------------------------------- 1 | define laser ranger 2 | ( 3 | sensor 4 | ( 5 | range_max 6.5 6 | fov 58.0 7 | samples 640 8 | ) 9 | # generic model properties 10 | color "black" 11 | size [ 0.06 0.15 0.03 ] 12 | ) 13 | 14 | define omnidir_robot position 15 | ( 16 | pose [ 0.0 0.0 0.0 0.0 ] 17 | 18 | odom_error [0.03 0.03 999999 999999 999999 0.02] 19 | 20 | size [ 0.25 0.25 0.40 ] 21 | origin [ 0.0 0.0 0.0 0.0 ] 22 | gui_nose 1 23 | color "red" 24 | 25 | # kinematics 26 | drive "omni" 27 | 28 | # spawn sensors 29 | laser(pose [ -0.1 0.0 -0.11 0.0 ]) 30 | ) 31 | -------------------------------------------------------------------------------- /mpc_local_planner_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mpc_local_planner_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.3 (2020-06-09) 6 | ------------------ 7 | * Changed minimum CMake version to 3.1 8 | * Contributors: Christoph Rösmann 9 | 10 | 0.0.2 (2020-03-12) 11 | ------------------ 12 | 13 | 0.0.1 (2020-02-20) 14 | ------------------ 15 | * First release 16 | * Contributors: Christoph Rösmann 17 | -------------------------------------------------------------------------------- /mpc_local_planner_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(mpc_local_planner_msgs) 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 | std_msgs 12 | message_generation 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | add_message_files( 50 | FILES 51 | StateFeedback.msg 52 | OptimalControlResult.msg 53 | ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | generate_messages( 71 | DEPENDENCIES 72 | std_msgs 73 | ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES mpc_local_planner_msgs 107 | CATKIN_DEPENDS std_msgs message_runtime 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | #include_directories( 118 | # include 119 | # ${catkin_INCLUDE_DIRS} 120 | #) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/mpc_local_planner_msgs.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/mpc_local_planner_msgs_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # install(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables for installation 167 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 168 | # install(TARGETS ${PROJECT_NAME}_node 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark libraries for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 174 | # install(TARGETS ${PROJECT_NAME} 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_local_planner_msgs.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | -------------------------------------------------------------------------------- /mpc_local_planner_msgs/msg/OptimalControlResult.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | int64 dim_states 4 | int64 dim_controls 5 | 6 | float64[] time_states 7 | float64[] states # Column Major 8 | float64[] time_controls 9 | float64[] controls # Column Major 10 | 11 | bool optimal_solution_found 12 | float64 cpu_time 13 | -------------------------------------------------------------------------------- /mpc_local_planner_msgs/msg/StateFeedback.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float64[] state 4 | -------------------------------------------------------------------------------- /mpc_local_planner_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mpc_local_planner_msgs 4 | 0.0.3 5 | This package provides message types that are used by the package mpc_local_planner 6 | 7 | Christoph Rösmann 8 | Christoph Rösmann 9 | 10 | GPLv3 11 | 12 | http://wiki.ros.org/mpc_local_planner_msgs 13 | 14 | catkin 15 | 16 | std_msgs 17 | message_generation 18 | 19 | std_msgs 20 | 21 | std_msgs 22 | message_runtime 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | --------------------------------------------------------------------------------