commit 238d8293ace6a34745100bb23fe37b231315d800 Author: Christoph Rösmann Date: Thu Feb 20 12:32:14 2020 +0100 project imported with clean history diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..53ef6c4 --- /dev/null +++ b/.gitignore @@ -0,0 +1,18 @@ +# Build files +build + +# Autosave files +*.autosave + +# MacOS temporary files +.DS_Store + +# QT project files +*.txt.user* + +# kDevelop project files +.kdev4 +*.kdev4 + +# Version control +.svn diff --git a/README.md b/README.md new file mode 100644 index 0000000..606ab5a --- /dev/null +++ b/README.md @@ -0,0 +1,61 @@ +mpc_local_planner ROS Package +============================= + +The mpc_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack. +It provides a generic and versatile model predictive control implementation with minimum-time and quadratic-form receding-horizon configurations. + +## Authors + +* Christoph Rösmann + +### Citing the Software + +*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.* + +**Standard MPC and Hypergraph** + +- C. Rösmann, M. Krämer, A. Makarow, F. Hoffmann und 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. + +**Time-Optimal MPC and Hypergraph** + +- 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, TU Dortmund University, Oct. 2019. +- 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. +- C. Rösmann, F. Hoffman und T. Bertram: Convergence Analysis of Time-Optimal Model Predictive Control under Limited Computational Resources, European Control Conference (ECC), Denmark, June 2016. + + +### License + +Copyright (c) 2020, +TU Dortmund - Institute of Control Theory and Systems Engineering. +All rights reserved. + +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . + +The package depends on third-party packages: +- *control_box_rst*, GPLv3, https://github.com/rst-tu-dortmund/control_box_rst + Please check also the dependencies of control_box_rst (not all modules + are included at the moment) +- *Ipopt*, EPL 1.0, https://github.com/coin-or/Ipopt + + +It depends on other ROS packages, which are listed in the package.xml. They are licensed under BSD resp. Apache 2.0. + + +### Requirements + +Install dependencies (listed in the *package.xml* and *CMakeLists.txt* file) using *rosdep*: + + rosdep install mpc_local_planner + + diff --git a/mpc_local_planner/.clang-format b/mpc_local_planner/.clang-format new file mode 100644 index 0000000..968859f --- /dev/null +++ b/mpc_local_planner/.clang-format @@ -0,0 +1,95 @@ +--- +Language: Cpp +# BasedOnStyle: Google +# Doc: https://clang.llvm.org/docs/ClangFormatStyleOptions.html +AccessModifierOffset: -3 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: true +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: true + AfterControlStatement: true + AfterEnum: false + AfterFunction: true + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false +BreakBeforeBinaryOperators: None +# Custom: break according to "BraceWrapping" +BreakBeforeBraces: Custom +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +ColumnLimit: 150 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +#FixNamespaceComments: true +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IndentCaseLabels: true +IndentWidth: 4 +IndentWrappedFunctionNames: false +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SpaceAfterCStyleCast: false +#SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +#Standard: Auto +TabWidth: 8 +UseTab: Never +... + diff --git a/mpc_local_planner/CMakeLists.txt b/mpc_local_planner/CMakeLists.txt new file mode 100644 index 0000000..2020647 --- /dev/null +++ b/mpc_local_planner/CMakeLists.txt @@ -0,0 +1,272 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mpc_local_planner) + +# Set to Release in order to speed up the program significantly +set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + base_local_planner + costmap_2d + costmap_converter + dynamic_reconfigure + roscpp + geometry_msgs + interactive_markers + nav_core + nav_msgs + mbf_costmap_core + mbf_msgs + mpc_local_planner_msgs + pluginlib + std_msgs + teb_local_planner + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs +) + +# Find control_box_rst +# TODO: Currently, we use QUIET rather than REQUIRED to avoid warnings +# as some components depend on each other (circularly) +# which is currently not handled well by the exported cmake config +find_package(control_box_rst QUIET COMPONENTS + core + communication + controllers + numerics + systems + optimization + optimal_control + systems + PATH_SUFFIXES share/control_box_rst control_box_rst + PATHS ${CMAKE_BINARY_DIR} /usr/local +) + +# Eigen3 FindScript Backward compatibility (ubuntu saucy) +# Since FindEigen.cmake is deprecated starting from jade. +# if (EXISTS "FindEigen3.cmake") +find_package(Eigen3 REQUIRED) + +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +set(EXTERNAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include ${EXTERNAL_INCLUDE_DIRS} + LIBRARIES mpc_local_planner_numerics mpc_local_planner_optimization + CATKIN_DEPENDS roscpp mpc_local_planner_msgs control_box_rst teb_local_planner + # DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${EXTERNAL_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(mpc_local_planner_utils + src/utils/publisher.cpp + src/utils/conversion.cpp + src/utils/time_series_se2.cpp +) + +add_library(mpc_local_planner_optimal_control + src/optimal_control/full_discretization_grid_base_se2.cpp + src/optimal_control/finite_differences_grid_se2.cpp + src/optimal_control/finite_differences_variable_grid_se2.cpp + src/optimal_control/stage_inequality_se2.cpp + src/optimal_control/quadratic_cost_se2.cpp + src/optimal_control/final_state_conditions_se2.cpp + src/optimal_control/min_time_via_points_cost.cpp +) + +add_library(mpc_local_planner + src/controller.cpp + src/mpc_local_planner_ros.cpp +) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(test_mpc_optim_node src/test_mpc_optim_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +add_dependencies(mpc_local_planner_utils corbo_core corbo_systems) +target_link_libraries(mpc_local_planner_utils + corbo_core + corbo_systems +) + +target_link_libraries(mpc_local_planner_optimal_control + corbo_optimal_control +) + +target_link_libraries(mpc_local_planner + mpc_local_planner_utils + mpc_local_planner_optimal_control + corbo_controllers + ${catkin_LIBRARIES} +) + + target_link_libraries(test_mpc_optim_node + mpc_local_planner + ${catkin_LIBRARIES} + ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_local_planner.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/mpc_local_planner/cfg/rviz_test_mpc_optim.rviz b/mpc_local_planner/cfg/rviz_test_mpc_optim.rviz new file mode 100644 index 0000000..90a374d --- /dev/null +++ b/mpc_local_planner/cfg/rviz_test_mpc_optim.rviz @@ -0,0 +1,189 @@ +Panels: + - Class: rviz/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Path1 + Splitter Ratio: 0.5 + Tree Height: 562 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.100000001 + Cell Size: 0.100000001 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Fine Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.119999997 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 0; 0 + Pose Style: Arrows + Radius: 0.0299999993 + Shaft Diameter: 0.0199999996 + Shaft Length: 0.100000001 + Topic: /test_mpc_optim_node/local_plan + Unreliable: false + Value: true + - Alpha: 1 + Arrow Length: 0.300000012 + Axes Length: 0.300000012 + Axes Radius: 0.00999999978 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.0700000003 + Head Radius: 0.0299999993 + Name: PoseArray + Shaft Length: 0.230000004 + Shaft Radius: 0.00999999978 + Shape: Arrow (Flat) + Topic: /test_optim_node/teb_poses + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /test_mpc_optim_node/mpc_markers + Name: Marker + Namespaces: + PointObstacles: true + Queue Size: 100 + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /marker_obstacles/update + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 9.30938148 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 2.73932815 + Y: 0.504589677 + Z: 0.0263484083 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 1.56479633 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.70042801 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1354 + X: 137 + Y: 50 diff --git a/mpc_local_planner/cfg/test_mpc_optim_node.yaml b/mpc_local_planner/cfg/test_mpc_optim_node.yaml new file mode 100644 index 0000000..1080587 --- /dev/null +++ b/mpc_local_planner/cfg/test_mpc_optim_node.yaml @@ -0,0 +1,113 @@ +## Robot settings +robot: + type: "unicycle" + unicycle: + max_vel_x: 0.4 + max_vel_x_backwards: 0.2 + max_vel_theta: 0.3 + acc_lim_x: 0.0 # deactive bounds with zero + dec_lim_x: 0.0 # deactive bounds with zero + acc_lim_theta: 0.0 # deactivate bounds with zero + simple_car: + wheelbase: 0.4 + front_wheel_driving: False + max_vel_x: 0.4 + max_vel_x_backwards: 0.2 + max_steering_angle: 1.4 + acc_lim_x: 0.0 # deactive bounds with zero + dec_lim_x: 0.0 # deactive bounds with zero + max_steering_rate: 0.5 # deactive bounds with zero + +## Footprint model for collision avoidance +footprint_model: + type: "point" + +collision_avoidance: + min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model + enable_dynamic_obstacles: False + force_inclusion_factor: 1.5 + cutoff_factor: 5 + +## Planning grid +grid: + type: "fd_grid" + grid_size_ref: 20 + dt_ref: 0.3 + xf_fixed: [True, True, True] # E.g., set last one to False in order to unfix the final orientation + warm_start: True + collocation_method: "forward_differences" + cost_integration_method: "left_sum" + variable_grid: + enable: True + min_dt: 0.0; + max_dt: 10.0; + grid_adaptation: + enable: True + dt_hyst_ratio: 0.1 + min_grid_size: 2 + max_grid_size: 50 + + +## Planning options +planning: + objective: + type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly + quadratic_form: + state_weights: [2.0, 2.0, 2.0] + control_weights: [1.0, 1.0] + integral_form: False + minimum_time_via_points: + position_weight: 8.0 + orientation_weight: 0.0 + via_points_ordered: False + terminal_cost: + type: "quadratic" # can be "none" + quadratic: + final_state_weights: [2.0, 2.0, 2.0] + terminal_constraint: + type: "none" # can be "none" + l2_ball: + weight_matrix: [1.0, 1.0, 1.0] + radius: 5 + + +## Controller options +controller: + outer_ocp_iterations: 1 + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.1 + force_reinit_new_goal_dist: 1.0 + force_reinit_new_goal_angular: 1.57 + force_reinit_num_steps: 0 + global_plan_overwrite_orientation: True + global_plan_viapoint_sep: -1 + allow_init_with_backward_motion: True + publish_ocp_results: True + print_cpu_time: False + +## Solver settings +solver: + type: "ipopt" + ipopt: + iterations: 100 + max_cpu_time: -1.0 + ipopt_numeric_options: + tol: 1e-4 + ipopt_string_options: + linear_solver: "mumps" + hessian_approximation: "limited-memory" # exact or limited-memory + ipopt_integer_options: + print_level: 2 + lsq_lm: + iterations: 10 + weight_init_eq: 2 + weight_init_ineq: 2 + weight_init_bounds: 2 + weight_adapt_factor_eq: 1.5 + weight_adapt_factor_ineq: 1.5 + weight_adapt_factor_bounds: 1.5 + weight_adapt_max_eq: 500 + weight_adapt_max_ineq: 500 + weight_adapt_max_bounds: 500 + + diff --git a/mpc_local_planner/include/mpc_local_planner/controller.h b/mpc_local_planner/include/mpc_local_planner/controller.h new file mode 100644 index 0000000..3ee540a --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/controller.h @@ -0,0 +1,126 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef CONTROLLER_H_ +#define CONTROLLER_H_ + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @brief MPC controller for mobile robots + * + * @ingroup controllers + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class Controller : public corbo::PredictiveController +{ + public: + using Ptr = std::shared_ptr; + using PoseSE2 = teb_local_planner::PoseSE2; + + Controller() = default; + + bool configure(ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles, teb_local_planner::RobotFootprintModelPtr robot_model, + const std::vector& via_points); + bool step(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist& vel, double dt, ros::Time t, corbo::TimeSeries::Ptr u_seq, + corbo::TimeSeries::Ptr x_seq); + + bool step(const std::vector& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t, + corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq); + + // implements interface method + corbo::ControllerInterface::Ptr getInstance() const override { return std::make_shared(); } + static corbo::ControllerInterface::Ptr getInstanceStatic() { return std::make_shared(); } + + void setOptimalControlProblem(corbo::OptimalControlProblemInterface::Ptr ocp) = delete; + + RobotDynamicsInterface::Ptr getRobotDynamics() { return _dynamics; } + StageInequalitySE2::Ptr getInequalityConstraint() { return _inequality_constraint; } + + void stateFeedbackCallback(const mpc_local_planner_msgs::StateFeedback::ConstPtr& msg); + + void publishOptimalControlResult(); + + void setInitialPlanEstimateOrientation(bool estimate) { _initial_plan_estimate_orientation = estimate; } + + // implements interface method + void reset() override; + + protected: + corbo::DiscretizationGridInterface::Ptr configureGrid(const ros::NodeHandle& nh); + RobotDynamicsInterface::Ptr configureRobotDynamics(const ros::NodeHandle& nh); + corbo::NlpSolverInterface::Ptr configureSolver(const ros::NodeHandle& nh); + corbo::StructuredOptimalControlProblem::Ptr configureOcp(const ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles, + teb_local_planner::RobotFootprintModelPtr robot_model, + const std::vector& via_points); + + bool generateInitialStateTrajectory(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, + const std::vector& initial_plan, bool backward); + + std::string _robot_type; + corbo::DiscretizationGridInterface::Ptr _grid; + RobotDynamicsInterface::Ptr _dynamics; + corbo::NlpSolverInterface::Ptr _solver; + StageInequalitySE2::Ptr _inequality_constraint; + corbo::StructuredOptimalControlProblem::Ptr _structured_ocp; + + ros::Publisher _ocp_result_pub; + bool _ocp_successful = false; + std::size_t _ocp_seq = 0; + bool _publish_ocp_results = false; + bool _print_cpu_time = false; + + bool _prefer_x_feedback = false; // prefer state feedback over odometry feedback + ros::Subscriber _x_feedback_sub; + std::mutex _x_feedback_mutex; + ros::Time _recent_x_time; + Eigen::VectorXd _recent_x_feedback; + + teb_local_planner::PoseSE2 _last_goal; + double _force_reinit_new_goal_dist = 1.0; + double _force_reinit_new_goal_angular = 0.5 * M_PI; + corbo::DiscreteTimeReferenceTrajectory _x_seq_init; + bool _initial_plan_estimate_orientation = true; + bool _guess_backwards_motion = true; + int _force_reinit_num_steps = 0; +}; + +} // namespace mpc_local_planner + +#endif // CONTROLLER_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h b/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h new file mode 100644 index 0000000..c1921bb --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h @@ -0,0 +1,430 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef MPC_LOCAL_PLANNER_ROS_H_ +#define MPC_LOCAL_PLANNER_ROS_H_ + +#include + +// base local planner base class and utilities +#include +#include +#include +#include +#include + +// mpc_local_planner related classes +#include +#include + +// teb_local_planner related classes +#include +#include +#include + +// message types +#include +#include +#include +#include +#include +#include + +// transforms +#include +#include + +// costmap +#include +#include + +// dynamic reconfigure +// #include +// #include + +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @class MpcLocalPlannerROS + * @brief Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract + * interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF). + * @todo Escape behavior, more efficient obstacle handling + */ +class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController +{ + using PoseSE2 = teb_local_planner::PoseSE2; + using RobotFootprintModelPtr = teb_local_planner::RobotFootprintModelPtr; + using Point2dContainer = teb_local_planner::Point2dContainer; + using ObstContainer = teb_local_planner::ObstContainer; + using ViaPointContainer = std::vector; + + using ObstaclePtr = teb_local_planner::ObstaclePtr; + using PointObstacle = teb_local_planner::PointObstacle; + using CircularObstacle = teb_local_planner::CircularObstacle; + using LineObstacle = teb_local_planner::LineObstacle; + using PolygonObstacle = teb_local_planner::PolygonObstacle; + + public: + /** + * @brief Default constructor of the plugin + */ + MpcLocalPlannerROS(); + /** + * @brief Destructor of the plugin + */ + ~MpcLocalPlannerROS(); + + /** + * @brief Initializes the teb plugin + * @param name The name of the instance + * @param tf Pointer to a tf buffer + * @param costmap_ros Cost map representing occupied and free space + */ + void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Set the plan that the teb local planner is following + * @param orig_global_plan The plan to pass to the local planner + * @return True if the plan was updated successfully, false otherwise + */ + bool setPlan(const std::vector& orig_global_plan); + + /** + * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base + * @param cmd_vel Will be filled with the velocity command to be passed to the robot base + * @return True if a valid trajectory was found, false otherwise + */ + bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel); + + /** + * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. + * @remark Extended version for MBF API + * @param pose the current pose of the robot. + * @param velocity the current velocity of the robot. + * @param cmd_vel Will be filled with the velocity command to be passed to the robot base. + * @param message Optional more detailed outcome as a string + * @return Result code as described on ExePath action result: + * SUCCESS = 0 + * 1..9 are reserved as plugin specific non-error results + * FAILURE = 100 Unspecified failure, only used for old, non-mfb_core based plugins + * CANCELED = 101 + * NO_VALID_CMD = 102 + * PAT_EXCEEDED = 103 + * COLLISION = 104 + * OSCILLATION = 105 + * ROBOT_STUCK = 106 + * MISSED_GOAL = 107 + * MISSED_PATH = 108 + * BLOCKED_PATH = 109 + * INVALID_PATH = 110 + * TF_ERROR = 111 + * NOT_INITIALIZED = 112 + * INVALID_PLUGIN = 113 + * INTERNAL_ERROR = 114 + * 121..149 are reserved as plugin specific errors + */ + uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, + geometry_msgs::TwistStamped& cmd_vel, std::string& message); + + /** + * @brief Check if the goal pose has been achieved + * + * The actual check is performed in computeVelocityCommands(). + * Only the status flag is checked here. + * @return True if achieved, false otherwise + */ + bool isGoalReached(); + + /** + * @brief Dummy version to satisfy MBF API + */ + bool isGoalReached(double xy_tolerance, double yaw_tolerance) { return isGoalReached(); }; + + /** + * @brief Requests the planner to cancel, e.g. if it takes too much time + * @remark New on MBF API + * @return True if a cancel has been successfully requested, false if not implemented. + */ + bool cancel() { return false; }; + + /** @name Public utility functions/methods */ + //@{ + + /** + * @brief Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities. + * + * Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component). + * @param tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle) + * @return Translational and angular velocity combined into an Eigen::Vector2d + */ + static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel); + + /** + * @brief Get the current robot footprint/contour model + * @param nh const reference to the local ros::NodeHandle + * @return Robot footprint model used for optimization + */ + static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh); + + /** + * @brief Set the footprint from the given XmlRpcValue. + * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros + * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point + * @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the + * sub-arrays should all have exactly 2 elements (x and y coordinates). + * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came. + * It is used only for reporting errors. + * @return container of vertices describing the polygon + */ + static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name); + + /** + * @brief Get a number from the given XmlRpcValue. + * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros + * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point + * @param value double value type + * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came. + * It is used only for reporting errors. + * @returns double value + */ + static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name); + + //@} + + protected: + /** + * @brief Update internal obstacle vector based on occupied costmap cells + * @remarks All occupied cells will be added as point obstacles. + * @remarks All previous obstacles are cleared. + * @sa updateObstacleContainerWithCostmapConverter + * @todo Include temporal coherence among obstacle msgs (id vector) + * @todo Include properties for dynamic obstacles (e.g. using constant velocity model) + */ + void updateObstacleContainerWithCostmap(); + + /** + * @brief Update internal obstacle vector based on polygons provided by a costmap_converter plugin + * @remarks Requires a loaded costmap_converter plugin. + * @remarks All previous obstacles are cleared. + * @sa updateObstacleContainerWithCostmap + */ + void updateObstacleContainerWithCostmapConverter(); + + /** + * @brief Update internal obstacle vector based on custom messages received via subscriber + * @remarks All previous obstacles are NOT cleared. Call this method after other update methods. + * @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter + */ + void updateObstacleContainerWithCustomObstacles(); + + /** + * @brief Update internal via-point container based on the current reference plan + * @remarks All previous via-points will be cleared. + * @param transformed_plan (local) portion of the global plan (which is already transformed to the planning frame) + * @param min_separation minimum separation between two consecutive via-points + */ + void updateViaPointsContainer(const std::vector& transformed_plan, double min_separation); + + /** + * @brief Callback for the dynamic_reconfigure node. + * + * This callback allows to modify parameters dynamically at runtime without restarting the node + * @param config Reference to the dynamic reconfigure config + * @param level Dynamic reconfigure level + */ + // void reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level); + + /** + * @brief Callback for custom obstacles that are not obtained from the costmap + * @param obst_msg pointer to the message containing a list of polygon shaped obstacles + */ + void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg); + + /** + * @brief Callback for custom via-points + * @param via_points_msg pointer to the message containing a list of via-points + */ + void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg); + + /** + * @brief Prune global plan such that already passed poses are cut off + * + * The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform. + * If no valid transformation can be found, the method returns \c false. + * The global plan is pruned until the distance to the robot is at least \c dist_behind_robot. + * If no pose within the specified treshold \c dist_behind_robot can be found, + * nothing will be pruned and the method returns \c false. + * @remarks Do not choose \c dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned. + * @param tf A reference to a tf buffer + * @param global_pose The global pose of the robot + * @param[in,out] global_plan The plan to be transformed + * @param dist_behind_robot Distance behind the robot that should be kept [meters] + * @return \c true if the plan is pruned, \c false in case of a transform exception or if no pose cannot be found inside the threshold + */ + bool pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose, + std::vector& global_plan, double dist_behind_robot = 1); + + /** + * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). + * + * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h + * such that the index of the current goal pose is returned as well as + * the transformation between the global plan and the planning frame. + * @param tf A reference to a tf buffer + * @param global_plan The plan to be transformed + * @param global_pose The global pose of the robot + * @param costmap A reference to the costmap being used so the window size for transforming can be computed + * @param global_frame The frame to transform the plan to + * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also + * bounded by the local costmap size!] + * @param[out] transformed_plan Populated with the transformed plan + * @param[out] current_goal_idx Index of the current (local) goal pose in the global plan + * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame + * @return \c true if the global plan is transformed, \c false otherwise + */ + bool transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector& global_plan, + const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, + double max_plan_length, std::vector& transformed_plan, int* current_goal_idx = NULL, + geometry_msgs::TransformStamped* tf_plan_to_global = NULL) const; + + /** + * @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner. + * + * If the current (local) goal point is not the final one (global) + * substitute the goal orientation by the angle of the direction vector between + * the local goal and the subsequent pose of the global plan. + * This is often helpful, if the global planner does not consider orientations. \n + * A moving average filter is utilized to smooth the orientation. + * @param global_plan The global plan + * @param local_goal Current local goal + * @param current_goal_idx Index of the current (local) goal pose in the global plan + * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame + * @param moving_average_length number of future poses of the global plan to be taken into account + * @return orientation (yaw-angle) estimate + */ + double estimateLocalGoalOrientation(const std::vector& global_plan, const geometry_msgs::PoseStamped& local_goal, + int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global, + int moving_average_length = 3) const; + + /** + * @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint + * + * This method prints warnings if validation fails. + * @remarks Currently, we only validate the inscribed radius of the footprints + * @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization + * @param costmap_inscribed_radius Inscribed radius of the footprint model used for the costmap + * @param min_obst_dist desired distance to obstacles + */ + void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist); + + private: + // Definition of member variables + + // external objects (store weak pointers) + costmap_2d::Costmap2DROS* _costmap_ros; //!< Pointer to the costmap ros wrapper, received from the navigation stack + costmap_2d::Costmap2D* _costmap; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper) + tf2_ros::Buffer* _tf; //!< pointer to tf buffer + + // internal objects + Controller _controller; + ObstContainer _obstacles; //!< Obstacle vector that should be considered during local trajectory optimization + Publisher _publisher; + std::shared_ptr _costmap_model; + + corbo::TimeSeries::Ptr _x_seq = std::make_shared(); + corbo::TimeSeries::Ptr _u_seq = std::make_shared(); + + std::vector _global_plan; //!< Store the current global plan + + base_local_planner::OdometryHelperRos _odom_helper; //!< Provides an interface to receive the current velocity from the robot + + pluginlib::ClassLoader _costmap_converter_loader; //!< Load costmap converter plugins at runtime + boost::shared_ptr _costmap_converter; //!< Store the current costmap_converter + + // std::shared_ptr> + // dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime + ros::Subscriber _custom_obst_sub; //!< Subscriber for custom obstacles received via a ObstacleMsg. + std::mutex _custom_obst_mutex; //!< Mutex that locks the obstacle array (multi-threaded) + costmap_converter::ObstacleArrayMsg _custom_obstacle_msg; //!< Copy of the most recent obstacle message + + ViaPointContainer _via_points; + ros::Subscriber _via_points_sub; //!< Subscriber for custom via-points received via a Path msg. + bool _custom_via_points_active = false; //!< Keep track whether valid via-points have been received from via_points_sub_ + std::mutex _via_point_mutex; //!< Mutex that locks the via_points container (multi-threaded) + + PoseSE2 _robot_pose; //!< Store current robot pose + PoseSE2 _robot_goal; //!< Store current robot goal + geometry_msgs::Twist _robot_vel; //!< Store current robot translational and angular velocity (vx, vy, omega) + bool _goal_reached = false; //!< store whether the goal is reached or not + ros::Time _time_last_infeasible_plan; //!< Store at which time stamp the last infeasible plan was detected + int _no_infeasible_plans = 0; //!< Store how many times in a row the planner failed to find a feasible plan. + geometry_msgs::Twist _last_cmd; //!< Store the last control command generated in computeVelocityCommands() + ros::Time _time_last_cmd; + + RobotFootprintModelPtr _robot_model; + + std::vector _footprint_spec; //!< Store the footprint of the robot + double _robot_inscribed_radius; //!< The radius of the inscribed circle of the robot (collision possible) + double _robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot + + std::string _global_frame; //!< The frame in which the controller will run + std::string _robot_base_frame; //!< Used as the base frame id of the robot + + // flags + bool _initialized; //!< Keeps track about the correct initialization of this class + + struct Parameters + { + double xy_goal_tolerance = 0.2; + double yaw_goal_tolerance = 0.1; + bool global_plan_overwrite_orientation = true; + double global_plan_prune_distance = 1.0; + double max_global_plan_lookahead_dist = 1.5; + bool is_footprint_dynamic = false; + bool include_costmap_obstacles = true; + double costmap_obstacles_behind_robot_dist = 1.5; + double global_plan_viapoint_sep = -1; + std::string odom_topic = "odom"; + double controller_frequency = 10; + + } _params; + + struct CostmapConverterPlugin + { + std::string costmap_converter_plugin; + double costmap_converter_rate = 5; + bool costmap_converter_spin_thread = true; + + } _costmap_conv_params; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +}; // end namespace mpc_local_planner + +#endif // MPC_LOCAL_PLANNER_ROS_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/fd_collocation_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/fd_collocation_se2.h new file mode 100644 index 0000000..72c274f --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/fd_collocation_se2.h @@ -0,0 +1,149 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef FINITE_DIFFERENCES_COLLOCATION_H_ +#define FINITE_DIFFERENCES_COLLOCATION_H_ + +#include + +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @brief Collocation via forward differences (specialized for SE2) + * + * Forward differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner: + * \f[ + * \frac{x_{k+1} - x_k}{\delta T} = f(x_k, u_k) + * \f] + * + * @see ForwardDiffCollocation FiniteDifferencesCollocationInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class ForwardDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface +{ + public: + // Implements interface method + corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared(); } + + // Implements interface method + void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, + const corbo::SystemDynamicsInterface& system, Eigen::Ref error) override + { + assert(error.size() == x1.size()); + assert(x1.size() >= 3); + // assert(dt > 0 && "dt must be greater then zero!"); + + system.dynamics(x1, u1, error); + error.head(2) -= (x2.head(2) - x1.head(2)) / dt; + error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt; + if (x1.size() > 3) + { + int n = x1.size() - 3; + error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt; + } + } +}; + +/** + * @brief Collocation via midpoint differences (specialized for SE2) + * + * Midpoint differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner: + * \f[ + * \frac{x_{k+1} - x_k}{\delta T} = f(0.5*(x_k + x_{k+1}), u_k) + * \f] + * + * @see FiniteDifferencesCollocationInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class MidpointDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface +{ + public: + // Implements interface method + corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared(); } + + // Implements interface method + void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, + const corbo::SystemDynamicsInterface& system, Eigen::Ref error) override + { + assert(error.size() == x1.size()); + assert(dt > 0 && "dt must be greater then zero!"); + + system.dynamics(0.5 * (x1 + x2), u1, error); + error.head(2) -= (x2.head(2) - x1.head(2)) / dt; + error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt; + if (x1.size() > 3) + { + int n = x1.size() - 3; + error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt; + } + } +}; + +/** + * @brief Collocation via Crank-Nicolson differences (specialized for SE2) + * + * Crank-Nicolson differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner: + * \f[ + * \frac{x_{k+1} - x_k}{\delta T} = 0.5 * ( f(x_k, u_k) + f(x_{k+1}, u_k)) + * \f] + * + * @see FiniteDifferencesCollocationInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class CrankNicolsonDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface +{ + public: + // Implements interface method + corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared(); } + + // Implements interface method + void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt, + const corbo::SystemDynamicsInterface& system, Eigen::Ref error) override + { + assert(error.size() == x1.size()); + assert(dt > 0 && "dt must be greater then zero!"); + + Eigen::VectorXd f1(x1.size()); + system.dynamics(x1, u1, f1); + system.dynamics(x2, u1, error); + // error = (x2 - x1) / dt - 0.5 * (f1 + error); + error.head(2) -= (x2.head(2) - x1.head(2)) / dt - 0.5 * (f1.head(2) + error.head(2)); + error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt - 0.5 * (f1.coeffRef(2) + error.coeffRef(2)); + if (x1.size() > 3) + { + int n = x1.size() - 3; + error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt - 0.5 * (f1.tail(n) + error.tail(n)); + } + } +}; + +} // namespace mpc_local_planner + +#endif // FINITE_DIFFERENCES_COLLOCATION_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/final_state_conditions_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/final_state_conditions_se2.h new file mode 100644 index 0000000..0e90c0b --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/final_state_conditions_se2.h @@ -0,0 +1,88 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef FINAL_STATE_CONDITIONS_SE2_H_ +#define FINAL_STATE_CONDITIONS_SE2_H_ + +#include +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @brief Quadratic final state cost (specialized for SE2) + * + * This class implements quadratic final costs + * \f[ + * J_f(x) = (x_{ref} - x)^T Q_f (x_{xref} - x) + * \f] + * However, this class ensures that the distance (x_{ref} - x) is + * computed properly in SO(2) for the third state component. + * + * @see corbo::QuadraticFinalStateCost + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class QuadraticFinalStateCostSE2 : public corbo::QuadraticFinalStateCost +{ + public: + using Ptr = std::shared_ptr; + + QuadraticFinalStateCostSE2() : corbo::QuadraticFinalStateCost() {} + + QuadraticFinalStateCostSE2(const Eigen::Ref& Qf, bool lsq_form = false) : corbo::QuadraticFinalStateCost(Qf, lsq_form) {} + + corbo::FinalStageCost::Ptr getInstance() const override { return std::make_shared(); } + + void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; +}; + +/** + * @brief Terminal ball constraint (specialized for SE2) + * + * This class ensures that the distance (x_{ref} - x) is + * computed properly in SO(2) for the third state component. + * + * @see corbo::TerminalBall + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class TerminalBallSE2 : public corbo::TerminalBall +{ + public: + using Ptr = std::shared_ptr; + + TerminalBallSE2() : corbo::TerminalBall() {} + + TerminalBallSE2(const Eigen::Ref& S, double gamma) : corbo::TerminalBall(S, gamma) {} + + corbo::FinalStageConstraint::Ptr getInstance() const override { return std::make_shared(); } + + void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; +}; + +} // namespace mpc_local_planner + +#endif // FINAL_STATE_CONDITIONS_SE2_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_grid_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_grid_se2.h new file mode 100644 index 0000000..b2c25fc --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_grid_se2.h @@ -0,0 +1,70 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef FINITE_DIFFERENCES_GRID_H_ +#define FINITE_DIFFERENCES_GRID_H_ + +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief Finite differences grid for SE2 + * + * This class implements a full discretization grid with finite difference collocation. + * The temporal resolution is fixed. + * + * @see corbo::FullDiscretizationGridBase corbo::DiscretizationGridInterface + * FiniteDifferencesGridSE2 + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class FiniteDifferencesGridSE2 : public FullDiscretizationGridBaseSE2 +{ + public: + using Ptr = std::shared_ptr; + + using BaseEdge = corbo::BaseEdge; + using BaseMixedEdge = corbo::BaseMixedEdge; + + //! Default constructor + FiniteDifferencesGridSE2() = default; + //! Default destructor + virtual ~FiniteDifferencesGridSE2() = default; + + //! Return a newly created shared instance of the implemented class + corbo::DiscretizationGridInterface::Ptr getInstance() const override { return std::make_shared(); } + + //! Get access to the associated factory + static corbo::Factory& getFactory() { return corbo::Factory::instance(); } + + protected: + void createEdges(NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics) override; + + bool isDtFixedIntended() const override { return true; } +}; + +} // namespace mpc_local_planner + +#endif // FINITE_DIFFERENCES_GRID_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_variable_grid_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_variable_grid_se2.h new file mode 100644 index 0000000..2781e29 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_variable_grid_se2.h @@ -0,0 +1,98 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef FINITE_DIFFERENCES_VARIABLE_GRID_H_ +#define FINITE_DIFFERENCES_VARIABLE_GRID_H_ + +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief Finite differences grid with variable resolution for SE2 + * + * This class implements a full discretization grid with finite difference collocation. + * The temporal resolution is free. + * This grid corresponds to the global uniform grid in time-optimal MPC as defined in [1,2]. + * + * [1] C. Rösmann, F. Hoffmann und T. Bertram: Timed-Elastic-Bands for Time-Optimal Point-to-Point Nonlinear Model Predictive Control, ECC 2015. + * [2] C. Rösmann: Time-optimal nonlinear model predictive control - Direct transcription methods with variable discretization and structural sparsity + * exploitation. Dissertation, TU Dortmund University, Oct. 2019. + * + * @see FiniteDifferencesGridSE2 + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class FiniteDifferencesVariableGridSE2 : public FiniteDifferencesGridSE2 +{ + public: + using Ptr = std::shared_ptr; + using UPtr = std::unique_ptr; + + enum class GridAdaptStrategy { NoGridAdapt, TimeBasedSingleStep, TimeBasedAggressiveEstimate, SimpleShrinkingHorizon }; + + FiniteDifferencesVariableGridSE2() = default; + virtual ~FiniteDifferencesVariableGridSE2() = default; + + //! Return a newly created shared instance of the implemented class + corbo::DiscretizationGridInterface::Ptr getInstance() const override { return std::make_shared(); } + + //! Get access to the associated factory + static corbo::Factory& getFactory() { return corbo::Factory::instance(); } + + //! Set lower and upper bounds for the temporal resolution + void setDtBounds(double dt_lb, double dt_ub); + //! Set minium grid size for grid adaptation + void setNmin(int n_min) { _n_min = n_min; } + //! Disable grid adapation + void disableGridAdaptation() { _grid_adapt = GridAdaptStrategy::NoGridAdapt; } + //! Set grid adaptation strategy to 'single step' + void setGridAdaptTimeBasedSingleStep(int n_max, double dt_hyst_ratio = 0.1, bool adapt_first_iter = false); + //! Set grid adaptation strategy to 'aggressive' + void setGridAdaptTimeBasedAggressiveEstimate(int n_max, double dt_hyst_ratio = 0.1, bool adapt_first_iter = false); + //! Set grid adaptation strategy to 'shrinking horizon' + void setGridAdaptSimpleShrinkingHorizon(bool adapt_first_iter = false); + + protected: + bool isDtFixedIntended() const override { return false; } + + bool adaptGrid(bool new_run, NlpFunctions& nlp_fun) override; + bool adaptGridTimeBasedSingleStep(NlpFunctions& nlp_fun); + bool adaptGridTimeBasedAggressiveEstimate(NlpFunctions& nlp_fun); + bool adaptGridSimpleShrinkingHorizon(NlpFunctions& nlp_fun); + + bool isMovingHorizonWarmStartActive() const override { return false; } + bool isGridAdaptActive() const override { return true; } + + // auto resize stuff + GridAdaptStrategy _grid_adapt = GridAdaptStrategy::NoGridAdapt; + int _n_max = 1000; + double _dt_hyst_ratio = 0.1; + int _n_min = 2; + bool _adapt_first_iter = false; +}; + +} // namespace mpc_local_planner + +#endif // FINITE_DIFFERENCES_VARIABLE_GRID_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h new file mode 100644 index 0000000..c8dc516 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h @@ -0,0 +1,232 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef FULL_DISCRETIZATION_GRID_BASE_H_ +#define FULL_DISCRETIZATION_GRID_BASE_H_ + +#include + +#include +#include +#include + +#include + +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief Full discretization grid specialization for SE2 + * + * This class defines the basis for full discretization grids + * similar to corbo::FullDiscretizationGridBase. + * However, the main difference is, that this grid + * requires the first three state components to be embedded in SE2. + * Whereas the first two states are ordinary real numbers on (-inf, inf), + * the third component must remain in [-pi, pi). + * + * Therefore, the minimum dimension state dimension is 3. + * However, larger dimensions are allowed, but the related state components + * are considered as standard real numbers on (-inf, inf). + * + * Note the full discretization grid currently supports only finite difference collocation + * to discretize the continuous time dynamics. + * Full discretization is also possible with multiple shooting and (expicit) numerical integration, + * but for this purpose we need to specialize corbo::ShootingGridBase for SE2. + * + * @see corbo::FullDiscretizationGridBase corbo::DiscretizationGridInterface + * FiniteDifferencesGridSE2 + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class FullDiscretizationGridBaseSE2 : public corbo::DiscretizationGridInterface +{ + public: + using Ptr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ReferenceTrajectoryInterface = corbo::ReferenceTrajectoryInterface; + using SystemDynamicsInterface = corbo::SystemDynamicsInterface; + using NlpFunctions = corbo::NlpFunctions; + using OptimizationEdgeSet = corbo::OptimizationEdgeSet; + using VertexInterface = corbo::VertexInterface; + using ScalarVertex = corbo::ScalarVertex; + using VectorVertex = corbo::VectorVertex; + using PartiallyFixedVectorVertex = corbo::PartiallyFixedVectorVertex; + using TimeSeries = corbo::TimeSeries; + + enum class CostIntegrationRule { LeftSum, TrapezoidalRule }; + + FullDiscretizationGridBaseSE2() = default; + virtual ~FullDiscretizationGridBaseSE2() = default; + + //! Return a newly created shared instance of the implemented class + corbo::DiscretizationGridInterface::Ptr getInstance() const override = 0; + + // implements interface method + corbo::GridUpdateResult update(const Eigen::VectorXd& x0, ReferenceTrajectoryInterface& xref, ReferenceTrajectoryInterface& uref, + NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics, bool new_run, + const corbo::Time& t, ReferenceTrajectoryInterface* sref = nullptr, const Eigen::VectorXd* prev_u = nullptr, + double prev_u_dt = 0, ReferenceTrajectoryInterface* xinit = nullptr, + ReferenceTrajectoryInterface* uinit = nullptr) override; + + // implements interface method + double getFirstDt() const override { return getDt(); } + // implements interface method + double getFinalTime() const override { return double(getN() - 1) * getDt(); } + + // implements interface method + bool hasConstantControls() const override { return true; } + // implements interface method + bool hasSingleDt() const override { return true; } + // implements interface method + bool isTimeVariableGrid() const override { return !isDtFixedIntended(); } + // implements interface method + bool isUniformGrid() const override { return true; } + // implements interface method + bool providesStateTrajectory() const override { return true; } + // implements interface method + bool getFirstControlInput(Eigen::VectorXd& u0) override; + + //! Return state at time stamp k + const Eigen::VectorXd& getState(int k) const + { + assert(k <= getN()); + if (k == _x_seq.size()) return _xf.values(); + return _x_seq[k].values(); + } + + // implements interface method + void getStateAndControlTimeSeries(TimeSeries::Ptr x_sequence, TimeSeries::Ptr u_sequence, double t_max = corbo::CORBO_INF_DBL) const override; + + // implements interface method + void clear() override; + + // implements interface method + bool isEmpty() const override { return _x_seq.empty() || _u_seq.empty(); } + // implements interface method + virtual bool isValid() const { return (_x_seq.size() == _u_seq.size()); } + + // implements interface method + void setN(int n, bool try_resample = true) override; + // implements interface method + void setInitialDt(double dt) override { setDtRef(dt); } + // implements interface method + double getInitialDt() const override { return getDtRef(); } + // implements interface method + int getInitialN() const override { return getNRef(); } + + //! get reference horizon length + int getNRef() const { return _n_ref; } + //! get current horizon length + int getN() const override { return _x_seq.size() + 1; } + //! set reference horizon length + void setNRef(int n); + //! set reference temporal resolution + void setDtRef(double dt) { _dt_ref = dt; } + //! get current reference temporal resolution + double getDtRef() const { return _dt_ref; } + //! get current temporal resolution + double getDt() const { return _dt.value(); } + //! activate or deactive warmstart + void setWarmStart(bool active) { _warm_start = active; } + //! Set individual components of the final state to fixed or unfixed + void setXfFixed(const Eigen::Matrix& xf_fixed) + { + _xf_fixed = xf_fixed; + setModified(true); + } + + //! Set finite differences collocation method + void setFiniteDifferencesCollocationMethod(corbo::FiniteDifferencesCollocationInterface::Ptr fd_eval) { _fd_eval = fd_eval; } + //! Set cost integration rule + void setCostIntegrationRule(CostIntegrationRule integration) { _cost_integration = integration; } + + /** + * @brief Find the closest pose (first part of the state vector) on the grid w.r.t. to a provided reference point. + * + * @param[in] x_ref X-position of the reference point + * @param[in] y_ref Y-position of the reference point + * @param[in] begin_idx Start search at this time stamp + * @param[out] distance [optional] the resulting minimum distance + * @return Index to the closest state/pose on the grid + */ + int findClosestPose(double x_ref, double y_ref, int start_idx = 0, double* distance = nullptr) const; + + // implements interface method + std::vector& getActiveVertices() override { return _active_vertices; } + // implements interface method + void getVertices(std::vector& vertices) override; + + protected: + virtual void initializeSequences(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun); + virtual void initializeSequences(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, ReferenceTrajectoryInterface& xref, + ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun); + virtual void warmStartShifting(const Eigen::VectorXd& x0); + + virtual bool adaptGrid(bool new_run, NlpFunctions& nlp_fun) { return false; } // A subclass might add a grid adaptation, returns true if adapted + + int findNearestState(const Eigen::VectorXd& x0); + + void updateBounds(const NlpFunctions& nlp_fun); + + virtual void resampleTrajectory(int n_new); + + bool checkAndInitializeXfFixedFlags(int dim_x); + + virtual void createEdges(NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics) = 0; + + virtual bool isDtFixedIntended() const { return true; } + virtual bool isMovingHorizonWarmStartActive() const { return _warm_start; } + virtual bool isGridAdaptActive() const { return false; } + + void computeActiveVertices() override; + + corbo::FiniteDifferencesCollocationInterface::Ptr _fd_eval = std::make_shared(); + + std::vector _x_seq; + std::vector _u_seq; + PartiallyFixedVectorVertexSE2 _xf; + std::vector _active_vertices; + + const NlpFunctions* _nlp_fun = nullptr; // cache -> for bounds + + int _n_ref = 11; + int _n_adapt = 0; // if adaption is on and warmstart off, we might use this n instead of n_ref (only if n_adapt > 0) + double _dt_ref = 0.1; + ScalarVertex _dt; // we need a ScalarVertex to use the helper methods in stage_functions.cpp + bool _warm_start = false; + bool _first_run = true; + + // might be required if the last dt should be fixed or if dt is not fixed + Eigen::Matrix _xf_fixed; + double _dt_lb = 0; + double _dt_ub = corbo::CORBO_INF_DBL; + + CostIntegrationRule _cost_integration; +}; + +} // namespace mpc_local_planner + +#endif // FULL_DISCRETIZATION_GRID_BASE_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/min_time_via_points_cost.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/min_time_via_points_cost.h new file mode 100644 index 0000000..9abb615 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/min_time_via_points_cost.h @@ -0,0 +1,127 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef MIN_TIME_VIA_POINTS_COSTS_H_ +#define MIN_TIME_VIA_POINTS_COSTS_H_ + +#include +#include + +#include + +#include +#include +#include +#include + +namespace mpc_local_planner { + +/** + * @brief Hybrid cost function with minimum time and via-point objectives + * + * This class defines a joint objective of minimum time and via-point attraction. + * A user defined weight controls the trade-off between both individual objectives. + * + * The current via-point strategy is borrowed from the teb_local_planner. + * + * @todo we can implement this class as LSQ version as well + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class MinTimeViaPointsCost : public corbo::StageCost +{ + public: + using Ptr = std::shared_ptr; + + using ViaPointContainer = std::vector; + + //! Default constructor + MinTimeViaPointsCost() = default; + + /** + * @brief Construct with via-point container and weights + * + * @param[in] via_points Reference to the via-point container (must remain allocated) + * @param[in] position_weight Via-point attraction weight for the position part + * @param[in] orientation_weight Via-point attraction weight for the angular part + * @param[in] via_points_orderedx True, if via-points should be associated w.r.t. the ordering in \c via_points + */ + MinTimeViaPointsCost(const ViaPointContainer& via_points, double position_weight, double orientation_weight, bool via_points_ordered) + : _via_points_ordered(via_points_ordered) + { + setViaPointContainer(via_points); + setViaPointWeights(position_weight, orientation_weight); + } + + // implements interface method + corbo::StageCost::Ptr getInstance() const override { return std::make_shared(); } + + // implements interface method + bool hasNonIntegralTerms(int k) const override { return true; } + // implements interface method + bool hasIntegralTerms(int k) const override { return false; } + + // implements interface method + int getNonIntegralDtTermDimension(int k) const override { return (k == 0 || !_single_dt) ? 1 : 0; } + // implements interface method + bool isLsqFormNonIntegralDtTerm(int k) const override { return false; } + // implements interface method + int getNonIntegralStateTermDimension(int k) const override; + + // implements interface method + bool update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/, + corbo::ReferenceTrajectoryInterface* /*sref*/, bool single_dt, const Eigen::VectorXd& x0, + corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector& /*dts*/, + const corbo::DiscretizationGridInterface* grid) override; + + // implements interface method + void computeNonIntegralDtTerm(int k, double dt, Eigen::Ref cost) const override; + // implements interface method + void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; + + //! Set reference to via-point container (warning, object must remain allocated) + void setViaPointContainer(const ViaPointContainer& via_points) { _via_points = &via_points; } + //! Set if the optimzier should try to match the via-point ordering + void setViaPointOrderedMode(bool ordered) { _via_points_ordered = ordered; } + //! Set weights for via-point attraction + void setViaPointWeights(double position_weight, double orientation_weight) + { + _vp_position_weight = position_weight; + _vp_orientation_weight = orientation_weight; + } + + protected: + bool _single_dt = false; + double _time_weight = 1; + + bool _via_points_ordered = false; + double _vp_position_weight = 1e-3; + double _vp_orientation_weight = 0; + + const ViaPointContainer* _via_points = nullptr; + std::vector, std::size_t>> _vp_association; + // also store previous number of associated points to detect structure changes +}; + +} // namespace mpc_local_planner + +#endif // MIN_TIME_VIA_POINTS_COSTS_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/quadratic_cost_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/quadratic_cost_se2.h new file mode 100644 index 0000000..3badcd7 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/quadratic_cost_se2.h @@ -0,0 +1,140 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef QUADRATIC_COST_SE2_H_ +#define QUADRATIC_COST_SE2_H_ + +#include +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @brief Quadratic form running cost (specialized for SE2) + * + * This class implements quadratic form running costs + * \f[ + * l(x, u) = (x_{ref} - x)^T Q (x_{xref} - x) + (u_{ref} - u)^T R (u_{ref} - u) + * \f] + * However, this class ensures that the state distance (x_{ref} - x) is + * computed properly in SO(2) for the third state component. + * + * Note, the class also provides a least squares form if not in integral form which is: + * \f$ l_{lsq} = \begin{bmatrix} \sqrt{Q} (x_{ref} - x) \\ \sqrt{R} (u_{ref} - u) \end{bmatrix} \f$ + * \f$ \sqrt{Q} \f$ \f$ \sqrt{R} \f$ are computed once using the cholesky decomposition. + * This form is later squared by the solver. + * + * @see corbo::QuadraticFinalStateCost + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class QuadraticFormCostSE2 : public corbo::QuadraticFormCost +{ + public: + using Ptr = std::shared_ptr; + + //! Default constructor + QuadraticFormCostSE2() : corbo::QuadraticFormCost() {} + + /** + * @brief Construct with weight matrices + * + * @param[in] Q Positive definite state weighting matrix (must match dimensions!) + * @param[in] R Positive definite control weighting matrix (must match dimensions!) + * @param[in] integral_form If true, the running costs are later integrated over the grid interval, + * otherwise, the values are just summed up as in sampled-data MPC. + * @param[in] lsq_form Set to true in order to prefer the least-squares form + */ + QuadraticFormCostSE2(const Eigen::Ref& Q, const Eigen::Ref& R, bool integral_form = false, + bool lsq_form = false) + : corbo::QuadraticFormCost(Q, R, integral_form, lsq_form) + { + } + + // implements interface method + corbo::StageCost::Ptr getInstance() const override { return std::make_shared(); } + + // implements interface method + void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; + + // implements interface method + void computeIntegralStateControlTerm(int k, const Eigen::Ref& x_k, const Eigen::Ref& u_k, + Eigen::Ref cost) const override; +}; + +/** + * @brief Quadratic state running cost (specialized for SE2) + * + * This class implements quadratic state running costs + * \f[ + * l(x) = (x_{ref} - x)^T Q (x_{xref} - x) + * \f] + * However, this class ensures that the state distance (x_{ref} - x) is + * computed properly in SO(2) for the third state component. + * + * Note, the class also provides a least squares form if not in integral form which is: + * \f$ l_{lsq} = \sqrt{Q} (x_{ref} - x) \f$ + * \f$ \sqrt{Q} \f$ is computed once using the cholesky decomposition. + * This form is later squared by the solver. + * + * @see corbo::QuadraticFinalStateCost + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class QuadraticStateCostSE2 : public corbo::QuadraticStateCost +{ + public: + using Ptr = std::shared_ptr; + + //! Default constructor + QuadraticStateCostSE2() : corbo::QuadraticStateCost() {} + + /** + * @brief Construct with weight matrices + * + * @param[in] Q Positive definite state weighting matrix (must match dimensions!) + * @param[in] integral_form If true, the running costs are later integrated over the grid interval, + * otherwise, the values are just summed up as in sampled-data MPC. + * @param[in] lsq_form Set to true in order to prefer the least-squares form + */ + QuadraticStateCostSE2(const Eigen::Ref& Q, bool integral_form = false, bool lsq_form = false) + : corbo::QuadraticStateCost(Q, integral_form, lsq_form) + { + } + + // implements interface method + corbo::StageCost::Ptr getInstance() const override { return std::make_shared(); } + + // implements interface method + void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; + + // implements interface method + void computeIntegralStateControlTerm(int k, const Eigen::Ref& x_k, const Eigen::Ref& u_k, + Eigen::Ref cost) const override; +}; + +} // namespace mpc_local_planner + +#endif // QUADRATIC_COST_SE2_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h new file mode 100644 index 0000000..6300961 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h @@ -0,0 +1,123 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef STAGE_INEQUALITY_SE2_H_ +#define STAGE_INEQUALITY_SE2_H_ + +#include +#include + +#include + +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @brief Stage inequality constraint for obstacle avoidance and control deviation limits + * + * This class defines the inequality constraint for obstacle avoidance and control + * deviation limits (in case limits are active). + * + * The current obstacle association strategy is borrowed from the teb_local_planner. + * It takes the robot footprint model and the geometric obstacle shapes into account. + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class StageInequalitySE2 : public corbo::StageInequalityConstraint +{ + public: + using Ptr = std::shared_ptr; + + using ObstaclePtr = teb_local_planner::ObstaclePtr; + + // implements interface method + corbo::StageInequalityConstraint::Ptr getInstance() const override { return std::make_shared(); } + + // implements interface method + bool hasNonIntegralTerms(int k) const override { return true; } + // implements interface method + bool hasIntegralTerms(int k) const override { return false; } + + // implements interface method + int getNonIntegralStateTermDimension(int k) const override; + // implements interface method + int getNonIntegralControlDeviationTermDimension(int k) const override; + + // implements interface method + bool update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/, + corbo::ReferenceTrajectoryInterface* /*sref*/, bool /* single_dt*/, const Eigen::VectorXd& x0, + corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector& /*dts*/, + const corbo::DiscretizationGridInterface* grid) override; + + // implements interface method + void computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const override; + // implements interface method + void computeNonIntegralControlDeviationTerm(int k, const Eigen::Ref& u_k, const Eigen::Ref& u_prev, + double dt, Eigen::Ref cost) const override; + + //! Set reference to obstacle vector (must remain allocated) + void setObstacleVector(const teb_local_planner::ObstContainer& obstacles) { _obstacles = &obstacles; } + //! Set robot footprint model + void setRobotFootprintModel(teb_local_planner::RobotFootprintModelPtr robot_model) { _robot_model = robot_model; } + + //! Set minimum distance allowed + void setMinimumDistance(double min_dist) { _min_obstacle_dist = min_dist; } + //! Set parameters for prior filtering of obstacles + void setObstacleFilterParameters(double force_inclusion_factor, double cutoff_factor) + { + _obstacle_filter_force_inclusion_factor = force_inclusion_factor; + _obstacle_filter_cutoff_factor = cutoff_factor; + } + //! Set to true to enable dynamic obstacle (constant-velocity prediction) + void setEnableDynamicObstacles(bool enable_dyn_obst) { _enable_dynamic_obstacles = enable_dyn_obst; } + //! Specify lower and upper bounds for limiting the control deviation + void setControlDeviationBounds(const Eigen::VectorXd& du_lb, const Eigen::VectorXd& du_ub); + + //! Get current minimum distance parameter value + double getMinimumDistance() const { return _min_obstacle_dist; } + + protected: + const teb_local_planner::ObstContainer* _obstacles = nullptr; + std::vector _relevant_obstacles; // TODO: we can also store raw pointers as _via_points is locked by mutex + + teb_local_planner::RobotFootprintModelPtr _robot_model; + + double _current_dt = 0.1; + int _num_du_lb_finite = 0; + int _num_du_ub_finite = 0; + + double _min_obstacle_dist = 0.1; + double _obstacle_filter_force_inclusion_factor = 1.5; + double _obstacle_filter_cutoff_factor = 5; + bool _enable_dynamic_obstacles = false; + + Eigen::VectorXd _du_lb; + Eigen::VectorXd _du_ub; +}; + +} // namespace mpc_local_planner + +#endif // STAGE_INEQUALITY_SE2_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/optimal_control/vector_vertex_se2.h b/mpc_local_planner/include/mpc_local_planner/optimal_control/vector_vertex_se2.h new file mode 100644 index 0000000..7acaeb4 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/optimal_control/vector_vertex_se2.h @@ -0,0 +1,316 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef VECTOR_SE_VERTEX_H_ +#define VECTOR_SE_VERTEX_H_ + +#include + +#include + +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @brief Vertex specialization for vectors in SE2 + * + * This class defines a vector vertex in which the first + * three components must be defined in SE2. + * Whereas the first two states are ordinary real numbers on (-inf, inf), + * the third component must remain in [-pi, pi). + * + * The minimum dimension of this vertex is 3. + * This vertex allows for larger dimensions of 3, + * but these components are considered as standard real numbers on (-inf, inf). + * + * @see corbo::VertexInterface corbo::PartiallyFixedVectorVertex + * corbo::HyperGraph corbo::EdgeInterface corbo::ScalarVertex + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class VectorVertexSE2 : public corbo::VectorVertex +{ + public: + using Ptr = std::shared_ptr; + using UPtr = std::unique_ptr; + + //! Default constructor + VectorVertexSE2() = default; + + explicit VectorVertexSE2(bool fixed) : corbo::VectorVertex(fixed) {} + + //! Construct and allocate memory for a given dimension + explicit VectorVertexSE2(int dimension, bool fixed = false) : VectorVertex(dimension, fixed) {} + + //! Construct vertex with given values + explicit VectorVertexSE2(const Eigen::Ref& values, bool fixed = false) : VectorVertex(values, fixed) {} + + //! Construct vertex with given values, lower and upper bounds + explicit VectorVertexSE2(const Eigen::Ref& values, const Eigen::Ref& lb, + const Eigen::Ref& ub, bool fixed = false) + : VectorVertex(values, lb, ub, fixed) + { + } + + // implements interface method + void plus(int idx, double inc) override + { + if (idx == 2) + _values[idx] = normalize_theta(_values[idx] + inc); + else + _values[idx] += inc; + } + // implements interface method + void plus(const double* inc) override + { + assert(getDimension() >= 3); + _values[0] += inc[0]; + _values[1] += inc[1]; + _values[2] = normalize_theta(_values[2] + inc[2]); + if (getDimension() > 3) _values.tail(getDimension() - 3) += Eigen::Map(inc + 3, getDimension() - 3); + } + // implements interface method + void plusUnfixed(const double* inc) override { plus(inc); } + + // implements interface method + void setData(int idx, double data) override + { + if (idx == 2) + _values[idx] = normalize_theta(data); + else + _values[idx] = data; + } + + //! Set values and bounds at once + virtual void set(const Eigen::Ref& values, const Eigen::Ref& lb, + const Eigen::Ref& ub, bool fixed = false) + { + assert(values.size() == lb.size()); + assert(values.size() == ub.size()); + assert(values.size() >= 3); + _values = values; + _values[2] = normalize_theta(_values[2]); + setLowerBounds(lb); + setUpperBounds(ub); + + setFixed(false); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/** + * @brief VectorVertexSE2 with support for partially fixed components + * + * + * The vertex extends VectorVertexSE2 by allowing the user to + * partially fix components of the underlying vector. + * + * @see VectorVertexSE2 corbo::VertexInterface corbo::VectorVertex + * corbo::HyperGraph corbo::EdgeInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class PartiallyFixedVectorVertexSE2 : public VectorVertexSE2 +{ + public: + using Ptr = std::shared_ptr; + using UPtr = std::unique_ptr; + + //! Default constructor + PartiallyFixedVectorVertexSE2() = default; + + explicit PartiallyFixedVectorVertexSE2(int dimension) + : VectorVertexSE2(dimension), _fixed(Eigen::Array::Constant(dimension, false)), _num_unfixed(dimension) + { + } + + //! Construct and allocate memory for a given dimension + explicit PartiallyFixedVectorVertexSE2(int dimension, const Eigen::Ref>& fixed) + : VectorVertexSE2(dimension), _fixed(fixed), _num_unfixed(dimension) + { + } + + //! Construct vertex with given values + explicit PartiallyFixedVectorVertexSE2(const Eigen::Ref& values) + : VectorVertexSE2(values), _fixed(Eigen::Array::Constant(values.size(), false)), _num_unfixed(values.size()) + { + } + + //! Construct vertex with given values and fixed components + explicit PartiallyFixedVectorVertexSE2(const Eigen::Ref& values, const Eigen::Ref>& fixed) + : VectorVertexSE2(values), _fixed(fixed), _num_unfixed(fixed.size() - fixed.count()) + { + } + + //! Construct vertex with given values, lower and upper bounds + explicit PartiallyFixedVectorVertexSE2(const Eigen::Ref& values, const Eigen::Ref& lb, + const Eigen::Ref& ub) + : VectorVertexSE2(values, lb, ub), _fixed(Eigen::Array::Constant(values.size(), false)), _num_unfixed(values.size()) + { + } + + // implements interface method + int getDimensionUnfixed() const override { return _num_unfixed; } + + // implements parent method + void setDimension(int dim) override + { + VectorVertexSE2::setDimension(dim); + _fixed.setConstant(dim, false); + _num_unfixed = dim; + } + + //! Set values and bounds at once + void set(const Eigen::Ref& values, const Eigen::Ref& lb, + const Eigen::Ref& ub, bool fixed = false) override + { + assert(values.size() == lb.size()); + assert(values.size() == ub.size()); + assert(values.size() >= 3); + _values = values; + _values[2] = normalize_theta(_values[2]); + setLowerBounds(lb); + setUpperBounds(ub); + + setFixed(fixed); + } + + //! Set values and bounds at once (overload with fixed vector) + void set(const Eigen::Ref& values, const Eigen::Ref& lb, + const Eigen::Ref& ub, const Eigen::Ref>& fixed) + { + assert(values.size() == lb.size()); + assert(values.size() == ub.size()); + assert(values.size() >= 3); + _values = values; + _values[2] = normalize_theta(_values[2]); + setLowerBounds(lb); + setUpperBounds(ub); + + setFixed(fixed); + } + + //! Set component with idx (0 <= idx < dimension()) to (un)fixed + void setFixed(int idx, bool fixed) + { + _fixed[idx] = fixed; + _num_unfixed = getDimension() - _fixed.count(); + } + + //! Set logical array [dimension() x 1] in order to fix selected components + void setFixed(const Eigen::Ref>& fixed) + { + _fixed = fixed; + _num_unfixed = getDimension() - _fixed.count(); + } + + // implements interface method + void setFixed(bool fixed) override + { + _fixed.setConstant(_values.size(), fixed); + _num_unfixed = fixed ? 0 : getDimension(); + } + + // implements interface method + void plusUnfixed(const double* inc) override + { + int idx = 0; + for (int i = 0; i < getDimension(); ++i) + { + if (!_fixed(i)) + { + plus(i, inc[idx]); + ++idx; + } + } + } + + // implements interface method + bool hasFixedComponents() const override { return _num_unfixed < getDimension(); } + // implements interface method + bool isFixedComponent(int idx) const override { return _fixed[idx]; } + + // implements interface method + int getNumberFiniteLowerBounds(bool unfixed_only) const override + { + if (unfixed_only && _num_unfixed > 0) + { + int num = 0; + for (int i = 0; i < getDimension(); ++i) + { + if (!_fixed[i] && _lb[i] > -corbo::CORBO_INF_DBL) num += 1; + } + return num; + } + else + return (_lb.array() > -corbo::CORBO_INF_DBL).count(); + } + + // implements interface method + int getNumberFiniteUpperBounds(bool unfixed_only) const override + { + if (unfixed_only && _num_unfixed > 0) + { + int num = 0; + for (int i = 0; i < getDimension(); ++i) + { + if (!_fixed[i] && _ub[i] < corbo::CORBO_INF_DBL) num += 1; + } + return num; + } + else + return (_ub.array() < corbo::CORBO_INF_DBL).count(); + } + + // implements interface method + int getNumberFiniteBounds(bool unfixed_only) const override + { + if (unfixed_only && _num_unfixed > 0) + { + int num = 0; + for (int i = 0; i < getDimension(); ++i) + { + if (!_fixed[i] && (_ub[i] < corbo::CORBO_INF_DBL || _lb[i] > -corbo::CORBO_INF_DBL)) num += 1; + } + return num; + } + else + return (_ub.array() < corbo::CORBO_INF_DBL || _lb.array() > -corbo::CORBO_INF_DBL).count(); + } + + //! Read-only access to the underlying logical array for fixed components + const Eigen::Array fixedArray() const { return _fixed; } + + protected: + Eigen::Array _fixed; + int _num_unfixed; +}; + +} // namespace mpc_local_planner + +#endif // SRC_OPTIMIZATION_INCLUDE_CORBO_OPTIMIZATION_HYPER_GRAPH_VECTOR_VERTEX_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/systems/base_robot_se2.h b/mpc_local_planner/include/mpc_local_planner/systems/base_robot_se2.h new file mode 100644 index 0000000..3332a94 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/systems/base_robot_se2.h @@ -0,0 +1,108 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef SYSTEMS_BASE_ROBOT_SE2_H_ +#define SYSTEMS_BASE_ROBOT_SE2_H_ + +#include + +namespace mpc_local_planner { + +/** + * @brief Specialization of RobotDynamicsInterface for mobile robots operating in SE2 + * + * This abstract class defines a base class for robots in which the full state vector x + * is embedded in SE2, i.e. x = [pos_x, pos_y, theta] with pos_x, pos_y as real numbers + * and theta in [-pi, pi). + * + * Note, these models allow for simple conversion between state vectors and poses. + * + * @see RobotDynamicsInterface corbo::SystemDynamicsInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class BaseRobotSE2 : public RobotDynamicsInterface +{ + public: + using Ptr = std::shared_ptr; + + //! Default constructor + BaseRobotSE2() = default; + + // implements interface method + corbo::SystemDynamicsInterface::Ptr getInstance() const override = 0; + + // implements interface method + int getInputDimension() const override = 0; + // implements interface method + int getStateDimension() const override { return 3; } + + // implements interface method + bool isContinuousTime() const override { return true; } + + // implements interface method + bool isLinear() const override { return false; } + + // implements interface method + void getPositionFromState(const Eigen::Ref& x, double& pos_x, double& pos_y) const override + { + assert(x.size() == getStateDimension()); + pos_x = x[0]; + pos_y = x[1]; + } + + // implements interface method + void getPoseSE2FromState(const Eigen::Ref& x, double& pos_x, double& pos_y, double& theta) const override + { + assert(x.size() == getStateDimension()); + pos_x = x[0]; + pos_y = x[1]; + theta = x[2]; + } + + // implements interface method + void getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref x) const override + { + assert(x.size() == getStateDimension()); + x[0] = pos_x; + x[1] = pos_y; + x[2] = theta; + if (x.size() > 3) x.tail(x.size() - 3).setZero(); + } + + // implements interface method + virtual bool mergeStateFeedbackAndOdomFeedback(const teb_local_planner::PoseSE2& odom_pose, const geometry_msgs::Twist& odom_twist, + Eigen::Ref x) const override + { + assert(x.size() == getStateDimension()); + x[0] = odom_pose.x(); + x[1] = odom_pose.y(); + x[2] = odom_pose.theta(); + } + + // implements interface method + void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override = 0; +}; + +} // namespace mpc_local_planner + +#endif // SYSTEMS_BASE_ROBOT_SE2_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/systems/robot_dynamics_interface.h b/mpc_local_planner/include/mpc_local_planner/systems/robot_dynamics_interface.h new file mode 100644 index 0000000..3b34750 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/systems/robot_dynamics_interface.h @@ -0,0 +1,151 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_ +#define SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_ + +#include +#include +#include +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief Specialization of SystemDynamicsInterface for mobile robots + * + * This abstract class extends cobro::SystemDynamicsInterface + * by (abstract) conversion methods between states, poses embedded in SE2, + * controls and twist. + * + * @see BaseRobotSE2 corbo::SystemDynamicsInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class RobotDynamicsInterface : public corbo::SystemDynamicsInterface +{ + public: + using Ptr = std::shared_ptr; + + /** + * @brief Convert state vector to position (x,y) + * + * @param[in] x Reference to the state vector [getStateDimension() x 1] + * @param[out] pos_x X-component of the position + * @param[out] pos_y Y-component of the position + */ + virtual void getPositionFromState(const Eigen::Ref& x, double& pos_x, double& pos_y) const = 0; + + /** + * @brief Convert state vector to pose (x,y,theta) + * + * @param[in] x Reference to the state vector [getStateDimension() x 1] + * @param[out] pos_x X-component of the position part + * @param[out] pos_y Y-component of the position part + * @param[out] theta Orientation (yaw angle) in [-pi, pi) + */ + virtual void getPoseSE2FromState(const Eigen::Ref& x, double& pos_x, double& pos_y, double& theta) const = 0; + + /** + * @brief Convert state vector to PoseSE2 object + * + * @param[in] x Reference to the state vector [getStateDimension() x 1] + * @param[out] pose PoseSE2 object + */ + virtual void getPoseSE2FromState(const Eigen::Ref& x, teb_local_planner::PoseSE2& pose) const + { + getPoseSE2FromState(x, pose.x(), pose.y(), pose.theta()); + } + + /** + * @brief Convert pose (x,y,theta) to steady state + * + * Converts a pose to state vector with dimensions [getStateDimension() x 1]. + * In case getStateDimension() > 3, the state information is incomplete + * and hence it is assumed that a steady state for this particular pose is avaiable, + * which means that there exist a control u such that f(x,u) = 0 (continuous time) or + * f(x,u) = x (discrete time). + * + * For example, in case the remaining state components correspond to just integrators, + * they can be set to zero. + * + * @param[in] pos_x X-component of the position part + * @param[in] pos_y Y-component of the position part + * @param[in] theta Orientation (yaw angle) in [-pi, pi) + * @param[out] x Reference to the state vector [getStateDimension() x 1] + */ + virtual void getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref x) const = 0; + + /** + * @brief Convert PoseSE2 to steady state + * + * See description of getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref x) + * + * @param[in] pose PoseSE2 object + * @param[out] x Reference to the state vector [getStateDimension() x 1] + */ + virtual void getSteadyStateFromPoseSE2(const teb_local_planner::PoseSE2& pose, Eigen::Ref x) const + { + getSteadyStateFromPoseSE2(pose.x(), pose.y(), pose.theta(), x); + } + + /** + * @brief Convert control vector to twist message + * + * Convert the control vector to a twist message (containing velocity information) + * if possible. + * + * @todo Maybe add current state x as optional input to allow for computing a twist out of the first state and control + * + * @param[in] u Reference to the control vector [getInputDimension() x 1] + * @param[out] twist Reference to the twist message + * @return true, if conversion was successful, false otherwise + */ + virtual bool getTwistFromControl(const Eigen::Ref& u, geometry_msgs::Twist& twist) const = 0; + + /** + * @brief Merge custom state feedback with pose and twist feedback + * + * Many robots in ROS publish their state information (pose, linear and angular velocity)e.g. via an odometry message + * (potentially corrected by localization). + * However, for more complex robot models, it might not be possible to obtain the full state information without any observer + * or further measurements. + * For this purpose, custom state feedback can be provided. But as the complete navigation stack relies on the information obtained by + * odometry, this method allows for merging of both sources. + * In particular, it overrides only state components related to the pose and velocity. + * + * @param[in] odom_pose PoseSE2 object obtained from odometry + * @param[in] odom_twist geometry::Twist message obtained from odometry + * @param[in,out] x Custom state feedback in which related will be overriden [getStateDimension() x 1] + * @return true, if merging was successful, false otherwise + */ + virtual bool mergeStateFeedbackAndOdomFeedback(const teb_local_planner::PoseSE2& odom_pose, const geometry_msgs::Twist& odom_twist, + Eigen::Ref x) const = 0; + + virtual void reset() {} +}; + +} // namespace mpc_local_planner + +#endif // SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/systems/simple_car.h b/mpc_local_planner/include/mpc_local_planner/systems/simple_car.h new file mode 100644 index 0000000..db2f720 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/systems/simple_car.h @@ -0,0 +1,146 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef SYSTEMS_SIMPLE_CAR_H_ +#define SYSTEMS_SIMPLE_CAR_H_ + +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief Simple car model + * + * This class implements the dynamics for a simple car model + * in which the rear wheels are actuated. + * The front wheels are the steering wheels (for wheelbase > 0). + * The state vector [x, y, theta] is defined at the center of the rear axle. + * See [1,2] for a mathematical description and a figure. + * + * [1] S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006. + * (Chapter 13, http://planning.cs.uiuc.edu/) + * [2] A. De Luca et al., Feedback Control of a Nonholonomic Car-like Robot, + * in Robot Motion Planning and Control (Ed. J.-P. Laumond), Springer, 1998. + * (https://homepages.laas.fr/jpl/promotion/chap4.pdf) + * + * @see SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface + * corbo::SystemDynamicsInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class SimpleCarModel : public BaseRobotSE2 +{ + public: + //! Default constructor + SimpleCarModel() = default; + + //! Constructs model with given wheelbase + SimpleCarModel(double wheelbase) : _wheelbase(wheelbase) {} + + // implements interface method + SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared(); } + + // implements interface method + int getInputDimension() const override { return 2; } + + // implements interface method + void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override + { + assert(x.size() == getStateDimension()); + assert(u.size() == getInputDimension()); + assert(x.size() == f.size() && "SimpleCarModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f."); + + f[0] = u[0] * std::cos(x[2]); + f[1] = u[0] * std::sin(x[2]); + f[2] = u[0] * std::tan(u[1]) / _wheelbase; + } + + // implements interface method + bool getTwistFromControl(const Eigen::Ref& u, geometry_msgs::Twist& twist) const override + { + assert(u.size() == getInputDimension()); + twist.linear.x = u[0]; + twist.linear.y = twist.linear.y = 0; + + twist.angular.z = u[1]; // warning, this is the angle and not the angular vel + twist.angular.x = twist.angular.y = 0; + + return true; + } + + //! Set wheelbase + void setWheelbase(double wheelbase) { _wheelbase = wheelbase; } + //! Get wheelbase + double getWheelbase() const { return _wheelbase; } + + protected: + double _wheelbase = 1.0; +}; + +/** + * @brief Simple car model with front wheel actuation + * + * This class implements the dynamics for a simple car model + * in which the front wheels are actuated and steered (for wheelbase > 0). + * The state vector [x, y, theta] is defined at the center of the rear axle. + * See [1] for a mathematical description and a figure. + * + * [1] A. De Luca et al., Feedback Control of a Nonholonomic Car-like Robot, + * in Robot Motion Planning and Control (Ed. J.-P. Laumond), Springer, 1998. + * (https://homepages.laas.fr/jpl/promotion/chap4.pdf) + * + * @see SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface + * corbo::SystemDynamicsInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class SimpleCarFrontWheelDrivingModel : public SimpleCarModel +{ + public: + //! Default constructor + SimpleCarFrontWheelDrivingModel() = default; + + //! Constructs model with given wheelbase + SimpleCarFrontWheelDrivingModel(double wheelbase) : SimpleCarModel(wheelbase) {} + + // implements interface method + SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared(); } + + // implements interface method + void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override + { + assert(x.size() == getStateDimension()); + assert(u.size() == getInputDimension()); + assert(x.size() == f.size() && + "SimpleCarFrontWheelDrivingModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f."); + + f[0] = u[0] * std::cos(x[2]); + f[1] = u[0] * std::sin(x[2]); + f[2] = u[0] * std::sin(u[1]) / _wheelbase; + } +}; + +} // namespace mpc_local_planner + +#endif // SYSTEMS_SIMPLE_CAR_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/systems/unicycle_robot.h b/mpc_local_planner/include/mpc_local_planner/systems/unicycle_robot.h new file mode 100644 index 0000000..f6a2882 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/systems/unicycle_robot.h @@ -0,0 +1,85 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef SYSTEMS_UNICYCLE_ROBOT_H_ +#define SYSTEMS_UNICYCLE_ROBOT_H_ + +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief Unicycle model + * + * This class implements the dynamics for a unicycle model + * which can be used e.g., for differential-drive robots. + * See [1] for a mathematical description and a figure. + * + * [1] S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006. + * (Chapter 13, http://planning.cs.uiuc.edu/) + * + * @see BaseRobotSE2 RobotDynamicsInterface corbo::SystemDynamicsInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class UnicycleModel : public BaseRobotSE2 +{ + public: + //! Default constructor + UnicycleModel() = default; + + // implements interface method + SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared(); } + + // implements interface method + int getInputDimension() const override { return 2; } + + // implements interface method + void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override + { + assert(x.size() == getStateDimension()); + assert(u.size() == getInputDimension()); + assert(x.size() == f.size() && "UnicycleModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f."); + + f[0] = u[0] * std::cos(x[2]); + f[1] = u[0] * std::sin(x[2]); + f[2] = u[1]; + } + + // implements interface method + bool getTwistFromControl(const Eigen::Ref& u, geometry_msgs::Twist& twist) const override + { + assert(u.size() == getInputDimension()); + twist.linear.x = u[0]; + twist.linear.y = twist.linear.y = 0; + + twist.angular.z = u[1]; + twist.angular.x = twist.angular.y = 0; + return true; + } +}; + +} // namespace mpc_local_planner + +#endif // SYSTEMS_UNICYCLE_ROBOT_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/utils/conversion.h b/mpc_local_planner/include/mpc_local_planner/utils/conversion.h new file mode 100644 index 0000000..1090d2d --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/utils/conversion.h @@ -0,0 +1,56 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef UTILS_CONVERSION_H_ +#define UTILS_CONVERSION_H_ + +#include +#include +#include + +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @brief Convert TimeSeries to pose array + * + * Converts TimeSeries to std::vector. + * + * @remarks Note, the actual data of the TimeSeries object is copied without interpolation. + * + * @todo We could avoid the system dynamics dependency by specifying a generic getter function for the SE2 poses + * + * @param[in] time_series corbo::TimeSeries object or any child class + * @param[in] dynamics Reference to the robot dynamics interface (to access state-to-SE2 conversion methods) + * @param[out] poses_stamped The resulting pose array (note, the incoming vector will be cleared) + * @param[in] frame_id The planning frame id that is added to the message header + */ +void convert(const corbo::TimeSeries& time_series, const RobotDynamicsInterface& dynamics, std::vector& poses_stamped, + const std::string& frame_id); + +} // namespace mpc_local_planner + +#endif // UTILS_CONVERSION_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/utils/math_utils.h b/mpc_local_planner/include/mpc_local_planner/utils/math_utils.h new file mode 100644 index 0000000..9cd471b --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/utils/math_utils.h @@ -0,0 +1,107 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef UTILS_MATH_UTILS_H_ +#define UTILS_MATH_UTILS_H_ + +#include + +namespace mpc_local_planner { + +/** + * @brief Return the average angle of an arbitrary number of given angles [rad] + * @param angles vector containing all angles + * @return average / mean angle, that is normalized to [-pi, pi] + */ +inline double average_angles(const std::vector& angles) +{ + double x = 0, y = 0; + for (std::vector::const_iterator it = angles.begin(); it != angles.end(); ++it) + { + x += std::cos(*it); + y += std::sin(*it); + } + if (x == 0 && y == 0) + return 0; + else + return std::atan2(y, x); +} + +/** + * @brief Calculate Euclidean distance between two 2D point datatypes + * @param point1 object containing fields x and y + * @param point2 object containing fields x and y + * @return Euclidean distance: ||point2-point1|| + */ +template +inline double distance_points2d(const P1& point1, const P2& point2) +{ + return std::sqrt(std::pow(point2.x - point1.x, 2) + std::pow(point2.y - point1.y, 2)); +} + +//! Calculate Euclidean distance between two 2D points +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)); } + +/** + * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) + * @param v1 object containing public methods x() and y() + * @param v2 object containing fields x() and y() + * @return magnitude that would result in the 3D case (along the z-axis) + */ +template +inline double cross2d(const V1& v1, const V2& v2) +{ + return v1.x() * v2.y() - v2.x() * v1.y(); +} + +/** + * @brief normalize angle to interval [-pi, pi) + * @remark This function is based on normalize_theta from g2o + * see: https://github.com/RainerKuemmerle/g2o/blob/master/g2o/stuff/misc.h + */ +inline double normalize_theta(double theta) +{ + if (theta >= -M_PI && theta < M_PI) return theta; + + double multiplier = std::floor(theta / (2.0 * M_PI)); + theta = theta - multiplier * 2.0 * M_PI; + if (theta >= M_PI) theta -= 2.0 * M_PI; + if (theta < -M_PI) theta += 2.0 * M_PI; + + return theta; +} + +/** + * @brief Return the interpolated angle between two angles [rad] + * @param angle1 + * @param angle2 + * @param factor in [0,1], or (1,inf) for extrapolation + * @return average / mean angle, that is normalized to [-pi, pi] + */ +inline double interpolate_angle(double angle1, double angle2, double factor) +{ + return normalize_theta(angle1 + factor * normalize_theta(angle2 - angle1)); +} + +} // namespace mpc_local_planner + +#endif // UTILS_MATH_UTILS_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/utils/publisher.h b/mpc_local_planner/include/mpc_local_planner/utils/publisher.h new file mode 100644 index 0000000..517c772 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/utils/publisher.h @@ -0,0 +1,150 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef UTILS_PUBLISHER_H_ +#define UTILS_PUBLISHER_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief This class provides publishing methods for common messages + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class Publisher +{ + using ObstaclePtr = teb_local_planner::ObstaclePtr; + using ObstContainer = teb_local_planner::ObstContainer; + using PointObstacle = teb_local_planner::PointObstacle; + using CircularObstacle = teb_local_planner::CircularObstacle; + using LineObstacle = teb_local_planner::LineObstacle; + using PolygonObstacle = teb_local_planner::PolygonObstacle; + using Point2dContainer = teb_local_planner::Point2dContainer; + + public: + /** + * @brief Default constructor + * @remarks do not forget to call initialize() + */ + Publisher() = default; + + /** + * @brief Constructor that initializes the class and registers topics + * @param[in] nh local ros::NodeHandle + * @param[in] map_frame the planning frame name + */ + Publisher(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame); + + /** + * @brief Initializes the class and registers topics. + * + * Call this function if only the default constructor has been called before. + * @param[in] nh local ros::NodeHandle + * @param[in] map_frame the planning frame name + */ + void initialize(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame); + + /** + * @brief Publish a given local plan to the ros topic \e ../../local_plan + * @param[in] local_plan Pose array describing the local plan + */ + void publishLocalPlan(const std::vector& local_plan) const; + + /** + * @brief Publish a given local plan to the ros topic \e ../../local_plan + * @param[in] local_plan Pose array describing the local plan + */ + void publishLocalPlan(const corbo::TimeSeries& ts) const; + + /** + * @brief Publish a given global plan to the ros topic \e ../../global_plan + * @param[in] global_plan Pose array describing the global plan + */ + void publishGlobalPlan(const std::vector& global_plan) const; + + /** + * @brief Publish the visualization of the robot model + * + * @param[in] current_pose Current pose of the robot + * @param[in] robot_model Subclass of BaseRobotFootprintModel + * @param[in] map_frame frame name for the msg header + * @param[in] ns Namespace for the marker objects + * @param[in] color Color of the footprint + */ + void publishRobotFootprintModel(const teb_local_planner::PoseSE2& current_pose, const teb_local_planner::BaseRobotFootprintModel& robot_model, + const std::string& ns = "RobotFootprintModel", const std_msgs::ColorRGBA& color = toColorMsg(0.5, 0.0, 0.8, 0.0)); + + /** + * @brief Publish obstacle positions to the ros topic \e ../../mpc_markers + * @todo Move filling of the marker message to polygon class in order to avoid checking types. + * @param[in] obstacles Obstacle container + */ + void publishObstacles(const teb_local_planner::ObstContainer& obstacles) const; + + /** + * @brief Publish via-points to the ros topic \e ../../teb_markers + * + * @todo add option to switch between points and poses (including orientation) to be published + * + * @param[in] via_points via-point container + * @param[in] ns marker namespace + */ + void publishViaPoints(const std::vector& via_points, const std::string& ns = "ViaPoints") const; + + /** + * @brief Helper function to generate a color message from single values + * @param[in] a Alpha value + * @param[in] r Red value + * @param[in] g Green value + * @param[in] b Blue value + * @return Color message + */ + static std_msgs::ColorRGBA toColorMsg(float a, float r, float g, float b); + + private: + bool _initialized = false; + + std::string _map_frame = "map"; + + RobotDynamicsInterface::Ptr _system; + + ros::Publisher _local_plan_pub; + ros::Publisher _global_plan_pub; + ros::Publisher _mpc_marker_pub; +}; + +} // namespace mpc_local_planner + +#endif // UTILS_PUBLISHER_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/utils/time_series_se2.h b/mpc_local_planner/include/mpc_local_planner/utils/time_series_se2.h new file mode 100644 index 0000000..d48bd83 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/utils/time_series_se2.h @@ -0,0 +1,93 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef UTILS_TIME_SERIES_SE2_H_ +#define UTILS_TIME_SERIES_SE2_H_ + +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief Time Series with SE2 support + * + * A time series represents a sequence of vectors of floating point numbers + * associated with time stamps. A time series might also be interpreted as a + * discrete-time trajectory. + * This class extends corbo::TimeSeries to support SE2 poses. + * Hereby, the third state component is always treated as orientation in [-pi, pi). + * In particular, whenever values are retrieved by interpolation + * (getValuesInterpolate()), the third state is ensured to remain properly in [-pi, pi). + * + * Note, as the first three state components define the SE2, any further auxiliary + * state component is treated as ordinary real number. + * + * @see corbo::TimeSeries + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class TimeSeriesSE2 : public corbo::TimeSeries +{ + public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + //! Default constructor + TimeSeriesSE2() = default; + //! Construct empty time series with a dresired value vector dimension + explicit TimeSeriesSE2(int value_dim) : TimeSeries(value_dim) {} + + /** + * @brief Retrieve value vector at a desired time stamp (seconds) via interpolation + * + * This method interpolates the underlying time series object at a desired + * time stamp (given in seconds). Interpolation and extrapolation settings + * can be passed as optional arguments. + * + * @warning This method currently assumes that the values / time pairs are + * stored with monotonically increasing time stamps (in time()). + * + * @param[in] time desired time stamp + * @param[out] values interpolated value vector w.r.t. \c time + * @param[in] interpolation interpolation method according to Interpolation enum + * @param[in] extrapolate specify whether exrapliation should be applied with a similar + * method like interpolation + * @param[in] tolerance specify a tolerance for which two time stamps are treated equally + * @returns true if successfull, false otherwise (e.g. if extrapolate is false but \c time > max(time) + */ + bool getValuesInterpolate(double time, Eigen::Ref values, Interpolation interpolation = Interpolation::Linear, + Extrapolation extrapolate = Extrapolation::NoExtrapolation, double tolerance = 1e-6) const override; + + //! Compute and return the mean value of all values among all dimensions + double computeMeanOverall() override; + /** + * @brief Compute and return the component-wise mean values + * @param[out] mean_values Resulting cwise mean values [getValueDimension() x 1] (vector size must be preallocated) + */ + void computeMeanCwise(Eigen::Ref mean_values) override; +}; + +} // namespace mpc_local_planner + +#endif // UTILS_TIME_SERIES_SE2_H_ diff --git a/mpc_local_planner/launch/test_mpc_optim_node.launch b/mpc_local_planner/launch/test_mpc_optim_node.launch new file mode 100644 index 0000000..ccc5700 --- /dev/null +++ b/mpc_local_planner/launch/test_mpc_optim_node.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mpc_local_planner/mpc_local_planner_plugin.xml b/mpc_local_planner/mpc_local_planner_plugin.xml new file mode 100644 index 0000000..8a52e25 --- /dev/null +++ b/mpc_local_planner/mpc_local_planner_plugin.xml @@ -0,0 +1,14 @@ + + + + The mpc_local_planner package implements a plugin + to the base_local_planner of the 2D navigation stack. + + + + + Same plugin implemented MBF CostmapController's extended interface. + + + + diff --git a/mpc_local_planner/package.xml b/mpc_local_planner/package.xml new file mode 100644 index 0000000..3d72d1f --- /dev/null +++ b/mpc_local_planner/package.xml @@ -0,0 +1,56 @@ + + + mpc_local_planner + 0.0.0 + The mpc_local_planner package implements a plugin + to the base_local_planner of the 2D navigation stack. + It provides a generic and versatile model predictive control implementation + with minimum-time and quadratic-form receding-horizon configurations. + + + Christoph Rösmann + Christoph Rösmann + + GPLv3 + + http://wiki.ros.org/mpc_local_planner + + + catkin + + roscpp + tf2_eigen + tf2_geometry_msgs + + roscpp + + roscpp + + control_box_rst + + base_local_planner + costmap_2d + costmap_converter + dynamic_reconfigure + eigen + geometry_msgs + interactive_markers + nav_core + nav_msgs + mbf_costmap_core + mbf_msgs + mpc_local_planner_msgs + pluginlib + std_msgs + teb_local_planner + tf2 + tf2_ros + visualization_msgs + + + + + + + + diff --git a/mpc_local_planner/scripts/plot_optimal_control_results.py b/mpc_local_planner/scripts/plot_optimal_control_results.py new file mode 100755 index 0000000..3b55ff2 --- /dev/null +++ b/mpc_local_planner/scripts/plot_optimal_control_results.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python +# +# +# Software License Agreement +# +# Copyright (c) 2020, Christoph Rösmann, All rights reserved. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +# +# Authors: Christoph Rösmann + +import rospy, math +from mpc_local_planner_msgs.msg import OptimalControlResult +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.ticker as ticker +from threading import Lock + +class OcpResultPlotter: + + def __init__(self, plot_states, topic_name): + + self.initialized = False + self.plot_states = plot_states + self.dim_states = 0 + self.dim_controls = 0 + self.x_fig = plt.Figure() + self.x_axes = [] + self.u_fig = plt.Figure() + self.u_axes = [] + self.tx = [] + self.x = [] + self.tu = [] + self.u = [] + self.mutex = Lock() + + self.sub = rospy.Subscriber(topic_name, OptimalControlResult, self.ocp_result_callback, queue_size = 1) + rospy.loginfo("Plotting OCP results published on '%s'.",topic_name) + rospy.loginfo("Make sure to enable rosparam 'controller/publish_ocp_results' in the mpc_local_planner.") + + def ocp_result_callback(self, data): + rospy.loginfo_once("First message received.") + if not self.initialized: + self.dim_states = data.dim_states + self.dim_controls = data.dim_controls + + # Read data + self.mutex.acquire() + if self.plot_states: + self.tx = np.array(data.time_states) + self.x = np.matrix(data.states) + self.x = np.reshape(self.x, (self.tx.size, int(self.dim_states))) + self.tu = np.array(data.time_controls) + self.u = np.matrix(data.controls) + self.u = np.reshape(self.u, (self.tu.size, int(self.dim_controls))) + self.mutex.release() + + def initializePlotWindows(self): + if self.plot_states: + self.x_fig, self.x_axes = plt.subplots(self.dim_states, sharex=True) + self.x_axes[0].set_title('States') + for idx, ax in enumerate(self.x_axes): + ax.set_ylabel("x" + str(idx)) + self.x_axes[-1].set_xlabel('Time [s]') + + self.u_fig, self.u_axes = plt.subplots(self.dim_controls, sharex=True) + self.u_axes[0].set_title('Controls') + for idx, ax in enumerate(self.u_axes): + ax.set_ylabel("u" + str(idx)) + self.u_axes[-1].set_xlabel('Time [s]') + plt.ion() + plt.show() + + + def plot(self): + # We recreate the plot every time, not fast, but okay for now.... + self.mutex.acquire() + if self.plot_states: + for idx, ax in enumerate(self.x_axes): + ax.cla() + ax.grid() + ax.plot(self.tx, self.x[:,idx]) + ax.get_yaxis().set_major_formatter(ticker.FuncFormatter(lambda x, p: "%.2f" % x)) + ax.set_ylabel("x" + str(idx)) + self.x_axes[0].set_title('States') + self.x_axes[-1].set_xlabel('Time [s]') + self.x_fig.canvas.draw() + + for idx, ax in enumerate(self.u_axes): + ax.cla() + ax.grid() + ax.step(self.tu, self.u[:, idx], where='post') + ax.get_yaxis().set_major_formatter(ticker.FuncFormatter(lambda x, p: "%.2f" % x)) + ax.set_ylabel("u" + str(idx)) + self.u_axes[0].set_title('Controls') + self.u_axes[-1].set_xlabel('Time [s]') + self.u_fig.canvas.draw() + self.mutex.release() + + def start(self, rate): + r = rospy.Rate(rate) # define rate here + while not rospy.is_shutdown(): + if not self.initialized and (self.dim_states > 0 or self.dim_controls > 0): + self.initializePlotWindows() + self.initialized = True + if self.initialized: + self.plot() + r.sleep() + + + +if __name__ == '__main__': + try: + + rospy.init_node("ocp_result_plotter", anonymous=True) + + topic_name = "/test_mpc_optim_node/ocp_result" + topic_name = rospy.get_param('~ocp_result_topic', topic_name) + + plot_states = rospy.get_param('~plot_states', False) + + result_plotter = OcpResultPlotter(plot_states, topic_name) + rate = 2 + topic_name = rospy.get_param('~plot_rate', rate) + result_plotter.start(rate) + except rospy.ROSInterruptException: + pass diff --git a/mpc_local_planner/src/controller.cpp b/mpc_local_planner/src/controller.cpp new file mode 100644 index 0000000..796829b --- /dev/null +++ b/mpc_local_planner/src/controller.cpp @@ -0,0 +1,790 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include + +namespace mpc_local_planner { + +bool Controller::configure(ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles, + teb_local_planner::RobotFootprintModelPtr robot_model, const std::vector& via_points) +{ + _dynamics = configureRobotDynamics(nh); + if (!_dynamics) return false; // we may need state and control dimensions to check other parameters + + _grid = configureGrid(nh); + _solver = configureSolver(nh); + + _structured_ocp = configureOcp(nh, obstacles, robot_model, via_points); + _ocp = _structured_ocp; // copy pointer also to parent member + + int outer_ocp_iterations = 1; + nh.param("controller/outer_ocp_iterations", outer_ocp_iterations, outer_ocp_iterations); + setNumOcpIterations(outer_ocp_iterations); + + // further goal opions + nh.param("controller/force_reinit_new_goal_dist", _force_reinit_new_goal_dist, _force_reinit_new_goal_dist); + nh.param("controller/force_reinit_new_goal_angular", _force_reinit_new_goal_angular, _force_reinit_new_goal_angular); + + nh.param("controller/allow_init_with_backward_motion", _guess_backwards_motion, _guess_backwards_motion); + nh.param("controller/force_reinit_num_steps", _force_reinit_num_steps, _force_reinit_num_steps); + + // custom feedback: + nh.param("controller/prefer_x_feedback", _prefer_x_feedback, _prefer_x_feedback); + _x_feedback_sub = nh.subscribe("state_feedback", 1, &Controller::stateFeedbackCallback, this); + + // result publisher: + _ocp_result_pub = nh.advertise("ocp_result", 100); + nh.param("controller/publish_ocp_results", _publish_ocp_results, _publish_ocp_results); + nh.param("controller/print_cpu_time", _print_cpu_time, _print_cpu_time); + + setAutoUpdatePreviousControl(false); // we want to update the previous control value manually + + if (_ocp->initialize()) + ROS_INFO("OCP initialized."); + else + { + ROS_ERROR("OCP initialization failed"); + return false; + } + return _grid && _dynamics && _solver && _structured_ocp; +} + +bool Controller::step(const Controller::PoseSE2& start, const Controller::PoseSE2& goal, const geometry_msgs::Twist& vel, double dt, ros::Time t, + corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq) +{ + std::vector initial_plan(2); + start.toPoseMsg(initial_plan.front().pose); + goal.toPoseMsg(initial_plan.back().pose); + return step(initial_plan, vel, dt, t, u_seq, x_seq); +} + +bool Controller::step(const std::vector& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t, + corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq) +{ + if (!_dynamics || !_grid || !_structured_ocp) + { + ROS_ERROR("Controller must be configured before invoking step()."); + return false; + } + if (initial_plan.size() < 2) + { + ROS_ERROR("Controller::step(): initial plan must contain at least two poses."); + return false; + } + + PoseSE2 start(initial_plan.front().pose); + PoseSE2 goal(initial_plan.back().pose); + + Eigen::VectorXd xf(_dynamics->getStateDimension()); + _dynamics->getSteadyStateFromPoseSE2(goal, xf); + + // retrieve or estimate current state + Eigen::VectorXd x(_dynamics->getStateDimension()); + // check for new measurements + bool new_x = false; + { + std::lock_guard lock(_x_feedback_mutex); + new_x = _recent_x_feedback.size() > 0 && (t - _recent_x_time).toSec() < 2.0 * dt; + if (new_x) x = _recent_x_feedback; + } + if (!new_x && (!_x_ts || _x_ts->isEmpty() || !_x_ts->getValuesInterpolate(dt, x))) // predict with previous state sequence + { + _dynamics->getSteadyStateFromPoseSE2(start, x); // otherwise initialize steady state + } + if (!new_x || !_prefer_x_feedback) + { + // Merge state feedback with odometry feedback if desired. + // Note, some models like unicycle overwrite the full state by odom feedback unless _prefer_x_measurement is set to true. + _dynamics->mergeStateFeedbackAndOdomFeedback(start, vel, x); + } + + // now check goal + if (_force_reinit_num_steps > 0 && _ocp_seq % _force_reinit_num_steps == 0) _grid->clear(); + if (!_grid->isEmpty() && ((goal.position() - _last_goal.position()).norm() > _force_reinit_new_goal_dist || + std::abs(normalize_theta(goal.theta() - _last_goal.theta())) > _force_reinit_new_goal_angular)) + { + // goal pose diverges a lot from the previous one, so force reinit + _grid->clear(); + } + if (_grid->isEmpty()) + { + // generate custom initialization based on initial_plan + // check if the goal is behind the start pose (w.r.t. start orientation) + bool backward = _guess_backwards_motion && (goal.position() - start.position()).dot(start.orientationUnitVec()) < 0; + generateInitialStateTrajectory(x, xf, initial_plan, backward); + } + corbo::Time time(t.toSec()); + _x_seq_init.setTimeFromStart(time); + + corbo::StaticReference xref(xf); // currently, we only support point-to-point transitions in ros + corbo::ZeroReference uref(_dynamics->getInputDimension()); + + _ocp_successful = PredictiveController::step(x, xref, uref, corbo::Duration(dt), time, u_seq, x_seq, nullptr, nullptr, &_x_seq_init); + // publish results if desired + if (_publish_ocp_results) publishOptimalControlResult(); // TODO(roesmann): we could also pass time t from above + ROS_INFO_STREAM_COND(_print_cpu_time, "Cpu time: " << _statistics.step_time.toSec() * 1000.0 << " ms."); + ++_ocp_seq; + _last_goal = goal; + return _ocp_successful; +} + +void Controller::stateFeedbackCallback(const mpc_local_planner_msgs::StateFeedback::ConstPtr& msg) +{ + if (!_dynamics) return; + + if ((int)msg->state.size() != _dynamics->getStateDimension()) + { + ROS_ERROR_STREAM("stateFeedbackCallback(): state feedback dimension does not match robot state dimension: " + << msg->state.size() << " != " << _dynamics->getStateDimension()); + return; + } + + std::lock_guard lock(_x_feedback_mutex); + _recent_x_time = msg->header.stamp; + _recent_x_feedback = Eigen::Map(msg->state.data(), (int)msg->state.size()); +} + +void Controller::publishOptimalControlResult() +{ + if (!_dynamics) return; + mpc_local_planner_msgs::OptimalControlResult msg; + msg.header.stamp = ros::Time::now(); + msg.header.seq = static_cast(_ocp_seq); + msg.dim_states = _dynamics->getStateDimension(); + msg.dim_controls = _dynamics->getInputDimension(); + msg.optimal_solution_found = _ocp_successful; + msg.cpu_time = _statistics.step_time.toSec(); + + if (_x_ts && !_x_ts->isEmpty()) + { + msg.time_states = _x_ts->getTime(); + msg.states = _x_ts->getValues(); + } + + if (_u_ts && !_u_ts->isEmpty()) + { + msg.time_controls = _u_ts->getTime(); + msg.controls = _u_ts->getValues(); + } + + _ocp_result_pub.publish(msg); +} + +void Controller::reset() { PredictiveController::reset(); } + +corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::NodeHandle& nh) +{ + if (!_dynamics) return {}; + + std::string grid_type = "fd_grid"; + nh.param("grid/type", grid_type, grid_type); + + if (grid_type == "fd_grid") + { + FiniteDifferencesGridSE2::Ptr grid; + + bool variable_grid = true; + nh.param("grid/variable_grid/enable", variable_grid, variable_grid); + if (variable_grid) + { + FiniteDifferencesVariableGridSE2::Ptr var_grid = std::make_shared(); + + double min_dt = 0.0; + nh.param("grid/variable_grid/min_dt", min_dt, min_dt); + double max_dt = 10.0; + nh.param("grid/variable_grid/max_dt", max_dt, max_dt); + var_grid->setDtBounds(min_dt, max_dt); + + bool grid_adaptation = true; + nh.param("grid/variable_grid/grid_adaptation/enable", grid_adaptation, grid_adaptation); + + if (grid_adaptation) + { + int max_grid_size = 50; + nh.param("grid/variable_grid/grid_adaptation/max_grid_size", max_grid_size, max_grid_size); + double dt_hyst_ratio = 0.1; + nh.param("grid/variable_grid/grid_adaptation/dt_hyst_ratio", dt_hyst_ratio, dt_hyst_ratio); + var_grid->setGridAdaptTimeBasedSingleStep(max_grid_size, dt_hyst_ratio, true); + + int min_grid_size = 2; + nh.param("grid/variable_grid/grid_adaptation/min_grid_size", min_grid_size, min_grid_size); + var_grid->setNmin(min_grid_size); + } + else + { + var_grid->disableGridAdaptation(); + } + grid = var_grid; + } + else + { + grid = std::make_shared(); + } + // common grid parameters + int grid_size_ref = 20; + nh.param("grid/grid_size_ref", grid_size_ref, grid_size_ref); + grid->setNRef(grid_size_ref); + + double dt_ref = 0.3; + nh.param("grid/dt_ref", dt_ref, dt_ref); + grid->setDtRef(dt_ref); + + std::vector xf_fixed = {true, true, true}; + nh.param("grid/xf_fixed", xf_fixed, xf_fixed); + if ((int)xf_fixed.size() != _dynamics->getStateDimension()) + { + ROS_ERROR_STREAM("Array size of `xf_fixed` does not match robot state dimension(): " << xf_fixed.size() + << " != " << _dynamics->getStateDimension()); + return {}; + } + Eigen::Matrix xf_fixed_eigen(xf_fixed.size()); // we cannot use Eigen::Map as vector does not provide raw access + for (int i = 0; i < (int)xf_fixed.size(); ++i) xf_fixed_eigen[i] = xf_fixed[i]; + grid->setXfFixed(xf_fixed_eigen); + + bool warm_start = true; + nh.param("warm_start", warm_start, warm_start); + grid->setWarmStart(warm_start); + + std::string collocation_method = "forward_differences"; + nh.param("collocation_method", collocation_method, collocation_method); + + if (collocation_method == "forward_differences") + { + grid->setFiniteDifferencesCollocationMethod(std::make_shared()); + } + else if (collocation_method == "midpoint_differences") + { + grid->setFiniteDifferencesCollocationMethod(std::make_shared()); + } + else if (collocation_method == "crank_nicolson_differences") + { + grid->setFiniteDifferencesCollocationMethod(std::make_shared()); + } + else + { + ROS_ERROR_STREAM("Unknown collocation method '" << collocation_method << "' specified. Falling back to default..."); + } + + std::string cost_integration_method = "left_sum"; + nh.param("cost_integration_method", cost_integration_method, cost_integration_method); + + if (cost_integration_method == "left_sum") + { + grid->setCostIntegrationRule(FullDiscretizationGridBaseSE2::CostIntegrationRule::LeftSum); + } + else if (cost_integration_method == "trapezoidal_rule") + { + grid->setCostIntegrationRule(FullDiscretizationGridBaseSE2::CostIntegrationRule::TrapezoidalRule); + } + else + { + ROS_ERROR_STREAM("Unknown cost integration method '" << cost_integration_method << "' specified. Falling back to default..."); + } + + return std::move(grid); + } + else + { + ROS_ERROR_STREAM("Unknown grid type '" << grid_type << "' specified."); + } + + return {}; +} + +RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHandle& nh) +{ + _robot_type = "unicycle"; + nh.param("robot/type", _robot_type, _robot_type); + + if (_robot_type == "unicycle") + { + return std::make_shared(); + } + else if (_robot_type == "simple_car") + { + double wheelbase = 0.5; + nh.param("robot/simple_car/wheelbase", wheelbase, wheelbase); + bool front_wheel_driving = false; + nh.param("robot/simple_car/front_wheel_driving", front_wheel_driving, front_wheel_driving); + if (front_wheel_driving) + return std::make_shared(wheelbase); + else + return std::make_shared(wheelbase); + } + else + { + ROS_ERROR_STREAM("Unknown robot type '" << _robot_type << "' specified."); + } + + return {}; +} + +corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle& nh) +{ + + std::string solver_type = "ipopt"; + nh.param("solver/type", solver_type, solver_type); + + if (solver_type == "ipopt") + { + corbo::SolverIpopt::Ptr solver = std::make_shared(); + solver->initialize(); // requried for setting parameters afterward + + int iterations = 100; + nh.param("solver/ipopt/iterations", iterations, iterations); + solver->setIterations(iterations); + + double max_cpu_time = -1.0; + nh.param("solver/ipopt/max_cpu_time", max_cpu_time, max_cpu_time); + solver->setMaxCpuTime(max_cpu_time); + + // now check for additional ipopt options + std::map numeric_options; + nh.param("solver/ipopt/ipopt_numeric_options", numeric_options, numeric_options); + for (const auto& item : numeric_options) + { + if (!solver->setIpoptOptionNumeric(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set."); + } + + std::map string_options; + nh.param("solver/ipopt/ipopt_string_options", string_options, string_options); + for (const auto& item : string_options) + { + if (!solver->setIpoptOptionString(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set."); + } + + std::map integer_options; + nh.param("solver/ipopt/ipopt_integer_options", integer_options, integer_options); + for (const auto& item : integer_options) + { + if (!solver->setIpoptOptionInt(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set."); + } + + return std::move(solver); + } + // else if (solver_type == "sqp") + // { + // corbo::SolverSQP::Ptr solver = std::make_shared(); + // solver->setUseObjectiveHessian(true); + // // solver->setQpZeroWarmstart(false); + // solver->setVerbose(true); + // corbo::SolverOsqp::Ptr qp_solver = std::make_shared(); + // qp_solver->getOsqpSettings()->verbose = 1; + // solver->setQpSolver(qp_solver); + // corbo::LineSearchL1::Ptr ls = std::make_shared(); + // ls->setVerbose(true); + // ls->setEta(1e-6); + // solver->setLineSearchAlgorithm(ls); + + // return std::move(solver); + // } + else if (solver_type == "lsq_lm") + { + corbo::LevenbergMarquardtSparse::Ptr solver = std::make_shared(); + + int iterations = 10; + nh.param("solver/lsq_lm/iterations", iterations, iterations); + solver->setIterations(iterations); + + double weight_init_eq = 2; + nh.param("solver/lsq_lm/weight_init_eq", weight_init_eq, weight_init_eq); + double weight_init_ineq = 2; + nh.param("solver/lsq_lm/weight_init_ineq", weight_init_ineq, weight_init_ineq); + double weight_init_bounds = 2; + nh.param("solver/lsq_lm/weight_init_bounds", weight_init_bounds, weight_init_bounds); + + solver->setPenaltyWeights(weight_init_eq, weight_init_ineq, weight_init_bounds); + + double weight_adapt_factor_eq = 1; + nh.param("solver/lsq_lm/weight_adapt_factor_eq", weight_adapt_factor_eq, weight_adapt_factor_eq); + double weight_adapt_factor_ineq = 1; + nh.param("solver/lsq_lm/weight_adapt_factor_ineq", weight_adapt_factor_ineq, weight_adapt_factor_ineq); + double weight_adapt_factor_bounds = 1; + nh.param("solver/lsq_lm/weight_adapt_factor_bounds", weight_adapt_factor_bounds, weight_adapt_factor_bounds); + + double weight_adapt_max_eq = 500; + nh.param("solver/lsq_lm/weight_adapt_max_eq", weight_adapt_max_eq, weight_adapt_max_eq); + double weight_adapt_max_ineq = 500; + nh.param("solver/lsq_lm/weight_init_eq", weight_adapt_max_ineq, weight_adapt_max_ineq); + double weight_adapt_max_bounds = 500; + nh.param("solver/lsq_lm/weight_adapt_max_bounds", weight_adapt_max_bounds, weight_adapt_max_bounds); + + solver->setWeightAdapation(weight_adapt_factor_eq, weight_adapt_factor_ineq, weight_adapt_factor_bounds, weight_adapt_max_eq, + weight_adapt_max_ineq, weight_adapt_max_bounds); + + return std::move(solver); + } + else + { + ROS_ERROR_STREAM("Unknown solver type '" << solver_type << "' specified."); + } + + return {}; +} + +corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles, + teb_local_planner::RobotFootprintModelPtr robot_model, + const std::vector& via_points) +{ + corbo::BaseHyperGraphOptimizationProblem::Ptr hg = std::make_shared(); + + corbo::StructuredOptimalControlProblem::Ptr ocp = std::make_shared(_grid, _dynamics, hg, _solver); + + const int x_dim = _dynamics->getStateDimension(); + const int u_dim = _dynamics->getInputDimension(); + + if (_robot_type == "unicycle") + { + double max_vel_x = 0.4; + nh.param("robot/unicycle/max_vel_x", max_vel_x, max_vel_x); + double max_vel_x_backwards = 0.2; + nh.param("robot/unicycle/max_vel_x_backwards", max_vel_x_backwards, max_vel_x_backwards); + if (max_vel_x_backwards < 0) + { + ROS_WARN("max_vel_x_backwards must be >= 0"); + max_vel_x_backwards *= -1; + } + double max_vel_theta = 0.3; + nh.param("robot/unicycle/max_vel_theta", max_vel_theta, max_vel_theta); + + // ocp->setBounds(Eigen::Vector3d(-corbo::CORBO_INF_DBL, -corbo::CORBO_INF_DBL, -corbo::CORBO_INF_DBL), + // Eigen::Vector3d(corbo::CORBO_INF_DBL, corbo::CORBO_INF_DBL, corbo::CORBO_INF_DBL), + // Eigen::Vector2d(-max_vel_x_backwards, -max_vel_theta), Eigen::Vector2d(max_vel_x, max_vel_theta)); + ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_vel_theta), Eigen::Vector2d(max_vel_x, max_vel_theta)); + } + else if (_robot_type == "simple_car") + { + double max_vel_x = 0.4; + nh.param("robot/simple_car/max_vel_x", max_vel_x, max_vel_x); + double max_vel_x_backwards = 0.2; + nh.param("robot/simple_car/max_vel_x_backwards", max_vel_x_backwards, max_vel_x_backwards); + if (max_vel_x_backwards < 0) + { + ROS_WARN("max_vel_x_backwards must be >= 0"); + max_vel_x_backwards *= -1; + } + double max_steering_angle = 1.5; + nh.param("robot/simple_car/max_steering_angle", max_steering_angle, max_steering_angle); + + ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_steering_angle), Eigen::Vector2d(max_vel_x, max_steering_angle)); + } + else + { + ROS_ERROR_STREAM("Cannot configure OCP for unknown robot type " << _robot_type << "."); + return {}; + } + + std::string objective_type = "minimum_time"; + nh.param("planning/objective/type", objective_type, objective_type); + bool lsq_solver = _solver->isLsqSolver(); + + if (objective_type == "minimum_time") + { + ocp->setStageCost(std::make_shared(lsq_solver)); + } + else if (objective_type == "quadratic_form") + { + std::vector state_weights; + nh.param("planning/objective/quadratic_form/state_weights", state_weights, state_weights); + Eigen::MatrixXd Q; + if (state_weights.size() == x_dim) + { + Q = Eigen::Map(state_weights.data(), x_dim).asDiagonal(); + } + else if (state_weights.size() == x_dim * x_dim) + { + Q = Eigen::Map(state_weights.data(), x_dim, x_dim); // Eigens default is column major + } + else + { + ROS_ERROR_STREAM("State weights dimension invalid. Must be either " << x_dim << " x 1 or " << x_dim << " x " << x_dim << "."); + return {}; + } + std::vector control_weights; + nh.param("planning/objective/quadratic_form/control_weights", control_weights, control_weights); + Eigen::MatrixXd R; + if (control_weights.size() == u_dim) + { + R = Eigen::Map(control_weights.data(), u_dim).asDiagonal(); + } + else if (control_weights.size() == u_dim * u_dim) + { + R = Eigen::Map(control_weights.data(), u_dim, u_dim); // Eigens default is column major + } + else + { + ROS_ERROR_STREAM("Control weights dimension invalid. Must be either " << u_dim << " x 1 or " << u_dim << " x " << u_dim << "."); + return {}; + } + bool integral_form = false; + nh.param("planning/objective/quadratic_form/integral_form", integral_form, integral_form); + bool q_zero = Q.isZero(); + bool r_zero = R.isZero(); + if (!q_zero && !r_zero) + ocp->setStageCost(std::make_shared(Q, R, integral_form, lsq_solver)); + else if (!q_zero && r_zero) + ocp->setStageCost(std::make_shared(Q, integral_form, lsq_solver)); + else if (q_zero && !r_zero) + ocp->setStageCost(std::make_shared(R, integral_form, lsq_solver)); + } + else if (objective_type == "minimum_time_via_points") + { + bool via_points_ordered = false; + nh.param("planning/objective/minimum_time_via_points/via_points_ordered", via_points_ordered, via_points_ordered); + double position_weight = 1.0; + nh.param("planning/objective/minimum_time_via_points/position_weight", position_weight, position_weight); + double orientation_weight = 0.0; + nh.param("planning/objective/minimum_time_via_points/orientation_weight", orientation_weight, orientation_weight); + ocp->setStageCost(std::make_shared(via_points, position_weight, orientation_weight, via_points_ordered)); + // TODO(roesmann): lsq version + } + else + { + ROS_ERROR_STREAM("Unknown objective type '" << objective_type << "' specified ('planning/objective/type')."); + return {}; + } + + std::string terminal_cost = "none"; + nh.param("planning/terminal_cost/type", terminal_cost, terminal_cost); + + if (terminal_cost == "none") + { + ocp->setFinalStageCost({}); + } + else if (terminal_cost == "quadratic") + { + std::vector state_weights; + nh.param("planning/terminal_cost/quadratic/final_state_weights", state_weights, state_weights); + Eigen::MatrixXd Qf; + if (state_weights.size() == x_dim) + { + Qf = Eigen::Map(state_weights.data(), x_dim).asDiagonal(); + } + else if (state_weights.size() == x_dim * x_dim) + { + Qf = Eigen::Map(state_weights.data(), x_dim, x_dim); // Eigens default is column major + } + else + { + ROS_ERROR_STREAM("Final state weights dimension invalid. Must be either " << x_dim << " x 1 or " << x_dim << " x " << x_dim << "."); + return {}; + } + ocp->setFinalStageCost(std::make_shared(Qf, lsq_solver)); + } + else + { + ROS_ERROR_STREAM("Unknown terminal_cost type '" << terminal_cost << "' specified ('planning/terminal_cost/type')."); + return {}; + } + + std::string terminal_constraint = "none"; + nh.param("planning/terminal_constraint/type", terminal_constraint, terminal_constraint); + + if (terminal_constraint == "none") + { + ocp->setFinalStageConstraint({}); + } + else if (terminal_constraint == "l2_ball") + { + std::vector weight_matrix; + nh.param("planning/terminal_constraint/l2_ball/weight_matrix", weight_matrix, weight_matrix); + Eigen::MatrixXd S; + if (weight_matrix.size() == x_dim) + { + S = Eigen::Map(weight_matrix.data(), x_dim).asDiagonal(); + } + else if (weight_matrix.size() == x_dim * x_dim) + { + S = Eigen::Map(weight_matrix.data(), x_dim, x_dim); // Eigens default is column major + } + else + { + ROS_ERROR_STREAM("l2-ball weight_matrix dimensions invalid. Must be either " << x_dim << " x 1 or " << x_dim << " x " << x_dim << "."); + return {}; + } + double radius = 1.0; + nh.param("planning/terminal_constraint/l2_ball/radius", radius, radius); + ocp->setFinalStageConstraint(std::make_shared(S, radius)); + } + else + { + ROS_ERROR_STREAM("Unknown terminal_constraint type '" << terminal_constraint << "' specified ('planning/terminal_constraint/type')."); + return {}; + } + + _inequality_constraint = std::make_shared(); + _inequality_constraint->setObstacleVector(obstacles); + _inequality_constraint->setRobotFootprintModel(robot_model); + + // configure collision avoidance + + double min_obstacle_dist = 0.5; + nh.param("collision_avoidance/min_obstacle_dist", min_obstacle_dist, min_obstacle_dist); + _inequality_constraint->setMinimumDistance(min_obstacle_dist); + + bool enable_dynamic_obstacles = false; + nh.param("collision_avoidance/enable_dynamic_obstacles", enable_dynamic_obstacles, enable_dynamic_obstacles); + _inequality_constraint->setEnableDynamicObstacles(enable_dynamic_obstacles); + + double force_inclusion_factor = 1.5; + nh.param("collision_avoidance/force_inclusion_factor", force_inclusion_factor, force_inclusion_factor); + double cutoff_factor = 5; + nh.param("collision_avoidance/cutoff_factor", cutoff_factor, cutoff_factor); + _inequality_constraint->setObstacleFilterParameters(force_inclusion_factor, cutoff_factor); + + // configure control deviation bounds + + if (_robot_type == "unicycle") + { + double acc_lim_x = 0.0; + nh.param("robot/unicycle/acc_lim_x", acc_lim_x, acc_lim_x); + double dec_lim_x = 0.0; + nh.param("robot/unicycle/dec_lim_x", dec_lim_x, dec_lim_x); + if (dec_lim_x < 0) + { + ROS_WARN("dec_lim_x must be >= 0"); + dec_lim_x *= -1; + } + double acc_lim_theta = 0.0; + nh.param("robot/unicycle/acc_lim_theta", acc_lim_theta, acc_lim_theta); + + if (acc_lim_x <= 0) acc_lim_x = corbo::CORBO_INF_DBL; + if (dec_lim_x <= 0) dec_lim_x = corbo::CORBO_INF_DBL; + if (acc_lim_theta <= 0) acc_lim_theta = corbo::CORBO_INF_DBL; + Eigen::Vector2d ud_lb(-dec_lim_x, -acc_lim_theta); + Eigen::Vector2d ud_ub(acc_lim_x, acc_lim_theta); + _inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub); + } + else if (_robot_type == "simple_car") + { + double acc_lim_x = 0.0; + nh.param("robot/simple_car/acc_lim_x", acc_lim_x, acc_lim_x); + double dec_lim_x = 0.0; + nh.param("robot/simple_car/dec_lim_x", dec_lim_x, dec_lim_x); + if (dec_lim_x < 0) + { + ROS_WARN("dec_lim_x must be >= 0"); + dec_lim_x *= -1; + } + double max_steering_rate = 0.0; + nh.param("robot/simple_car/max_steering_rate", max_steering_rate, max_steering_rate); + + if (acc_lim_x <= 0) acc_lim_x = corbo::CORBO_INF_DBL; + if (dec_lim_x <= 0) dec_lim_x = corbo::CORBO_INF_DBL; + if (max_steering_rate <= 0) max_steering_rate = corbo::CORBO_INF_DBL; + Eigen::Vector2d ud_lb(-dec_lim_x, -max_steering_rate); + Eigen::Vector2d ud_ub(acc_lim_x, max_steering_rate); + _inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub); + } + else + { + ROS_ERROR_STREAM("Cannot configure control deviation bounds for unknown robot type " << _robot_type << "."); + return {}; + } + + ocp->setStageInequalityConstraint(_inequality_constraint); + + return ocp; +} + +bool Controller::generateInitialStateTrajectory(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, + const std::vector& initial_plan, bool backward) +{ + if (initial_plan.size() < 2 || !_dynamics) return false; + + TimeSeriesSE2::Ptr ts = std::make_shared(); + + int n_init = (int)initial_plan.size(); + int n_ref = _grid->getInitialN(); + if (n_ref < 2) + { + ROS_ERROR("Controller::generateInitialStateTrajectory(): grid not properly initialized"); + return false; + } + ts->add(0.0, x0); + + double dt_ref = _grid->getInitialDt(); + double tf_ref = (double)(n_ref - 1) * dt_ref; + + Eigen::VectorXd x(_dynamics->getStateDimension()); + + // we initialize by assuming equally distributed poses + double dt_init = tf_ref / double(n_init - 1); + + double t = dt_init; + for (int i = 1; i < n_init - 1; ++i) + { + // get yaw from the orientation of the distance vector between pose_{i+1} and pose_{i} + double yaw; + if (_initial_plan_estimate_orientation) + { + double dx = initial_plan[i + 1].pose.position.x - initial_plan[i].pose.position.x; + double dy = initial_plan[i + 1].pose.position.y - initial_plan[i].pose.position.y; + yaw = std::atan2(dy, dx); + if (backward) normalize_theta(yaw + M_PI); + } + else + { + yaw = tf2::getYaw(initial_plan[i].pose.orientation); + } + PoseSE2 intermediate_pose(initial_plan[i].pose.position.x, initial_plan[i].pose.position.y, yaw); + _dynamics->getSteadyStateFromPoseSE2(intermediate_pose, x); + ts->add(t, x); + t += dt_init; + } + + ts->add(tf_ref, xf); + + _x_seq_init.setTrajectory(ts, corbo::TimeSeries::Interpolation::Linear); + return true; +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp new file mode 100644 index 0000000..782a19d --- /dev/null +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -0,0 +1,1031 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include +#include + +#include + +// MBF return codes +#include + +// pluginlib macros +#include + +// register this planner both as a BaseLocalPlanner and as a MBF's CostmapController plugin +PLUGINLIB_EXPORT_CLASS(mpc_local_planner::MpcLocalPlannerROS, nav_core::BaseLocalPlanner) +PLUGINLIB_EXPORT_CLASS(mpc_local_planner::MpcLocalPlannerROS, mbf_costmap_core::CostmapController) + +namespace mpc_local_planner { + +MpcLocalPlannerROS::MpcLocalPlannerROS() + : _costmap_ros(nullptr), + _tf(nullptr), + _costmap_model(nullptr), + _costmap_converter_loader("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), + /*dynamic_recfg_(NULL),*/ + _goal_reached(false), + _no_infeasible_plans(0), + /*last_preferred_rotdir_(RotType::none),*/ + _initialized(false) +{ +} + +MpcLocalPlannerROS::~MpcLocalPlannerROS() {} + +/* +void MpcLocalPlannerROS::reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level) +{ + cfg_.reconfigure(config); +} +*/ + +void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) +{ + // check if the plugin is already initialized + if (!_initialized) + { + // create Node Handle with name of plugin (as used in move_base for loading) + ros::NodeHandle nh("~/" + name); + + // load plugin related main parameters + nh.param("controller/xy_goal_tolerance", _params.xy_goal_tolerance, _params.xy_goal_tolerance); + nh.param("controller/yaw_goal_tolerance", _params.yaw_goal_tolerance, _params.yaw_goal_tolerance); + nh.param("controller/global_plan_overwrite_orientation", _params.global_plan_overwrite_orientation, + _params.global_plan_overwrite_orientation); + nh.param("controller/global_plan_prune_distance", _params.global_plan_prune_distance, _params.global_plan_prune_distance); + nh.param("controller/max_global_plan_lookahead_dist", _params.max_global_plan_lookahead_dist, _params.max_global_plan_lookahead_dist); + nh.param("controller/global_plan_viapoint_sep", _params.global_plan_viapoint_sep, _params.global_plan_viapoint_sep); + _controller.setInitialPlanEstimateOrientation(_params.global_plan_overwrite_orientation); + + // special parameters + nh.param("odom_topic", _params.odom_topic, _params.odom_topic); + nh.param("footprint_model/is_footprint_dynamic", _params.is_footprint_dynamic, _params.is_footprint_dynamic); + nh.param("collision_avoidance/include_costmap_obstacles", _params.include_costmap_obstacles, _params.include_costmap_obstacles); + nh.param("collision_avoidance/costmap_obstacles_behind_robot_dist", _params.costmap_obstacles_behind_robot_dist, + _params.costmap_obstacles_behind_robot_dist); + + // costmap converter plugin related parameters + nh.param("costmap_converter_plugin", _costmap_conv_params.costmap_converter_plugin, _costmap_conv_params.costmap_converter_plugin); + nh.param("costmap_converter_rate", _costmap_conv_params.costmap_converter_rate, _costmap_conv_params.costmap_converter_rate); + nh.param("costmap_converter_spin_thread", _costmap_conv_params.costmap_converter_spin_thread, + _costmap_conv_params.costmap_converter_spin_thread); + + // reserve some memory for obstacles + _obstacles.reserve(700); + + // create robot footprint/contour model for optimization + _robot_model = getRobotFootprintFromParamServer(nh); + + // create the planner instance + if (!_controller.configure(nh, _obstacles, _robot_model, _via_points)) + { + ROS_ERROR("Controller configuration failed."); + return; + } + + // init other variables + _tf = tf; + _costmap_ros = costmap_ros; + _costmap = _costmap_ros->getCostmap(); // locking should be done in MoveBase. + + _costmap_model = std::make_shared(*_costmap); + + _global_frame = _costmap_ros->getGlobalFrameID(); + _robot_base_frame = _costmap_ros->getBaseFrameID(); + + // create visualization instance + _publisher.initialize(nh, _controller.getRobotDynamics(), _global_frame); + + // Initialize a costmap to polygon converter + if (!_costmap_conv_params.costmap_converter_plugin.empty()) + { + try + { + _costmap_converter = _costmap_converter_loader.createInstance(_costmap_conv_params.costmap_converter_plugin); + std::string converter_name = _costmap_converter_loader.getName(_costmap_conv_params.costmap_converter_plugin); + // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace + boost::replace_all(converter_name, "::", "/"); + _costmap_converter->setOdomTopic(_params.odom_topic); + _costmap_converter->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); + _costmap_converter->setCostmap2D(_costmap); + + _costmap_converter->startWorker(ros::Rate(_costmap_conv_params.costmap_converter_rate), _costmap, + _costmap_conv_params.costmap_converter_spin_thread); + ROS_INFO_STREAM("Costmap conversion plugin " << _costmap_conv_params.costmap_converter_plugin << " loaded."); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_WARN( + "The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error " + "message: %s", + ex.what()); + _costmap_converter.reset(); + } + } + else + ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); + + // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. + _footprint_spec = _costmap_ros->getRobotFootprint(); + costmap_2d::calculateMinAndMaxDistances(_footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius); + + // init the odom helper to receive the robot's velocity from odom messages + _odom_helper.setOdomTopic(_params.odom_topic); + + // setup dynamic reconfigure + /* + dynamic_recfg_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&MpcLocalPlanner::reconfigureCB, this, _1, _2); + dynamic_recfg_->setCallback(cb); + */ + + // validate optimization footprint and costmap footprint + validateFootprints(_robot_model->getInscribedRadius(), _robot_inscribed_radius, _controller.getInequalityConstraint()->getMinimumDistance()); + + // setup callback for custom obstacles + _custom_obst_sub = nh.subscribe("obstacles", 1, &MpcLocalPlannerROS::customObstacleCB, this); + + // setup callback for custom via-points + _via_points_sub = nh.subscribe("via_points", 1, &MpcLocalPlannerROS::customViaPointsCB, this); + + // additional move base params + ros::NodeHandle nh_move_base("~"); + nh_move_base.param("controller_frequency", _params.controller_frequency, _params.controller_frequency); + + // set initialized flag + _initialized = true; + + ROS_DEBUG("mpc_local_planner plugin initialized."); + } + else + { + ROS_WARN("mpc_local_planner has already been initialized, doing nothing."); + } +} + +bool MpcLocalPlannerROS::setPlan(const std::vector& orig_global_plan) +{ + // check if plugin is initialized + if (!_initialized) + { + ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // store the global plan + _global_plan.clear(); + _global_plan = orig_global_plan; + + // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. + // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. + + // reset goal_reached_ flag + _goal_reached = false; + + return true; +} + +bool MpcLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) +{ + std::string dummy_message; + geometry_msgs::PoseStamped dummy_pose; + geometry_msgs::TwistStamped dummy_velocity, cmd_vel_stamped; + uint32_t outcome = computeVelocityCommands(dummy_pose, dummy_velocity, cmd_vel_stamped, dummy_message); + cmd_vel = cmd_vel_stamped.twist; + return outcome == mbf_msgs::ExePathResult::SUCCESS; +} + +uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, + geometry_msgs::TwistStamped& cmd_vel, std::string& message) +{ + // check if plugin initialized + if (!_initialized) + { + ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner"); + message = "mpc_local_planner has not been initialized"; + return mbf_msgs::ExePathResult::NOT_INITIALIZED; + } + + static uint32_t seq = 0; + cmd_vel.header.seq = seq++; + cmd_vel.header.stamp = ros::Time::now(); + cmd_vel.header.frame_id = _robot_base_frame; + cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; + _goal_reached = false; + + // Get robot pose + geometry_msgs::PoseStamped robot_pose; + _costmap_ros->getRobotPose(robot_pose); + _robot_pose = PoseSE2(robot_pose.pose); + + // Get robot velocity + geometry_msgs::PoseStamped robot_vel_tf; + _odom_helper.getRobotVel(robot_vel_tf); + _robot_vel.linear.x = robot_vel_tf.pose.position.x; + _robot_vel.linear.y = robot_vel_tf.pose.position.y; + _robot_vel.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation); + + // prune global plan to cut off parts of the past (spatially before the robot) + pruneGlobalPlan(*_tf, robot_pose, _global_plan, _params.global_plan_prune_distance); + + // Transform global plan to the frame of interest (w.r.t. the local costmap) + std::vector transformed_plan; + int goal_idx; + geometry_msgs::TransformStamped tf_plan_to_global; + if (!transformGlobalPlan(*_tf, _global_plan, robot_pose, *_costmap, _global_frame, _params.max_global_plan_lookahead_dist, transformed_plan, + &goal_idx, &tf_plan_to_global)) + { + ROS_WARN("Could not transform the global plan to the frame of the controller"); + message = "Could not transform the global plan to the frame of the controller"; + return mbf_msgs::ExePathResult::INTERNAL_ERROR; + } + + // update via-points container + if (!_custom_via_points_active) updateViaPointsContainer(transformed_plan, _params.global_plan_viapoint_sep); + + // check if global goal is reached + geometry_msgs::PoseStamped global_goal; + tf2::doTransform(_global_plan.back(), global_goal, tf_plan_to_global); + double dx = global_goal.pose.position.x - _robot_pose.x(); + double dy = global_goal.pose.position.y - _robot_pose.y(); + double delta_orient = g2o::normalize_theta(tf2::getYaw(global_goal.pose.orientation) - _robot_pose.theta()); + if (std::abs(std::sqrt(dx * dx + dy * dy)) < _params.xy_goal_tolerance && std::abs(delta_orient) < _params.yaw_goal_tolerance) + { + _goal_reached = true; + return mbf_msgs::ExePathResult::SUCCESS; + } + + // Return false if the transformed global plan is empty + if (transformed_plan.empty()) + { + ROS_WARN("Transformed plan is empty. Cannot determine a local plan."); + message = "Transformed plan is empty"; + return mbf_msgs::ExePathResult::INVALID_PATH; + } + + // Get current goal point (last point of the transformed plan) + _robot_goal.x() = transformed_plan.back().pose.position.x; + _robot_goal.y() = transformed_plan.back().pose.position.y; + // Overwrite goal orientation if needed + if (_params.global_plan_overwrite_orientation) + { + _robot_goal.theta() = estimateLocalGoalOrientation(_global_plan, transformed_plan.back(), goal_idx, tf_plan_to_global); + // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization) + tf2::Quaternion q; + q.setRPY(0, 0, _robot_goal.theta()); + tf2::convert(q, transformed_plan.back().pose.orientation); + } + else + { + _robot_goal.theta() = tf2::getYaw(transformed_plan.back().pose.orientation); + } + + // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory) + if (transformed_plan.size() == 1) // plan only contains the goal + { + transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized) + } + transformed_plan.front() = robot_pose; // update start + + // clear currently existing obstacles + _obstacles.clear(); + + // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin + if (_costmap_converter) + updateObstacleContainerWithCostmapConverter(); + else + updateObstacleContainerWithCostmap(); + + // also consider custom obstacles (must be called after other updates, since the container is not cleared) + updateObstacleContainerWithCustomObstacles(); + + // estimate current state vector and previous control + // updateControlFromTwist() + + // Do not allow config changes during the following optimization step + // TODO(roesmann): we need something similar in case we allow dynamic reconfiguration: + // boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); + + // Now perform the actual planning + // bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel); + ros::Time t = ros::Time::now(); + // controller_frequency might not be established in case planning takes longer: + // value dt affects e.g. control deviation bounds (acceleration limits) and we want a goot start value + // let's test how it works with the expected frequency instead of using the actual one + double dt = 1.0 / _params.controller_frequency; + // double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec(); + + // set previous control value for control deviation bounds + if (_u_seq && !_u_seq->isEmpty()) _controller.getOptimalControlProblem()->setPreviousControlInput(_u_seq->getValuesMap(0), dt); + + bool success = false; + + { + std::lock_guard vp_lock(_via_point_mutex); + std::lock_guard obst_lock(_custom_obst_mutex); + success = _controller.step(transformed_plan, _robot_vel, dt, t, _u_seq, _x_seq); + } + + if (!success) + { + _controller.reset(); // force reinitialization for next time + ROS_WARN("mpc_local_planner was not able to obtain a local plan for the current setting."); + + ++_no_infeasible_plans; // increase number of infeasible solutions in a row + _time_last_infeasible_plan = ros::Time::now(); + _last_cmd = cmd_vel.twist; + message = "mpc_local_planner was not able to obtain a local plan"; + return mbf_msgs::ExePathResult::NO_VALID_CMD; + } + + // Check feasibility (but within the first few states only) + if (_params.is_footprint_dynamic) + { + // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. + _footprint_spec = _costmap_ros->getRobotFootprint(); + costmap_2d::calculateMinAndMaxDistances(_footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius); + } + + /* + // check feasibility w.r.t. the original costmap footprint + bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, + cfg_.trajectory.feasibility_check_no_poses); + if (!feasible) + { + cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; + + // now we reset everything to start again with the initialization of new trajectories. + planner_->clearPlanner(); + ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner..."); + + ++no_infeasible_plans_; // increase number of infeasible solutions in a row + time_last_infeasible_plan_ = ros::Time::now(); + last_cmd_ = cmd_vel.twist; + message = "teb_local_planner trajectory is not feasible"; + return mbf_msgs::ExePathResult::NO_VALID_CMD; + } +*/ + // Get the velocity command for this sampling interval + // TODO(roesmann): we might also command more than just the imminent action, e.g. in a separate thread, until a new command is available + if (!_u_seq || !_controller.getRobotDynamics()->getTwistFromControl(_u_seq->getValuesMap(0), cmd_vel.twist)) + { + _controller.reset(); + ROS_WARN("MpcLocalPlannerROS: velocity command invalid. Resetting controller..."); + ++_no_infeasible_plans; // increase number of infeasible solutions in a row + _time_last_infeasible_plan = ros::Time::now(); + _last_cmd = cmd_vel.twist; + message = "mpc_local_planner velocity command invalid"; + return mbf_msgs::ExePathResult::NO_VALID_CMD; + } + + // Saturate velocity, if the optimization results violates the constraints (could be possible due to early termination or soft cosntraints). + // saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, + // cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards); + + // a feasible solution should be found, reset counter + _no_infeasible_plans = 0; + + // store last command (for recovery analysis etc.) + _last_cmd = cmd_vel.twist; + _time_last_cmd = ros::Time::now(); + + // Now visualize everything + _publisher.publishLocalPlan(*_x_seq); + _publisher.publishObstacles(_obstacles); + _publisher.publishGlobalPlan(_global_plan); + _publisher.publishViaPoints(_via_points); + _publisher.publishRobotFootprintModel(_robot_pose, *_robot_model); + return mbf_msgs::ExePathResult::SUCCESS; +} + +bool MpcLocalPlannerROS::isGoalReached() +{ + if (_goal_reached) + { + ROS_INFO("GOAL Reached!"); + // planner_->clearPlanner(); + return true; + } + return false; +} + +void MpcLocalPlannerROS::updateObstacleContainerWithCostmap() +{ + // Add costmap obstacles if desired + if (_params.include_costmap_obstacles) + { + Eigen::Vector2d robot_orient = _robot_pose.orientationUnitVec(); + + for (unsigned int i = 0; i < _costmap->getSizeInCellsX() - 1; ++i) + { + for (unsigned int j = 0; j < _costmap->getSizeInCellsY() - 1; ++j) + { + if (_costmap->getCost(i, j) == costmap_2d::LETHAL_OBSTACLE) + { + Eigen::Vector2d obs; + _costmap->mapToWorld(i, j, obs.coeffRef(0), obs.coeffRef(1)); + + // check if obstacle is interesting (e.g. not far behind the robot) + Eigen::Vector2d obs_dir = obs - _robot_pose.position(); + if (obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > _params.costmap_obstacles_behind_robot_dist) continue; + + _obstacles.push_back(ObstaclePtr(new PointObstacle(obs))); + } + } + } + } +} + +void MpcLocalPlannerROS::updateObstacleContainerWithCostmapConverter() +{ + if (!_costmap_converter) return; + + // Get obstacles from costmap converter + costmap_converter::ObstacleArrayConstPtr obstacles = _costmap_converter->getObstacles(); + if (!obstacles) return; + + for (std::size_t i = 0; i < obstacles->obstacles.size(); ++i) + { + const costmap_converter::ObstacleMsg* obstacle = &obstacles->obstacles.at(i); + const geometry_msgs::Polygon* polygon = &obstacle->polygon; + + if (polygon->points.size() == 1 && obstacle->radius > 0) // Circle + { + _obstacles.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius))); + } + else if (polygon->points.size() == 1) // Point + { + _obstacles.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y))); + } + else if (polygon->points.size() == 2) // Line + { + _obstacles.push_back( + ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y, polygon->points[1].x, polygon->points[1].y))); + } + else if (polygon->points.size() > 2) // Real polygon + { + PolygonObstacle* polyobst = new PolygonObstacle; + for (std::size_t j = 0; j < polygon->points.size(); ++j) + { + polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y); + } + polyobst->finalizePolygon(); + _obstacles.push_back(ObstaclePtr(polyobst)); + } + + // Set velocity, if obstacle is moving + if (!_obstacles.empty()) _obstacles.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation); + } +} + +void MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles() +{ + // Add custom obstacles obtained via message + std::lock_guard l(_custom_obst_mutex); + + if (!_custom_obstacle_msg.obstacles.empty()) + { + // We only use the global header to specify the obstacle coordinate system instead of individual ones + Eigen::Affine3d obstacle_to_map_eig; + try + { + geometry_msgs::TransformStamped obstacle_to_map = + _tf->lookupTransform(_global_frame, ros::Time(0), _custom_obstacle_msg.header.frame_id, ros::Time(0), + _custom_obstacle_msg.header.frame_id, ros::Duration(0.5)); + obstacle_to_map_eig = tf2::transformToEigen(obstacle_to_map); + } + catch (tf::TransformException ex) + { + ROS_ERROR("%s", ex.what()); + obstacle_to_map_eig.setIdentity(); + } + + for (size_t i = 0; i < _custom_obstacle_msg.obstacles.size(); ++i) + { + if (_custom_obstacle_msg.obstacles.at(i).polygon.points.size() == 1 && _custom_obstacle_msg.obstacles.at(i).radius > 0) // circle + { + Eigen::Vector3d pos(_custom_obstacle_msg.obstacles.at(i).polygon.points.front().x, + _custom_obstacle_msg.obstacles.at(i).polygon.points.front().y, + _custom_obstacle_msg.obstacles.at(i).polygon.points.front().z); + _obstacles.push_back( + ObstaclePtr(new CircularObstacle((obstacle_to_map_eig * pos).head(2), _custom_obstacle_msg.obstacles.at(i).radius))); + } + else if (_custom_obstacle_msg.obstacles.at(i).polygon.points.size() == 1) // point + { + Eigen::Vector3d pos(_custom_obstacle_msg.obstacles.at(i).polygon.points.front().x, + _custom_obstacle_msg.obstacles.at(i).polygon.points.front().y, + _custom_obstacle_msg.obstacles.at(i).polygon.points.front().z); + _obstacles.push_back(ObstaclePtr(new PointObstacle((obstacle_to_map_eig * pos).head(2)))); + } + else if (_custom_obstacle_msg.obstacles.at(i).polygon.points.size() == 2) // line + { + Eigen::Vector3d line_start(_custom_obstacle_msg.obstacles.at(i).polygon.points.front().x, + _custom_obstacle_msg.obstacles.at(i).polygon.points.front().y, + _custom_obstacle_msg.obstacles.at(i).polygon.points.front().z); + Eigen::Vector3d line_end(_custom_obstacle_msg.obstacles.at(i).polygon.points.back().x, + _custom_obstacle_msg.obstacles.at(i).polygon.points.back().y, + _custom_obstacle_msg.obstacles.at(i).polygon.points.back().z); + _obstacles.push_back( + ObstaclePtr(new LineObstacle((obstacle_to_map_eig * line_start).head(2), (obstacle_to_map_eig * line_end).head(2)))); + } + else if (_custom_obstacle_msg.obstacles.at(i).polygon.points.empty()) + { + ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping..."); + continue; + } + else // polygon + { + PolygonObstacle* polyobst = new PolygonObstacle; + for (size_t j = 0; j < _custom_obstacle_msg.obstacles.at(i).polygon.points.size(); ++j) + { + Eigen::Vector3d pos(_custom_obstacle_msg.obstacles.at(i).polygon.points[j].x, + _custom_obstacle_msg.obstacles.at(i).polygon.points[j].y, + _custom_obstacle_msg.obstacles.at(i).polygon.points[j].z); + polyobst->pushBackVertex((obstacle_to_map_eig * pos).head(2)); + } + polyobst->finalizePolygon(); + _obstacles.push_back(ObstaclePtr(polyobst)); + } + + // Set velocity, if obstacle is moving + if (!_obstacles.empty()) + _obstacles.back()->setCentroidVelocity(_custom_obstacle_msg.obstacles[i].velocities, _custom_obstacle_msg.obstacles[i].orientation); + } + } +} + +void MpcLocalPlannerROS::updateViaPointsContainer(const std::vector& transformed_plan, double min_separation) +{ + _via_points.clear(); + + if (min_separation <= 0) return; + + std::size_t prev_idx = 0; + for (std::size_t i = 1; i < transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m] + { + // check separation to the previous via-point inserted + if (distance_points2d(transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position) < min_separation) continue; + + // add via-point + _via_points.emplace_back(transformed_plan[i].pose); + prev_idx = i; + } +} + +Eigen::Vector2d MpcLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel) +{ + Eigen::Vector2d vel; + vel.coeffRef(0) = std::sqrt(tf_vel.getOrigin().getX() * tf_vel.getOrigin().getX() + tf_vel.getOrigin().getY() * tf_vel.getOrigin().getY()); + vel.coeffRef(1) = tf::getYaw(tf_vel.getRotation()); + return vel; +} + +bool MpcLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose, + std::vector& global_plan, double dist_behind_robot) +{ + if (global_plan.empty()) return true; + + try + { + // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times) + geometry_msgs::TransformStamped global_to_plan_transform = + tf.lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, ros::Time(0)); + geometry_msgs::PoseStamped robot; + tf2::doTransform(global_pose, robot, global_to_plan_transform); + + double dist_thresh_sq = dist_behind_robot * dist_behind_robot; + + // iterate plan until a pose close the robot is found + std::vector::iterator it = global_plan.begin(); + std::vector::iterator erase_end = it; + while (it != global_plan.end()) + { + double dx = robot.pose.position.x - it->pose.position.x; + double dy = robot.pose.position.y - it->pose.position.y; + double dist_sq = dx * dx + dy * dy; + if (dist_sq < dist_thresh_sq) + { + erase_end = it; + break; + } + ++it; + } + if (erase_end == global_plan.end()) return false; + + if (erase_end != global_plan.begin()) global_plan.erase(global_plan.begin(), erase_end); + } + catch (const tf::TransformException& ex) + { + ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what()); + return false; + } + return true; +} + +bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector& global_plan, + const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, + const std::string& global_frame, double max_plan_length, + std::vector& transformed_plan, int* current_goal_idx, + geometry_msgs::TransformStamped* tf_plan_to_global) const +{ + // this method is a slightly modified version of base_local_planner/goal_functions.h + + const geometry_msgs::PoseStamped& plan_pose = global_plan[0]; + + transformed_plan.clear(); + + try + { + if (global_plan.empty()) + { + ROS_ERROR("Received plan with zero length"); + *current_goal_idx = 0; + return false; + } + + // get plan_to_global_transform from plan frame to global_frame + geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform( + global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(0.5)); + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::PoseStamped robot_pose; + tf.transform(global_pose, robot_pose, plan_pose.header.frame_id); + + // we'll discard points on the plan that are outside the local costmap + double dist_threshold = + std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0); + dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are + // located on the border of the local costmap + + int i = 0; + double sq_dist_threshold = dist_threshold * dist_threshold; + double sq_dist = 1e10; + + // we need to loop to a point on the plan that is within a certain distance of the robot + for (int j = 0; j < (int)global_plan.size(); ++j) + { + double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x; + double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y; + double new_sq_dist = x_diff * x_diff + y_diff * y_diff; + if (new_sq_dist > sq_dist_threshold) break; // force stop if we have reached the costmap border + + if (new_sq_dist < sq_dist) // find closest distance + { + sq_dist = new_sq_dist; + i = j; + } + } + + geometry_msgs::PoseStamped newer_pose; + + double plan_length = 0; // check cumulative Euclidean distance along the plan + + // now we'll transform until points are outside of our distance threshold + while (i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length <= 0 || plan_length <= max_plan_length)) + { + const geometry_msgs::PoseStamped& pose = global_plan[i]; + tf2::doTransform(pose, newer_pose, plan_to_global_transform); + + transformed_plan.push_back(newer_pose); + + double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x; + double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + // caclulate distance to previous pose + if (i > 0 && max_plan_length > 0) + plan_length += teb_local_planner::distance_points2d(global_plan[i - 1].pose.position, global_plan[i].pose.position); + + ++i; + } + + // if we are really close to the goal (>0) + // the resulting transformed plan can be empty. In that case we explicitly inject the global goal. + if (transformed_plan.empty()) + { + tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform); + + transformed_plan.push_back(newer_pose); + + // Return the index of the current goal point (inside the distance threshold) + if (current_goal_idx) *current_goal_idx = int(global_plan.size()) - 1; + } + else + { + // Return the index of the current goal point (inside the distance threshold) + if (current_goal_idx) *current_goal_idx = i - 1; // subtract 1, since i was increased once before leaving the loop + } + + // Return the transformation from the global plan to the global planning frame if desired + if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform; + } + catch (tf::LookupException& ex) + { + ROS_ERROR("No Transform available Error: %s\n", ex.what()); + return false; + } + catch (tf::ConnectivityException& ex) + { + ROS_ERROR("Connectivity Error: %s\n", ex.what()); + return false; + } + catch (tf::ExtrapolationException& ex) + { + ROS_ERROR("Extrapolation Error: %s\n", ex.what()); + if (global_plan.size() > 0) + ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), + global_plan[0].header.frame_id.c_str()); + + return false; + } + + return true; +} + +double MpcLocalPlannerROS::estimateLocalGoalOrientation(const std::vector& global_plan, + const geometry_msgs::PoseStamped& local_goal, int current_goal_idx, + const geometry_msgs::TransformStamped& tf_plan_to_global, int moving_average_length) const +{ + int n = (int)global_plan.size(); + + // check if we are near the global goal already + if (current_goal_idx > n - moving_average_length - 2) + { + if (current_goal_idx >= n - 1) // we've exactly reached the goal + { + return tf2::getYaw(local_goal.pose.orientation); + } + else + { + tf2::Quaternion global_orientation; + tf2::convert(global_plan.back().pose.orientation, global_orientation); + tf2::Quaternion rotation; + tf2::convert(tf_plan_to_global.transform.rotation, rotation); + // TODO(roesmann): avoid conversion to tf2::Quaternion + return tf2::getYaw(rotation * global_orientation); + } + } + + // reduce number of poses taken into account if the desired number of poses is not available + moving_average_length = + std::min(moving_average_length, n - current_goal_idx - 1); // maybe redundant, since we have checked the vicinity of the goal before + + std::vector candidates; + geometry_msgs::PoseStamped tf_pose_k = local_goal; + geometry_msgs::PoseStamped tf_pose_kp1; + + int range_end = current_goal_idx + moving_average_length; + for (int i = current_goal_idx; i < range_end; ++i) + { + // Transform pose of the global plan to the planning frame + tf2::doTransform(global_plan.at(i + 1), tf_pose_kp1, tf_plan_to_global); + + // calculate yaw angle + candidates.push_back( + std::atan2(tf_pose_kp1.pose.position.y - tf_pose_k.pose.position.y, tf_pose_kp1.pose.position.x - tf_pose_k.pose.position.x)); + + if (i < range_end - 1) tf_pose_k = tf_pose_kp1; + } + return teb_local_planner::average_angles(candidates); +} + +void MpcLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist) +{ + ROS_WARN_COND(opt_inscribed_radius + min_obst_dist < costmap_inscribed_radius, + "The inscribed radius of the footprint specified for TEB optimization (%f) + min_obstacle_dist (%f) are smaller " + "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). " + "Infeasible optimziation results might occur frequently!", + opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius); +} + +void MpcLocalPlannerROS::customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg) +{ + std::lock_guard l(_custom_obst_mutex); + _custom_obstacle_msg = *obst_msg; +} + +void MpcLocalPlannerROS::customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg) +{ + ROS_INFO_ONCE("Via-points received. This message is printed once."); + if (_params.global_plan_viapoint_sep > 0) + { + ROS_WARN( + "Via-points are already obtained from the global plan (global_plan_viapoint_sep>0)." + "Ignoring custom via-points."); + _custom_via_points_active = false; + return; + } + + std::lock_guard lock(_via_point_mutex); + _via_points.clear(); + for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses) + { + _via_points.emplace_back(pose.pose); + } + _custom_via_points_active = !_via_points.empty(); +} + +teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh) +{ + std::string model_name; + if (!nh.getParam("footprint_model/type", model_name)) + { + ROS_INFO("No robot footprint model specified for trajectory optimization. Using point-shaped model."); + return boost::make_shared(); + } + + // point + if (model_name.compare("point") == 0) + { + ROS_INFO("Footprint model 'point' loaded for trajectory optimization."); + return boost::make_shared(); + } + + // circular + if (model_name.compare("circular") == 0) + { + // get radius + double radius; + if (!nh.getParam("footprint_model/radius", radius)) + { + ROS_ERROR_STREAM("Footprint model 'circular' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() << "/footprint_model/radius' does not exist. Using point-model instead."); + return boost::make_shared(); + } + ROS_INFO_STREAM("Footprint model 'circular' (radius: " << radius << "m) loaded for trajectory optimization."); + return boost::make_shared(radius); + } + + // line + if (model_name.compare("line") == 0) + { + // check parameters + if (!nh.hasParam("footprint_model/line_start") || !nh.hasParam("footprint_model/line_end")) + { + ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() << "/footprint_model/line_start' and/or '.../line_end' do not exist. Using point-model instead."); + return boost::make_shared(); + } + // get line coordinates + std::vector line_start, line_end; + nh.getParam("footprint_model/line_start", line_start); + nh.getParam("footprint_model/line_end", line_end); + if (line_start.size() != 2 || line_end.size() != 2) + { + ROS_ERROR_STREAM( + "Footprint model 'line' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() + << "/footprint_model/line_start' and/or '.../line_end' do not contain x and y coordinates (2D). Using point-model instead."); + return boost::make_shared(); + } + + ROS_INFO_STREAM("Footprint model 'line' (line_start: [" << line_start[0] << "," << line_start[1] << "]m, line_end: [" << line_end[0] << "," + << line_end[1] << "]m) loaded for trajectory optimization."); + return boost::make_shared(Eigen::Map(line_start.data()), + Eigen::Map(line_end.data())); + } + + // two circles + if (model_name.compare("two_circles") == 0) + { + // check parameters + if (!nh.hasParam("footprint_model/front_offset") || !nh.hasParam("footprint_model/front_radius") || + !nh.hasParam("footprint_model/rear_offset") || !nh.hasParam("footprint_model/rear_radius")) + { + ROS_ERROR_STREAM("Footprint model 'two_circles' cannot be loaded for trajectory optimization, since params '" + << nh.getNamespace() + << "/footprint_model/front_offset', '.../front_radius', '.../rear_offset' and '.../rear_radius' do not exist. Using " + "point-model instead."); + return boost::make_shared(); + } + double front_offset, front_radius, rear_offset, rear_radius; + nh.getParam("footprint_model/front_offset", front_offset); + nh.getParam("footprint_model/front_radius", front_radius); + nh.getParam("footprint_model/rear_offset", rear_offset); + nh.getParam("footprint_model/rear_radius", rear_radius); + ROS_INFO_STREAM("Footprint model 'two_circles' (front_offset: " << front_offset << "m, front_radius: " << front_radius + << "m, rear_offset: " << rear_offset << "m, rear_radius: " << rear_radius + << "m) loaded for trajectory optimization."); + return boost::make_shared(front_offset, front_radius, rear_offset, rear_radius); + } + + // polygon + if (model_name.compare("polygon") == 0) + { + + // check parameters + XmlRpc::XmlRpcValue footprint_xmlrpc; + if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc)) + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() << "/footprint_model/vertices' does not exist. Using point-model instead."); + return boost::make_shared(); + } + // get vertices + if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + try + { + Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, "/footprint_model/vertices"); + ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization."); + return boost::make_shared(polygon); + } + catch (const std::exception& ex) + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: " << ex.what() + << ". Using point-model instead."); + return boost::make_shared(); + } + } + else + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() + << "/footprint_model/vertices' does not define an array of coordinates. Using point-model instead."); + return boost::make_shared(); + } + } + + // otherwise + ROS_WARN_STREAM("Unknown robot footprint model specified with parameter '" << nh.getNamespace() + << "/footprint_model/type'. Using point model instead."); + return boost::make_shared(); +} + +teb_local_planner::Point2dContainer MpcLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, + const std::string& full_param_name) +{ + // Make sure we have an array of at least 3 elements. + if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || footprint_xmlrpc.size() < 3) + { + ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", full_param_name.c_str(), + std::string(footprint_xmlrpc).c_str()); + throw std::runtime_error( + "The footprint must be specified as list of lists on the parameter server with at least " + "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); + } + + Point2dContainer footprint; + Eigen::Vector2d pt; + + for (int i = 0; i < footprint_xmlrpc.size(); ++i) + { + // Make sure each element of the list is an array of size 2. (x and y coordinates) + XmlRpc::XmlRpcValue point = footprint_xmlrpc[i]; + if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || point.size() != 2) + { + ROS_FATAL( + "The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.", + full_param_name.c_str()); + throw std::runtime_error( + "The footprint must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); + } + + pt.x() = getNumberFromXMLRPC(point[0], full_param_name); + pt.y() = getNumberFromXMLRPC(point[1], full_param_name); + + footprint.push_back(pt); + } + return footprint; +} + +double MpcLocalPlannerROS::getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) +{ + // Make sure that the value we're looking at is either a double or an int. + if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && value.getType() != XmlRpc::XmlRpcValue::TypeDouble) + { + std::string& value_string = value; + ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.", full_param_name.c_str(), value_string.c_str()); + throw std::runtime_error("Values in the footprint specification must be numbers"); + } + return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); +} + +} // end namespace mpc_local_planner diff --git a/mpc_local_planner/src/optimal_control/final_state_conditions_se2.cpp b/mpc_local_planner/src/optimal_control/final_state_conditions_se2.cpp new file mode 100644 index 0000000..86c029c --- /dev/null +++ b/mpc_local_planner/src/optimal_control/final_state_conditions_se2.cpp @@ -0,0 +1,66 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include + +namespace mpc_local_planner { + +void QuadraticFinalStateCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, + Eigen::Ref cost) const +{ + assert(cost.size() == getNonIntegralStateTermDimension(k)); + + Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); + xd[2] = normalize_theta(xd[2]); + if (_lsq_form) + { + if (_diagonal_mode) + cost.noalias() = _Qf_diag_sqrt * xd; + else + cost.noalias() = _Qf_sqrt * xd; + } + else + { + if (_diagonal_mode) + cost.noalias() = xd.transpose() * _Qf_diag * xd; + else + cost.noalias() = xd.transpose() * _Qf * xd; + } +} + +void TerminalBallSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const +{ + assert(cost.size() == getNonIntegralStateTermDimension(k)); + + Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); + xd[2] = normalize_theta(xd[2]); + if (_diagonal_mode) + cost[0] = xd.transpose() * _S_diag * xd - _gamma; + else + cost[0] = xd.transpose() * _S * xd - _gamma; +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/optimal_control/finite_differences_grid_se2.cpp b/mpc_local_planner/src/optimal_control/finite_differences_grid_se2.cpp new file mode 100644 index 0000000..f591053 --- /dev/null +++ b/mpc_local_planner/src/optimal_control/finite_differences_grid_se2.cpp @@ -0,0 +1,156 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace mpc_local_planner { + +void FiniteDifferencesGridSE2::createEdges(NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics) +{ + assert(isValid()); + + // clear edges first + // TODO(roesmann): we could implement a more efficient strategy without recreating the whole edgeset everytime + edges.clear(); + + int n = getN(); + + std::vector cost_terms, eq_terms, ineq_terms; + for (int k = 0; k < n - 1; ++k) + { + VectorVertexSE2& x_next = (k < n - 2) ? _x_seq[k + 1] : _xf; + VectorVertex& u_prev = (k > 0) ? _u_seq[k - 1] : _u_prev; + ScalarVertex& dt_prev = (k > 0) ? _dt : _u_prev_dt; + + cost_terms.clear(); + eq_terms.clear(); + ineq_terms.clear(); + nlp_fun.getNonIntegralStageFunctionEdges(k, _x_seq[k], _u_seq[k], _dt, u_prev, dt_prev, cost_terms, eq_terms, ineq_terms); + for (BaseEdge::Ptr& edge : cost_terms) edges.addObjectiveEdge(edge); + for (BaseEdge::Ptr& edge : eq_terms) edges.addEqualityEdge(edge); + for (BaseEdge::Ptr& edge : ineq_terms) edges.addInequalityEdge(edge); + + if (nlp_fun.stage_cost && nlp_fun.stage_cost->hasIntegralTerms(k)) + { + if (_cost_integration == CostIntegrationRule::TrapezoidalRule) + { + corbo::TrapezoidalIntegralCostEdge::Ptr edge = + std::make_shared(_x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_cost, k); + edges.addObjectiveEdge(edge); + } + else if (_cost_integration == CostIntegrationRule::LeftSum) + { + corbo::LeftSumCostEdge::Ptr edge = std::make_shared(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_cost, k); + edges.addObjectiveEdge(edge); + } + else + PRINT_ERROR_NAMED("Cost integration rule not implemented"); + } + + if (nlp_fun.stage_equalities && nlp_fun.stage_equalities->hasIntegralTerms(k)) + { + if (_cost_integration == CostIntegrationRule::TrapezoidalRule) + { + corbo::TrapezoidalIntegralEqualityDynamicsEdge::Ptr edge = std::make_shared( + dynamics, _x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_equalities, k); + edge->setFiniteDifferencesCollocationMethod(_fd_eval); + edges.addEqualityEdge(edge); + } + else if (_cost_integration == CostIntegrationRule::LeftSum) + { + corbo::LeftSumEqualityEdge::Ptr edge = + std::make_shared(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_equalities, k); + edges.addEqualityEdge(edge); + + // system dynamics edge + corbo::FDCollocationEdge::Ptr sys_edge = std::make_shared(dynamics, _x_seq[k], _u_seq[k], x_next, _dt); + sys_edge->setFiniteDifferencesCollocationMethod(_fd_eval); + edges.addEqualityEdge(sys_edge); + } + else + PRINT_ERROR_NAMED("Cost integration rule not implemented"); + } + else + { + // just the system dynamics edge + corbo::FDCollocationEdge::Ptr edge = std::make_shared(dynamics, _x_seq[k], _u_seq[k], x_next, _dt); + edge->setFiniteDifferencesCollocationMethod(_fd_eval); + edges.addEqualityEdge(edge); + } + + if (nlp_fun.stage_inequalities && nlp_fun.stage_inequalities->hasIntegralTerms(k)) + { + if (_cost_integration == CostIntegrationRule::TrapezoidalRule) + { + corbo::TrapezoidalIntegralInequalityEdge::Ptr edge = + std::make_shared(_x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_inequalities, k); + edges.addInequalityEdge(edge); + } + else if (_cost_integration == CostIntegrationRule::LeftSum) + { + corbo::LeftSumInequalityEdge::Ptr edge = + std::make_shared(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_inequalities, k); + edges.addInequalityEdge(edge); + } + else + PRINT_ERROR_NAMED("Cost integration rule not implemented"); + } + } + + // check if we have a separate unfixed final state + if (!_xf.isFixed()) + { + // set final state cost + BaseEdge::Ptr cost_edge = nlp_fun.getFinalStateCostEdge(n - 1, _xf); + if (cost_edge) edges.addObjectiveEdge(cost_edge); + + // set final state constraint + BaseEdge::Ptr constr_edge = nlp_fun.getFinalStateConstraintEdge(n - 1, _xf); + if (constr_edge) + { + if (nlp_fun.final_stage_constraints->isEqualityConstraint()) + edges.addEqualityEdge(constr_edge); + else + edges.addInequalityEdge(constr_edge); + } + } + + // add control deviation edges for last control + cost_terms.clear(); + eq_terms.clear(); + ineq_terms.clear(); + nlp_fun.getFinalControlDeviationEdges(n, _u_ref, _u_seq.back(), _dt, cost_terms, eq_terms, ineq_terms); + for (BaseEdge::Ptr& edge : cost_terms) edges.addObjectiveEdge(edge); + for (BaseEdge::Ptr& edge : eq_terms) edges.addEqualityEdge(edge); + for (BaseEdge::Ptr& edge : ineq_terms) edges.addInequalityEdge(edge); +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/optimal_control/finite_differences_variable_grid_se2.cpp b/mpc_local_planner/src/optimal_control/finite_differences_variable_grid_se2.cpp new file mode 100644 index 0000000..8e14313 --- /dev/null +++ b/mpc_local_planner/src/optimal_control/finite_differences_variable_grid_se2.cpp @@ -0,0 +1,163 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace mpc_local_planner { + +void FiniteDifferencesVariableGridSE2::setDtBounds(double dt_lb, double dt_ub) +{ + _dt_lb = dt_lb; + _dt_ub = dt_ub; +} + +void FiniteDifferencesVariableGridSE2::setGridAdaptTimeBasedSingleStep(int n_max, double dt_hyst_ratio, bool adapt_first_iter) +{ + _grid_adapt = GridAdaptStrategy::TimeBasedSingleStep; + _n_max = n_max; + _dt_hyst_ratio = dt_hyst_ratio; + _adapt_first_iter = adapt_first_iter; +} + +void FiniteDifferencesVariableGridSE2::setGridAdaptTimeBasedAggressiveEstimate(int n_max, double dt_hyst_ratio, bool adapt_first_iter) +{ + _grid_adapt = GridAdaptStrategy::TimeBasedAggressiveEstimate; + _n_max = n_max; + _dt_hyst_ratio = dt_hyst_ratio; + _adapt_first_iter = adapt_first_iter; +} + +void FiniteDifferencesVariableGridSE2::setGridAdaptSimpleShrinkingHorizon(bool adapt_first_iter) +{ + _grid_adapt = GridAdaptStrategy::SimpleShrinkingHorizon; + _adapt_first_iter = adapt_first_iter; +} + +bool FiniteDifferencesVariableGridSE2::adaptGrid(bool new_run, NlpFunctions& nlp_fun) +{ + // do not adapt grid in a new run + if (new_run && !_adapt_first_iter) return false; + + bool changed = false; + switch (_grid_adapt) + { + case GridAdaptStrategy::NoGridAdapt: + { + break; + } + case GridAdaptStrategy::TimeBasedSingleStep: + { + changed = adaptGridTimeBasedSingleStep(nlp_fun); + break; + } + case GridAdaptStrategy::TimeBasedAggressiveEstimate: + { + changed = adaptGridTimeBasedAggressiveEstimate(nlp_fun); + break; + } + case GridAdaptStrategy::SimpleShrinkingHorizon: + { + changed = adaptGridSimpleShrinkingHorizon(nlp_fun); + break; + } + default: + { + PRINT_ERROR_NAMED("selected grid adaptation strategy not implemented."); + } + } + return changed; +} + +bool FiniteDifferencesVariableGridSE2::adaptGridTimeBasedSingleStep(NlpFunctions& nlp_fun) +{ + PRINT_WARNING_COND_NAMED(!isTimeVariableGrid(), "time based adaptation might only be used with a fixed dt."); + + _nlp_fun = &nlp_fun; + + int n = getN(); + + double dt = getDt(); + if (dt > _dt_ref * (1.0 + _dt_hyst_ratio) && n < _n_max) + { + resampleTrajectory(n + 1); + _n_adapt = n + 1; + return true; + } + else if (dt < _dt_ref * (1.0 - _dt_hyst_ratio) && n > _n_min) + { + resampleTrajectory(n - 1); + _n_adapt = n - 1; + return true; + } + return false; +} + +bool FiniteDifferencesVariableGridSE2::adaptGridTimeBasedAggressiveEstimate(NlpFunctions& nlp_fun) +{ + PRINT_WARNING_COND_NAMED(!isTimeVariableGrid(), "time based adaptation might only be used with a fixed dt."); + + _nlp_fun = &nlp_fun; + int n = getN(); + double dt = getDt(); + + // check if hysteresis is satisfied + if (dt >= _dt_ref * (1.0 - _dt_hyst_ratio) && dt <= _dt_ref * (1.0 + _dt_hyst_ratio)) return false; + + // estimate number of samples based on the fraction dt/dt_ref. + // dt is the time difference obtained in a previous solution (with a coarser resp. finer trajectory resolution) + int new_n = std::round((double)n * (dt / _dt_ref)); + + // bound value + if (new_n > _n_max) + new_n = _n_max; + else if (new_n < _n_min) + new_n = _n_min; + + if (new_n == n) return false; + + // and now resample + resampleTrajectory(new_n); + _n_adapt = new_n; + return true; +} + +bool FiniteDifferencesVariableGridSE2::adaptGridSimpleShrinkingHorizon(NlpFunctions& nlp_fun) +{ + int n = getN(); + if (n > _n_min) + { + resampleTrajectory(n - 1); + _n_adapt = n - 1; + } + return false; +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp b/mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp new file mode 100644 index 0000000..c477c1a --- /dev/null +++ b/mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp @@ -0,0 +1,614 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace mpc_local_planner { + +corbo::GridUpdateResult FullDiscretizationGridBaseSE2::update(const Eigen::VectorXd& x0, ReferenceTrajectoryInterface& xref, + ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, + SystemDynamicsInterface::Ptr dynamics, bool new_run, const corbo::Time& t, + ReferenceTrajectoryInterface* sref, const Eigen::VectorXd* prev_u, double prev_u_dt, + ReferenceTrajectoryInterface* xinit, ReferenceTrajectoryInterface* uinit) +{ + assert(x0.size() == dynamics->getStateDimension()); + assert(xref.getDimension() == x0.size()); + assert(uref.getDimension() == dynamics->getInputDimension()); + assert(x0.size() >= 3); // SE2 version here + + corbo::GridUpdateResult result; + _nlp_fun = &nlp_fun; // for bound access in e.g. resampleTrajectory() + + if (isGridAdaptActive() && !_first_run && !isEmpty()) // TODO(roesmann): this might not be efficient in case warm start is deactivated + { + // adapt grid if implemented by subclass + adaptGrid(new_run, nlp_fun); // note, isModified() should return true if something was changed!, we could also use the return value... + } + + // check if we need to cache the reference trajectory values // TODO(roesmann): we could restrict this to new_run==true only as long as we do not + // have a grid resize... + int n = std::max(std::max(getNRef(), getN()), _n_adapt); + if (!xref.isCached(getDt(), n, t)) xref.precompute(getDt(), n, t); + if (!uref.isCached(getDt(), n, t)) uref.precompute(getDt(), n, t); + if (sref && !sref->isCached(getDt(), n, t)) sref->precompute(getDt(), n, t); + if (xinit && !xinit->isCached(getDt(), n, t)) xinit->precompute(getDt(), n, t); + if (uinit && !uinit->isCached(getDt(), n, t)) uinit->precompute(getDt(), n, t); + + // set previous control which might be utilized by some edges + if (prev_u && prev_u->size() > 0) + setPreviousControl(*prev_u, prev_u_dt); + else + setPreviousControl(Eigen::VectorXd::Zero(dynamics->getInputDimension()), prev_u_dt); + + // set last control to uref + setLastControlRef(uref.getReferenceCached(n)); + + // TODO(roesmann): we do not check if bounds in nlp_fun are updated + // updateBounds(); // calling this everytime is not efficient + + // initialize trajectories if empty or if the goal differs from the last one + // reinit if goal is far away + if (isEmpty()) // TODO(roesmann): threshold?: if (_cached_xf.rows() > 0 && (xf - _cached_xf).norm() > _warm_start_goal_dist_reinit) + { + if (xref.isStatic() && !xinit) + { + // TODO(roesmann): optional initialization references x_init, u_init + initializeSequences(x0, xref.getReferenceCached(n - 1), uinit ? *uinit : uref, nlp_fun); + } + else + { + initializeSequences(x0, xref.getReferenceCached(n - 1), xinit ? *xinit : xref, uinit ? *uinit : uref, nlp_fun); + } + } + else + { + if (new_run && isMovingHorizonWarmStartActive()) + { + // warm_start + warmStartShifting(x0); + } + if (new_run) + { + // make sure to always overwrite the start with the actual (measured) values + _x_seq.front().values() = x0; + // update fixed goal states + for (int i = 0; i < _xf_fixed.size(); ++i) + { + if (_xf_fixed[i]) _xf.values()[i] = xref.getReferenceCached(getN() - 1)[i]; + } + } + } + + result.vertices_updated = isModified(); // isModified() returns if the underlying vertex set is updated + + if (new_run || result.updated()) // new run -> new t + { + // update NLP functions w.r.t. the current discretization + result.edges_updated = + nlp_fun.update(getN(), t.toSec(), xref, uref, sref, hasSingleDt(), x0, {getDt()}, this); // returns true if edge dimensions changed + // TODO(roesmann): sref is not yet implemented for testing, add this later... + // TODO(roesmann): we do not yet check if dt was updated, this might affect nlp update + } + + if (result.updated()) // vertices or eges updated + { + // create grid specific edges + // TODO(roesmann): for now, we assume that we are the only maintainer of the edge set (which might be not true generally!) + // so we clear the edge set whenever we want + createEdges(nlp_fun, edges, dynamics); + result.edges_updated = true; // now we definitely updated the edgeset + } + _first_run = false; + return result; +} + +void FullDiscretizationGridBaseSE2::initializeSequences(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, ReferenceTrajectoryInterface& uref, + NlpFunctions& nlp_fun) +{ + assert(x0.size() >= 3); + + // clear(); // make sure everything is cleared + _x_seq.clear(); + _u_seq.clear(); + _xf.clear(); + _active_vertices.clear(); + + // check or initalize bounds if empty + nlp_fun.checkAndInitializeBoundDimensions(x0.size(), uref.getDimension()); + + // check x_fixed + checkAndInitializeXfFixedFlags(x0.size()); + + int n_init = _n_adapt > 0 ? _n_adapt : _n_ref; + + int num_intervals = n_init - 1; + + // we initialize the state trajectory linearly + Eigen::VectorXd dir = xf - x0; + double dist = dir.norm(); + if (dist != 0) dir /= dist; + double step = dist / num_intervals; + + // For the SE2 case here, we set the third state (orientation) to the goal heading + double orient_init = std::atan2(dir[1], dir[0]); + // but check if goal is behind start pose (w.r.t. start orientation) + if (dir.head(2).dot(Eigen::Vector2d(std::cos(x0[2]), std::sin(x0[2]))) < 0) orient_init = normalize_theta(orient_init + M_PI); + + for (int k = 0; k < num_intervals; ++k) + { + // add new state by linear interpolation + Eigen::VectorXd new_x = x0 + (double)k * step * dir; + new_x[2] = orient_init; + _x_seq.emplace_back(new_x, nlp_fun.x_lb, nlp_fun.x_ub); + // add new constant control (assume u0 to hold + _u_seq.emplace_back(uref.getReferenceCached(k), nlp_fun.u_lb, nlp_fun.u_ub); + } + // add final state + _xf.set(xf, nlp_fun.x_lb, nlp_fun.x_ub, _xf_fixed); + + // set first state as fixed + _x_seq.front().setFixed(true); + + // set dt + _dt.set(_dt_ref, _dt_lb, _dt_ub, isDtFixedIntended()); + + assert(_x_seq.size() > 0); + + // notify graph that vertices are changed + setModified(true); +} + +void FullDiscretizationGridBaseSE2::initializeSequences(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, ReferenceTrajectoryInterface& xref, + ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun) +{ + // clear(); // make sure everything is cleared + _x_seq.clear(); + _u_seq.clear(); + _xf.clear(); + _active_vertices.clear(); + + // TODO(roesmann): check if this is ever needed in any pracital realization: + // if ((x0 - xref.getReferenceCached(0)).norm() > _non_static_ref_dist_threshold) + // { + // initializeSequences(x0, xref.getReferenceCached(getN() - 1), uref, nlp_fun); + // } + + // check or initalize bounds if empty + nlp_fun.checkAndInitializeBoundDimensions(x0.size(), uref.getDimension()); + + // check x_fixed + checkAndInitializeXfFixedFlags(x0.size()); + + int n_init = _n_adapt > 0 ? _n_adapt : _n_ref; + + int num_intervals = n_init - 1; + + _x_seq.emplace_back(x0, nlp_fun.x_lb, nlp_fun.x_ub); // always add the (exact) x0 + _u_seq.emplace_back(uref.getReferenceCached(0), nlp_fun.u_lb, nlp_fun.u_ub); + for (int k = 1; k < num_intervals; ++k) + { + // add new state by linear interpolation + _x_seq.emplace_back(xref.getReferenceCached(k), nlp_fun.x_lb, nlp_fun.x_ub); + // add new constant control (assume u0 to hold + _u_seq.emplace_back(uref.getReferenceCached(k), nlp_fun.u_lb, nlp_fun.u_ub); + } + // add final state + _xf.set(xf, nlp_fun.x_lb, nlp_fun.x_ub, _xf_fixed); + + // set first state as fixed + _x_seq.front().setFixed(true); + + // set dt + _dt.set(_dt_ref, _dt_lb, _dt_ub, isDtFixedIntended()); + + assert(_x_seq.size() > 1); + + // notify graph that vertices are changed + setModified(true); +} + +void FullDiscretizationGridBaseSE2::warmStartShifting(const Eigen::VectorXd& x0) +{ + // find nearest state to x0 (ideally it is the second one in _x_seq). + int num_shift = findNearestState(x0); + if (num_shift <= 0) return; + + if (num_shift > getN() - 2) + { + PRINT_ERROR_NAMED("Cannot shift if num_shift > N-2"); + return; + } + + // the simplest strategy would be to just remove remove x_seq[0], append xf to x_seq and replace xf (num_shift=1) + // however, this requries to change the structure which always triggers an edge recreation and so the solver must reallocate memory. + // TOOD(roesmann): for time-optimal stuff, if we are sure that we resize the grid, it is then more efficent to implement the strategy above... + + // shift from end to front: + for (int i = 0; i < getN() - num_shift; ++i) + { + int idx = i + num_shift; + if (idx == getN() - 1) + { + // final state reached + _x_seq[i].values() = _xf.values(); + } + else + { + _x_seq[i].values() = _x_seq[idx].values(); + _u_seq[i].values() = _u_seq[idx].values(); + } + } + + int idx = getN() - num_shift; + if (idx < 0) + { + PRINT_ERROR_NAMED("idx < 0..."); + return; + } + for (int i = 0; i < num_shift; ++i, ++idx) + { + // now extrapolate + assert(idx > 1); + + // linearly extrapolate states + if (i == num_shift - 1) // consider xf + { + _xf.values() = _x_seq[idx - 2].values() + 2.0 * (_x_seq[idx - 1].values() - _x_seq[idx - 2].values()); + // TODO(roesmann): we could avoid computing the third row in the previous command, however, the code + // increases if we support additional states + _xf.values()[2] = interpolate_angle(_x_seq[idx - 2].values()[2], _x_seq[idx - 1].values()[2], 2.0); + } + else + { + // TODO(roesmann) multiply by fraction of last dt and _dt + _x_seq[idx].values() = _x_seq[idx - 2].values() + 2.0 * (_x_seq[idx - 1].values() - _x_seq[idx - 2].values()); + // TODO(roesmann): we could avoid computing the third row in the previous command, however, the code + // increases if we support additional states + _x_seq[idx].values()[2] = interpolate_angle(_x_seq[idx - 2].values()[2], _x_seq[idx - 1].values()[2], 2.0); + } + _u_seq[idx - 1].values() = _u_seq[idx - 2].values(); + } +} + +int FullDiscretizationGridBaseSE2::findNearestState(const Eigen::VectorXd& x0) +{ + assert(!isEmpty()); + assert(isValid()); + + // same start as before + double first_dist = (x0 - _x_seq.front().values()).norm(); + if (std::abs(first_dist) < 1e-12) return 0; + + // find nearest state (using l2-norm) in order to prune the trajectory + // (remove already passed states, should be 1 if the sampling times of the planning and controller call match, but could be higher if rates + // differ) + + int num_interv = getN() - 1; + + double dist_cache = first_dist; + double dist; + int num_keep_interv = 1; // std::max(1, _num_states_min - 1); // we need to keep at least this number of intervals in the trajectory + int lookahead = std::min(num_interv - num_keep_interv, 20); // max 20 samples + + int nearest_idx = 0; + + for (int i = 1; i <= lookahead; ++i) + { + dist = (x0 - _x_seq[i].values()).norm(); + if (dist < dist_cache) + { + dist_cache = dist; + nearest_idx = i; + } + else + break; + } + + return nearest_idx; +} + +bool FullDiscretizationGridBaseSE2::getFirstControlInput(Eigen::VectorXd& u0) +{ + if (isEmpty() || !isValid()) return false; + assert(!_u_seq.empty()); + + u0 = _u_seq.front().values(); + return true; +} + +void FullDiscretizationGridBaseSE2::setNRef(int n) +{ + // clear grid if we change n + if (n != getN()) clear(); + if (n < 2) + { + PRINT_ERROR_NAMED("Number of states must be n>1."); + _n_ref = 2; + return; + } + _n_ref = n; + _n_adapt = 0; +} + +int FullDiscretizationGridBaseSE2::findClosestPose(double x_ref, double y_ref, int start_idx, double* distance) const +{ + double min_dist = std::numeric_limits::max(); + int min_idx = -1; + + for (int i = start_idx; i < (int)_x_seq.size(); ++i) + { + double dist = distance_points2d(x_ref, y_ref, _x_seq[i].values()[0], _x_seq[i].values()[1]); + if (dist < min_dist) + { + min_dist = dist; + min_idx = i; + } + } + double dist = distance_points2d(x_ref, y_ref, _xf.values()[0], _xf.values()[1]); + if (dist < min_dist) + { + min_dist = dist; + min_idx = (int)_x_seq.size(); + } + + if (distance) *distance = min_dist; + + return min_idx; +} + +bool FullDiscretizationGridBaseSE2::checkAndInitializeXfFixedFlags(int dim_x) +{ + if (_xf_fixed.size() == 0) + { + _xf_fixed.setConstant(dim_x, false); + return true; + } + else if (_xf_fixed.size() == dim_x) + return true; + + PRINT_ERROR_NAMED("Dimensions mismatch between xf_fixed and xf. Setting xf_fixed to false."); + _xf_fixed.setConstant(dim_x, false); + return false; +} + +void FullDiscretizationGridBaseSE2::updateBounds(const NlpFunctions& nlp_fun) +{ + if (isEmpty()) return; + for (VectorVertexSE2& vtx : _x_seq) + { + if (vtx.getDimension() == nlp_fun.x_lb.size()) + vtx.setLowerBounds(nlp_fun.x_lb); + else + PRINT_ERROR_NAMED("Cannot update lower state bounds due to dimensions mismatch"); + + if (vtx.getDimension() == nlp_fun.x_ub.size()) + vtx.setUpperBounds(nlp_fun.u_ub); + else + PRINT_ERROR_NAMED("Cannot update upper state bounds due to dimensions mismatch"); + } + + for (VectorVertex& vtx : _u_seq) + { + if (vtx.getDimension() == nlp_fun.u_lb.size()) + vtx.setLowerBounds(nlp_fun.u_lb); + else + PRINT_ERROR_NAMED("Cannot update lower control input bounds due to dimensions mismatch"); + + if (vtx.getDimension() == nlp_fun.u_ub.size()) + vtx.setUpperBounds(nlp_fun.u_ub); + else + PRINT_ERROR_NAMED("Cannot update upper control input bounds due to dimensions mismatch"); + } + + _dt.setLowerBound(_dt_lb); + _dt.setUpperBound(_dt_ub); + + _nlp_fun = &nlp_fun; // for bound access in e.g. resampleTrajectory() +} + +void FullDiscretizationGridBaseSE2::resampleTrajectory(int n_new) +{ + assert(isValid()); + int n = getN(); + if (n == n_new) return; + + if (!_nlp_fun) + { + PRINT_ERROR_NAMED("We currently need _nlp_fun to be valid in order to retrieve bounds"); + } + + // copy vertices (vertices itself are shared pointers) + //! @todo(roesmann) More efficient strategy without copying containers at all? + //! + TimeSeries::Ptr ts_states_old = std::make_shared(); + TimeSeries::Ptr ts_controls_old = std::make_shared(); + getStateAndControlTimeSeries(ts_states_old, ts_controls_old); + + TimeSeries::ValuesMatMap x_seq_old = ts_states_old->getValuesMatrixView(); + TimeSeries::ValuesMatMap u_seq_old = ts_controls_old->getValuesMatrixView(); + + int num_interv = n - 1; + + double dt_old = getDt(); + // compute new time diff + double dt_new = dt_old * double(n - 1) / double(n_new - 1); + + double t_new; + int idx_old = 1; + double t_old_p1 = dt_old; // time for old sample with index idx_old (acutally its the subsequent time step w.r.t t_new) + + for (int idx_new = 1; idx_new < n_new - 1; ++idx_new) // n_new-1 since last state is identical and copied later. idx_new=1, since start + // sample is already valid (we do not touch it) + // we allow a small mismatch for the control u_1 and let the optimizer correct it later + { + t_new = dt_new * double(idx_new); + while (t_new > double(idx_old) * dt_old && idx_old < n) + { + ++idx_old; + }; // find idx_old that represents the state subsequent to the new one (w.r.t. time) + t_old_p1 = double(idx_old) * dt_old; + + const Eigen::VectorXd& x_prev = x_seq_old.col(idx_old - 1); + const Eigen::VectorXd& x_cur = (idx_old < n - 1) ? x_seq_old.col(idx_old) : _xf.values(); + + if (idx_new < num_interv) + { + // states + _x_seq[idx_new].values() = x_prev + (t_new - (t_old_p1 - dt_old)) / dt_old * (x_cur - x_prev); + // overwrite angle (since we are in SE2, at least the first three states are SE2) + // TODO(roesmann): we could avoid computing the third row in the previous command, however, the code + // increases if we support additional states + _x_seq[idx_new].values()[2] = interpolate_angle(x_prev[2], x_cur[2], (t_new - (t_old_p1 - dt_old)) / dt_old); + + // controls + // TODO(roesmann): we do not have a valid control (u_f), so hold last + _u_seq[idx_new].values() = u_seq_old.col(idx_old - 1); + } + else + { + // add new pair + _x_seq.emplace_back(x_prev + (t_new - (t_old_p1 - dt_old)) / dt_old * (x_cur - x_prev), _nlp_fun->x_lb, _nlp_fun->x_ub); + // TODO(roesmann): we could avoid computing the third row in the previous command, however, the code + // increases if we support additional states + _x_seq.back().values()[2] = interpolate_angle(x_prev[2], x_cur[2], (t_new - (t_old_p1 - dt_old)) / dt_old); + _u_seq.emplace_back(u_seq_old.col(idx_old - 1), _nlp_fun->u_lb, _nlp_fun->u_ub); + } + } + + // clear invalid states + if (n_new < n) + { + _x_seq.resize(n_new - 1); + _u_seq.resize(n_new - 1); + } + + // save new dt + _dt.value() = dt_new; + + // set first vertex always to fixed + // _x_seq.front().setFixed(true); // should not be necessary here (we just change values in the first node) + + // notify graph that vertices are changed + setModified(true); +} + +void FullDiscretizationGridBaseSE2::clear() +{ + _x_seq.clear(); + _u_seq.clear(); + _xf.clear(); + _active_vertices.clear(); + _nlp_fun = nullptr; + _n_adapt = 0; + _first_run = true; + setModified(true); +} + +void FullDiscretizationGridBaseSE2::setN(int n, bool try_resample) +{ + if (try_resample && _nlp_fun && !isEmpty()) + { + resampleTrajectory(n); + } + else + { + clear(); // resampling not yet implemented + } + setNRef(n); +} + +void FullDiscretizationGridBaseSE2::getVertices(std::vector& vertices) +{ + vertices.clear(); + // order doesn't matter here + for (VectorVertexSE2& vtx : _x_seq) vertices.push_back(&vtx); + for (VectorVertex& vtx : _u_seq) vertices.push_back(&vtx); + vertices.push_back(&_xf); + vertices.push_back(&_dt); // make sure to make it fixed if desired in any subclass + vertices.push_back(&_u_prev); // always fixed... +} + +void FullDiscretizationGridBaseSE2::computeActiveVertices() +{ + _active_vertices.clear(); + assert(isValid()); + int n = getN(); + + for (int i = 0; i < n - 1; ++i) + { + if (!_x_seq[i].isFixed()) _active_vertices.push_back(&_x_seq[i]); + if (!_u_seq[i].isFixed()) _active_vertices.push_back(&_u_seq[i]); + } + if (!_xf.isFixed()) _active_vertices.push_back(&_xf); + if (!_dt.isFixed()) _active_vertices.push_back(&_dt); +} + +void FullDiscretizationGridBaseSE2::getStateAndControlTimeSeries(TimeSeries::Ptr x_sequence, TimeSeries::Ptr u_sequence, double t_max) const +{ + if (x_sequence) x_sequence->clear(); + if (u_sequence) u_sequence->clear(); + + if (isEmpty()) return; + assert(isValid()); + + PRINT_ERROR_COND_NAMED(t_max < 0, "t_max >= 0 required"); + + double dt = getDt(); + + if (x_sequence) + { + double t = 0; + for (int i = 0; i < _x_seq.size(); ++i) + { + x_sequence->add(t, _x_seq[i].values()); + t += dt; + if (t > t_max) break; + } + if (t <= t_max) x_sequence->add(t, _xf.values()); + } + + if (u_sequence) + { + double t = 0; + for (int i = 0; i < _u_seq.size(); ++i) + { + u_sequence->add(t, _u_seq[i].values()); + t += dt; + if (t > t_max) break; + } + // duplicate last u to have the sampe time stamps as x_sequence + if (t <= t_max) u_sequence->add(t, _u_seq.back().values()); + } +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/optimal_control/min_time_via_points_cost.cpp b/mpc_local_planner/src/optimal_control/min_time_via_points_cost.cpp new file mode 100644 index 0000000..6c1604b --- /dev/null +++ b/mpc_local_planner/src/optimal_control/min_time_via_points_cost.cpp @@ -0,0 +1,147 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include +#include + +#include +#include + +namespace mpc_local_planner { + +int MinTimeViaPointsCost::getNonIntegralStateTermDimension(int k) const +{ + assert(k < _vp_association.size()); + assert(_vp_association[k].first.size() == _vp_association[k].second); + return (int)_vp_association[k].second; +} + +bool MinTimeViaPointsCost::update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/, + corbo::ReferenceTrajectoryInterface* /*sref*/, bool single_dt, const Eigen::VectorXd& x0, + corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector& /*dts*/, + const corbo::DiscretizationGridInterface* grid) +{ + if (!_via_points) + { + PRINT_WARNING("MinTimeViaPointsCost::update(): no via_point container specified"); + return !_vp_association.empty(); + } + + // setup minimum time cost + _single_dt = single_dt; + if (single_dt) + _time_weight = n - 1; + else + _time_weight = 1.0; + + // Setup via points + + // we currently require a full discretization grid as we want to have fast access to + // individual states without requiring any new simulation. + // Alternatively, other grids could be used in combination with method getStateAndControlTimeSeries() + const FullDiscretizationGridBaseSE2* fd_grid = static_cast(grid); + + assert(n == fd_grid->getN()); + + bool new_structure = (n != _vp_association.size()); + if (new_structure) + { + _vp_association.resize(n, std::make_pair, std::size_t>({}, 0)); + } + + // clear previous association + for (auto& item : _vp_association) + { + item.first.clear(); + } + + int start_pose_idx = 0; + + for (const teb_local_planner::PoseSE2& pose : *_via_points) + { + int idx = fd_grid->findClosestPose(pose.x(), pose.y(), start_pose_idx); + + if (_via_points_ordered) start_pose_idx = idx + 2; // skip a point to have a DOF inbetween for further via-points + + // check if point coincides with final state or is located behind it + if (idx > n - 2) idx = n - 2; // set to a pose before the goal, since it is never fixed + + // check if point coincides with start or is located before it + if (idx < 1) + { + if (_via_points_ordered) + idx = 1; // try to connect the via point with the second pose. Grid adaptation might add new poses inbetween later if enabled. + else + { + PRINT_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose."); + continue; // skip via points really close or behind the current robot pose + } + } + + _vp_association[idx].first.push_back(&pose); + } + + // check for structure changes + for (auto& item : _vp_association) + { + if (item.first.size() != item.second) + { + item.second = item.first.size(); + new_structure = true; + // but continue until end for next time + } + } + + // return true if structure requires a changed + return new_structure; +} + +void MinTimeViaPointsCost::computeNonIntegralDtTerm(int k, double dt, Eigen::Ref cost) const +{ + if (!_single_dt || k == 0) + cost[0] = _time_weight * dt; + else + { + PRINT_DEBUG_NAMED("this method should not be called in single_dt mode and k>0"); + } +} + +void MinTimeViaPointsCost::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const +{ + assert(_via_points); + assert(k < _vp_association.size()); + assert(cost.size() == _vp_association[k].first.size()); + assert(_vp_association[k].second == _vp_association[k].first.size()); + + for (int i = 0; i < (int)_vp_association[k].second; ++i) + { + cost[i] = _vp_position_weight * (_vp_association[k].first[i]->position() - x_k.head(2)).squaredNorm(); + if (_vp_orientation_weight > 0) + { + cost[i] += _vp_orientation_weight * normalize_theta(_vp_association[k].first[i]->theta() - x_k[2]); + } + } +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/optimal_control/quadratic_cost_se2.cpp b/mpc_local_planner/src/optimal_control/quadratic_cost_se2.cpp new file mode 100644 index 0000000..becaec6 --- /dev/null +++ b/mpc_local_planner/src/optimal_control/quadratic_cost_se2.cpp @@ -0,0 +1,125 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include + +namespace mpc_local_planner { + +void QuadraticFormCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const +{ + assert(!_integral_form); + assert(cost.size() == getNonIntegralStateTermDimension(k)); + + Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); + xd[2] = normalize_theta(xd[2]); + if (_lsq_form) + { + if (_Q_diagonal_mode) + cost.noalias() = _Q_diag_sqrt * xd; + else + cost.noalias() = _Q_sqrt * xd; + } + else + { + if (_Q_diagonal_mode) + cost.noalias() = xd.transpose() * _Q_diag * xd; + else + cost.noalias() = xd.transpose() * _Q * xd; + } +} + +void QuadraticFormCostSE2::computeIntegralStateControlTerm(int k, const Eigen::Ref& x_k, + const Eigen::Ref& u_k, Eigen::Ref cost) const +{ + assert(_integral_form); + assert(cost.size() == 1); + + cost[0] = 0; + + Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); + xd[2] = normalize_theta(xd[2]); + if (_Q_diagonal_mode) + cost[0] += xd.transpose() * _Q_diag * xd; + else + cost[0] += xd.transpose() * _Q * xd; + + if (_zero_u_ref) + { + if (_R_diagonal_mode) + cost[0] += u_k.transpose() * _R_diag * u_k; + else + cost[0] += u_k.transpose() * _R * u_k; + } + else + { + Eigen::VectorXd ud = u_k - _u_ref->getReferenceCached(k); + if (_R_diagonal_mode) + cost[0] += ud.transpose() * _R_diag * ud; + else + cost[0] += ud.transpose() * _R * ud; + } +} + +void QuadraticStateCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const +{ + assert(!_integral_form); + assert(cost.size() == getNonIntegralStateTermDimension(k)); + + Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); + xd[2] = normalize_theta(xd[2]); + if (_lsq_form) + { + if (_diagonal_mode) + cost.noalias() = _Q_diag_sqrt * xd; + else + cost.noalias() = _Q_sqrt * xd; + } + else + { + if (_diagonal_mode) + cost.noalias() = xd.transpose() * _Q_diag * xd; + else + cost.noalias() = xd.transpose() * _Q * xd; + } +} + +void QuadraticStateCostSE2::computeIntegralStateControlTerm(int k, const Eigen::Ref& x_k, + const Eigen::Ref& u_k, Eigen::Ref cost) const +{ + assert(_integral_form); + assert(cost.size() == 1); + + cost[0] = 0; + + Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k); + xd[2] = normalize_theta(xd[2]); + if (_diagonal_mode) + cost[0] += xd.transpose() * _Q_diag * xd; + else + cost[0] += xd.transpose() * _Q * xd; +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp b/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp new file mode 100644 index 0000000..4d6de49 --- /dev/null +++ b/mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp @@ -0,0 +1,210 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include +#include + +#include +#include + +#include +#include + +namespace mpc_local_planner { + +int StageInequalitySE2::getNonIntegralControlDeviationTermDimension(int k) const { return _num_du_lb_finite + _num_du_ub_finite; } + +int StageInequalitySE2::getNonIntegralStateTermDimension(int k) const +{ + assert(k < _relevant_obstacles.size()); + return (int)_relevant_obstacles[k].size(); +} + +bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/, + corbo::ReferenceTrajectoryInterface* /*sref*/, bool /* single_dt*/, const Eigen::VectorXd& x0, + corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector& /*dts*/, + const corbo::DiscretizationGridInterface* grid) +{ + assert(_obstacles); + assert(_robot_model); + + // Setup obstacle avoidance + + // we currently require a full discretization grid as we want to have fast access to + // individual states without requiring any new simulation. + // Alternatively, other grids could be used in combination with method getStateAndControlTimeSeries() + const FullDiscretizationGridBaseSE2* fd_grid = static_cast(grid); + + bool new_dimensions = (n != _relevant_obstacles.size()); + + _relevant_obstacles.resize(n); + + teb_local_planner::PoseSE2 pose; + + // iterate states but skip start state as it is not subject to optimization + for (int k = 1; k < n; ++k) + { + double left_min_dist = std::numeric_limits::max(); + double right_min_dist = std::numeric_limits::max(); + ObstaclePtr left_obstacle; + ObstaclePtr right_obstacle; + + const Eigen::VectorXd& xk = fd_grid->getState(k); + assert(xk.size() >= 3); + // TODO(roesmann): Overload robot fooprint model functions in teb_local_planner to avoid this copy: + pose.position() = xk.head(2); + pose.theta() = xk.coeffRef(2); + Eigen::Vector2d pose_orient = pose.orientationUnitVec(); + // Eigen::Vector2d pose_orient(std::cos(xk[2]), std::sin(xk[2])); + + int num_prev_obst = (int)_relevant_obstacles[k].size(); + _relevant_obstacles[k].clear(); + + // iterate obstacles + for (const ObstaclePtr& obst : *_obstacles) + { + // check for dynamic obstacle + if (_enable_dynamic_obstacles && obst->isDynamic()) + { + // we consider all dynamic obstacles by now + // TODO(roesmann): we might remove obstacles that "go away" from the trajectory + // or more generally that any intersection in the future is unlikely + _relevant_obstacles[k].push_back(obst); + continue; + } + + // calculate distance to robot model + double dist = _robot_model->calculateDistance(pose, obst.get()); + + // force considering obstacle if really close to the current pose + if (dist < _min_obstacle_dist * _obstacle_filter_force_inclusion_factor) + { + _relevant_obstacles[k].push_back(obst); + continue; + } + // cut-off distance + if (dist > _min_obstacle_dist * _obstacle_filter_cutoff_factor) continue; + + // determine side (left or right) and assign obstacle if closer than the previous one + if (cross2d(pose_orient, obst->getCentroid()) > 0) // left + { + if (dist < left_min_dist) + { + left_min_dist = dist; + left_obstacle = obst; + } + } + else + { + if (dist < right_min_dist) + { + right_min_dist = dist; + right_obstacle = obst; + } + } + } + + // add left and right obstacle + if (left_obstacle) _relevant_obstacles[k].push_back(left_obstacle); + if (right_obstacle) _relevant_obstacles[k].push_back(right_obstacle); + + // check if dimensions changed + new_dimensions = new_dimensions || (_relevant_obstacles[k].size() != num_prev_obst); + } + + // update current dt + _current_dt = grid->getFirstDt(); // TODO(roesmann): this does currently not work with non-uniform grids + + // setup control deviation constraint + int num_du_lb_finite = _du_lb.size() > 0 ? (_du_lb.array() > -corbo::CORBO_INF_DBL).count() : 0; + int num_du_ub_finite = _du_ub.size() > 0 ? (_du_ub.array() < corbo::CORBO_INF_DBL).count() : 0; + if (num_du_lb_finite != _num_du_lb_finite) new_dimensions = true; + if (num_du_ub_finite != _num_du_ub_finite) new_dimensions = true; + _num_du_lb_finite = num_du_lb_finite; + _num_du_ub_finite = num_du_ub_finite; + + // return true if structure requires a changed + return new_dimensions; +} + +void StageInequalitySE2::computeNonIntegralStateTerm(int k, const Eigen::Ref& x_k, Eigen::Ref cost) const +{ + assert(_obstacles); + assert(k < _relevant_obstacles.size()); + assert(cost.size() == _relevant_obstacles[k].size()); + + // TODO(roesmann): Overload robot fooprint model functions in teb_local_planner to avoid this copy: + teb_local_planner::PoseSE2 pose(x_k[0], x_k[1], x_k[2]); + for (int i = 0; i < (int)_relevant_obstacles[k].size(); ++i) + { + if (!_enable_dynamic_obstacles || !_relevant_obstacles[k][i]->isDynamic()) + { + cost[i] = _min_obstacle_dist - _robot_model->calculateDistance(pose, _relevant_obstacles[k][i].get()); + } + else + { + cost[i] = + _min_obstacle_dist - _robot_model->estimateSpatioTemporalDistance(pose, _relevant_obstacles[k][i].get(), (double)k * _current_dt); + } + } +} + +void StageInequalitySE2::computeNonIntegralControlDeviationTerm(int k, const Eigen::Ref& u_k, + const Eigen::Ref& u_prev, double dt_prev, + Eigen::Ref cost) const +{ + assert(cost.size() == _num_du_lb_finite + _num_du_ub_finite); + if (cost.size() == 0) return; // TODO(roesmann): necessary? + if (k == 0 && dt_prev == 0) + { + cost.setZero(); // this is fine as dt_prev for k==0 is not subject to optimization + return; + } + + assert(_num_du_lb_finite == 0 || _du_lb.size() == u_k.size()); + assert(_num_du_ub_finite == 0 || _du_ub.size() == u_k.size()); + assert(dt_prev != 0); + + int idx_lb = 0; + int idx_ub = 0; + for (int i = 0; i < u_k.size(); ++i) + { + if (_du_lb[i] > -corbo::CORBO_INF_DBL) + { + cost[idx_lb] = _du_lb[i] - (u_k[i] - u_prev[i]) / dt_prev; + ++idx_lb; + } + if (_du_ub[i] < corbo::CORBO_INF_DBL) + { + cost[_num_du_lb_finite + idx_ub] = (u_k[i] - u_prev[i]) / dt_prev - _du_ub[i]; + ++idx_ub; + } + } +} + +void StageInequalitySE2::setControlDeviationBounds(const Eigen::VectorXd& du_lb, const Eigen::VectorXd& du_ub) +{ + _du_lb = du_lb; + _du_ub = du_ub; +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/test_mpc_optim_node.cpp b/mpc_local_planner/src/test_mpc_optim_node.cpp new file mode 100644 index 0000000..135e53c --- /dev/null +++ b/mpc_local_planner/src/test_mpc_optim_node.cpp @@ -0,0 +1,262 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include + +namespace mpc = mpc_local_planner; + +class TestMpcOptimNode +{ + public: + TestMpcOptimNode() = default; + + void start(ros::NodeHandle& nh); + + protected: + void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, + interactive_markers::InteractiveMarkerServer* marker_server); + void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg); + void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg); + void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg); + + teb_local_planner::ObstContainer _obstacles; + int _no_fixed_obstacles; + std::vector _via_points; +}; + +void TestMpcOptimNode::start(ros::NodeHandle& nh) +{ + std::string map_frame = "map"; + + // interactive marker server for simulated dynamic obstacles + interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles"); + + // configure obstacles + _obstacles.push_back(boost::make_shared(-3, 1)); + _obstacles.push_back(boost::make_shared(6, 2)); + _obstacles.push_back(boost::make_shared(4, 0.1)); + + // Add interactive markers + for (int i = 0; i < (int)_obstacles.size(); ++i) + { + // Add interactive markers for all point obstacles + boost::shared_ptr pobst = boost::dynamic_pointer_cast(_obstacles[i]); + if (pobst) + { + CreateInteractiveMarker(pobst->x(), pobst->y(), i, map_frame, &marker_server); + } + } + marker_server.applyChanges(); + _no_fixed_obstacles = (int)_obstacles.size(); + + // setup callback for custom obstacles + ros::Subscriber custom_obst_sub = nh.subscribe("obstacles", 1, &TestMpcOptimNode::CB_customObstacle, this); + + // setup callback for clicked points (in rviz) that are considered as via-points + ros::Subscriber clicked_points_sub = nh.subscribe("/clicked_point", 5, &TestMpcOptimNode::CB_clicked_points, this); + + // setup callback for via-points (callback overwrites previously set via-points) + ros::Subscriber via_points_sub = nh.subscribe("via_points", 1, &TestMpcOptimNode::CB_via_points, this); + + // Setup robot shape model + teb_local_planner::RobotFootprintModelPtr robot_model = mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer(nh); + + mpc_local_planner::Controller controller; + if (!controller.configure(nh, _obstacles, robot_model, _via_points)) + { + ROS_ERROR("Controller configuration failed."); + return; + } + + mpc::Publisher publisher(nh, controller.getRobotDynamics(), map_frame); + + teb_local_planner::PoseSE2 x0(0, 0, 0); + teb_local_planner::PoseSE2 xf(5, 2, 0); + + corbo::TimeSeries::Ptr x_seq = std::make_shared(); + corbo::TimeSeries::Ptr u_seq = std::make_shared(); + + geometry_msgs::Twist vel; + + bool success = false; + + ros::Rate rate(20); + while (ros::ok()) + { + success = controller.step(x0, xf, vel, rate.expectedCycleTime().toSec(), ros::Time::now(), u_seq, x_seq); + + if (success) + publisher.publishLocalPlan(*x_seq); + else + ROS_ERROR("OCP solving failed."); + + publisher.publishObstacles(_obstacles); + publisher.publishRobotFootprintModel(x0, *robot_model); + publisher.publishViaPoints(_via_points); + ros::spinOnce(); + rate.sleep(); + } +} + +void TestMpcOptimNode::CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, + interactive_markers::InteractiveMarkerServer* marker_server) +{ + // create an interactive marker for our server + visualization_msgs::InteractiveMarker i_marker; + i_marker.header.frame_id = frame; + i_marker.header.stamp = ros::Time::now(); + std::ostringstream oss; + // oss << "obstacle" << id; + oss << id; + i_marker.name = oss.str(); + i_marker.description = "Obstacle"; + i_marker.pose.position.x = init_x; + i_marker.pose.position.y = init_y; + i_marker.pose.orientation.w = 1.0f; // make quaternion normalized + + // create a grey box marker + visualization_msgs::Marker box_marker; + box_marker.type = visualization_msgs::Marker::CUBE; + box_marker.id = id; + box_marker.scale.x = 0.2; + box_marker.scale.y = 0.2; + box_marker.scale.z = 0.2; + box_marker.color.r = 0.5; + box_marker.color.g = 0.5; + box_marker.color.b = 0.5; + box_marker.color.a = 1.0; + box_marker.pose.orientation.w = 1.0f; // make quaternion normalized + + // create a non-interactive control which contains the box + visualization_msgs::InteractiveMarkerControl box_control; + box_control.always_visible = true; + box_control.markers.push_back(box_marker); + + // add the control to the interactive marker + i_marker.controls.push_back(box_control); + + // create a control which will move the box, rviz will insert 2 arrows + visualization_msgs::InteractiveMarkerControl move_control; + move_control.name = "move_x"; + move_control.orientation.w = 0.707107f; + move_control.orientation.x = 0; + move_control.orientation.y = 0.707107f; + move_control.orientation.z = 0; + move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + + // add the control to the interactive marker + i_marker.controls.push_back(move_control); + + // add the interactive marker to our collection + marker_server->insert(i_marker); + marker_server->setCallback(i_marker.name, boost::bind(&TestMpcOptimNode::CB_obstacle_marker, this, boost::placeholders::_1)); +} + +void TestMpcOptimNode::CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) +{ + std::stringstream ss(feedback->marker_name); + unsigned int index; + ss >> index; + + if (index >= _no_fixed_obstacles) return; + teb_local_planner::PointObstacle* pobst = static_cast(_obstacles.at(index).get()); + pobst->position() = Eigen::Vector2d(feedback->pose.position.x, feedback->pose.position.y); +} + +void TestMpcOptimNode::CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg) +{ + // resize such that the vector contains only the fixed obstacles specified inside the main function + _obstacles.resize(_no_fixed_obstacles); + + // Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame) + for (size_t i = 0; i < obst_msg->obstacles.size(); ++i) + { + if (obst_msg->obstacles.at(i).polygon.points.size() == 1) + { + if (obst_msg->obstacles.at(i).radius == 0) + { + _obstacles.push_back(teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle( + obst_msg->obstacles.at(i).polygon.points.front().x, obst_msg->obstacles.at(i).polygon.points.front().y))); + } + else + { + _obstacles.push_back(teb_local_planner::ObstaclePtr( + new teb_local_planner::CircularObstacle(obst_msg->obstacles.at(i).polygon.points.front().x, + obst_msg->obstacles.at(i).polygon.points.front().y, obst_msg->obstacles.at(i).radius))); + } + } + else + { + teb_local_planner::PolygonObstacle* polyobst = new teb_local_planner::PolygonObstacle; + for (size_t j = 0; j < obst_msg->obstacles.at(i).polygon.points.size(); ++j) + { + polyobst->pushBackVertex(obst_msg->obstacles.at(i).polygon.points[j].x, obst_msg->obstacles.at(i).polygon.points[j].y); + } + polyobst->finalizePolygon(); + _obstacles.push_back(teb_local_planner::ObstaclePtr(polyobst)); + } + + if (!_obstacles.empty()) _obstacles.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation); + } +} + +void TestMpcOptimNode::CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg) +{ + // we assume for simplicity that the fixed frame is already the map/planning frame + // consider clicked points as via-points + _via_points.emplace_back(point_msg->point.x, point_msg->point.y, 0.0); + ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added."); +} + +void TestMpcOptimNode::CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg) +{ + ROS_INFO_ONCE("Via-points received. This message is printed once."); + _via_points.clear(); + for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses) + { + _via_points.emplace_back(pose.pose.position.x, pose.pose.position.y, 0); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "test_optim_node"); + ros::NodeHandle n("~"); + + TestMpcOptimNode mpc_test; + mpc_test.start(n); + + return 0; +} diff --git a/mpc_local_planner/src/utils/conversion.cpp b/mpc_local_planner/src/utils/conversion.cpp new file mode 100644 index 0000000..6989427 --- /dev/null +++ b/mpc_local_planner/src/utils/conversion.cpp @@ -0,0 +1,48 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +namespace mpc_local_planner { + +void convert(const corbo::TimeSeries& time_series, const RobotDynamicsInterface& dynamics, std::vector& poses_stamped, + const std::string& frame_id) +{ + poses_stamped.clear(); + + if (time_series.isEmpty()) return; + + for (int i = 0; i < time_series.getTimeDimension(); ++i) + { + poses_stamped.emplace_back(); + + double theta = 0; + dynamics.getPoseSE2FromState(time_series.getValuesMap(i), poses_stamped.back().pose.position.x, poses_stamped.back().pose.position.y, theta); + poses_stamped.back().pose.orientation = tf::createQuaternionMsgFromYaw(theta); + poses_stamped.back().header.frame_id = frame_id; + poses_stamped.back().header.stamp = ros::Time::now(); // TODO(roesmann) should we use now()? + } +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/utils/publisher.cpp b/mpc_local_planner/src/utils/publisher.cpp new file mode 100644 index 0000000..b8e3d0a --- /dev/null +++ b/mpc_local_planner/src/utils/publisher.cpp @@ -0,0 +1,276 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace mpc_local_planner { + +Publisher::Publisher(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame) { initialize(nh, system, map_frame); } + +void Publisher::initialize(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame) +{ + if (_initialized) ROS_WARN("mpc_local_planner: Publisher already initialized. Reinitalizing..."); + + _system = system; + _map_frame = map_frame; + + // register topics + _global_plan_pub = nh.advertise("global_plan", 1); + _local_plan_pub = nh.advertise("local_plan", 1); + _mpc_marker_pub = nh.advertise("mpc_markers", 1000); + + _initialized = true; +} + +void Publisher::publishLocalPlan(const std::vector& local_plan) const +{ + if (!_initialized) return; + base_local_planner::publishPlan(local_plan, _local_plan_pub); +} + +void Publisher::publishLocalPlan(const corbo::TimeSeries& ts) const +{ + if (!_initialized) return; + if (!_system) + { + ROS_ERROR("Publisher::publishLocalPlan(): cannot publish since the system class is not provided."); + return; + } + + std::vector local_plan; + convert(ts, *_system, local_plan, _map_frame); + base_local_planner::publishPlan(local_plan, _local_plan_pub); +} + +void Publisher::publishGlobalPlan(const std::vector& global_plan) const +{ + if (!_initialized) return; + base_local_planner::publishPlan(global_plan, _global_plan_pub); +} + +void Publisher::publishRobotFootprintModel(const teb_local_planner::PoseSE2& current_pose, + const teb_local_planner::BaseRobotFootprintModel& robot_model, const std::string& ns, + const std_msgs::ColorRGBA& color) +{ + if (!_initialized) return; + + std::vector markers; + robot_model.visualizeRobot(current_pose, markers, color); + if (markers.empty()) return; + + int idx = 1000000; // avoid overshadowing by obstacles + for (visualization_msgs::Marker& marker : markers) + { + marker.header.frame_id = _map_frame; + marker.header.stamp = ros::Time::now(); + marker.action = visualization_msgs::Marker::ADD; + marker.ns = ns; + marker.id = idx++; + marker.lifetime = ros::Duration(2.0); + _mpc_marker_pub.publish(marker); + } +} + +void Publisher::publishObstacles(const teb_local_planner::ObstContainer& obstacles) const +{ + if (obstacles.empty() || !_initialized) return; + + // Visualize point obstacles + { + visualization_msgs::Marker marker; + marker.header.frame_id = _map_frame; + marker.header.stamp = ros::Time::now(); + marker.ns = "PointObstacles"; + marker.id = 0; + marker.type = visualization_msgs::Marker::POINTS; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(2.0); + + for (const ObstaclePtr& obst : obstacles) + { + // std::shared_ptr pobst = std::dynamic_pointer_cast(obst); // TODO(roesmann): change teb_local_planner + // types to std lib + boost::shared_ptr pobst = boost::dynamic_pointer_cast(obst); + if (!pobst) continue; + + geometry_msgs::Point point; + point.x = pobst->x(); + point.y = pobst->y(); + point.z = 0; + marker.points.push_back(point); + } + + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + + _mpc_marker_pub.publish(marker); + } + + // Visualize line obstacles + { + int idx = 0; + for (const ObstaclePtr& obst : obstacles) + { + // LineObstacle::Ptr pobst = std::dynamic_pointer_cast(obst); + boost::shared_ptr pobst = boost::dynamic_pointer_cast(obst); + if (!pobst) continue; + + visualization_msgs::Marker marker; + marker.header.frame_id = _map_frame; + marker.header.stamp = ros::Time::now(); + marker.ns = "LineObstacles"; + marker.id = idx++; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(2.0); + geometry_msgs::Point start; + start.x = pobst->start().x(); + start.y = pobst->start().y(); + start.z = 0; + marker.points.push_back(start); + geometry_msgs::Point end; + end.x = pobst->end().x(); + end.y = pobst->end().y(); + end.z = 0; + marker.points.push_back(end); + + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + + _mpc_marker_pub.publish(marker); + } + } + + // Visualize polygon obstacles + { + int idx = 0; + for (const ObstaclePtr& obst : obstacles) + { + // PolygonObstacle::Ptr pobst = std::dynamic_pointer_cast(obst); + boost::shared_ptr pobst = boost::dynamic_pointer_cast(obst); + if (!pobst) continue; + + visualization_msgs::Marker marker; + marker.header.frame_id = _map_frame; + marker.header.stamp = ros::Time::now(); + marker.ns = "PolyObstacles"; + marker.id = idx++; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(2.0); + + for (Point2dContainer::const_iterator vertex = pobst->vertices().begin(); vertex != pobst->vertices().end(); ++vertex) + { + geometry_msgs::Point point; + point.x = vertex->x(); + point.y = vertex->y(); + point.z = 0; + marker.points.push_back(point); + } + + // Also add last point to close the polygon + // but only if polygon has more than 2 points (it is not a line) + if (pobst->vertices().size() > 2) + { + geometry_msgs::Point point; + point.x = pobst->vertices().front().x(); + point.y = pobst->vertices().front().y(); + point.z = 0; + marker.points.push_back(point); + } + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + + _mpc_marker_pub.publish(marker); + } + } +} + +void Publisher::publishViaPoints(const std::vector& via_points, const std::string& ns) const +{ + if (via_points.empty() || !_initialized) return; + + visualization_msgs::Marker marker; + marker.header.frame_id = _map_frame; + marker.header.stamp = ros::Time::now(); + marker.ns = ns; + marker.id = 55555; + marker.type = visualization_msgs::Marker::POINTS; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(2.0); + + for (const teb_local_planner::PoseSE2& via_point : via_points) + { + geometry_msgs::Point point; + point.x = via_point.x(); + point.y = via_point.y(); + point.z = 0; + marker.points.push_back(point); + } + + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + + _mpc_marker_pub.publish(marker); +} + +std_msgs::ColorRGBA Publisher::toColorMsg(float a, float r, float g, float b) +{ + std_msgs::ColorRGBA color; + color.a = a; + color.r = r; + color.g = g; + color.b = b; + return color; +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner/src/utils/time_series_se2.cpp b/mpc_local_planner/src/utils/time_series_se2.cpp new file mode 100644 index 0000000..358ea86 --- /dev/null +++ b/mpc_local_planner/src/utils/time_series_se2.cpp @@ -0,0 +1,131 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include + +#include +#include + +namespace mpc_local_planner { + +bool TimeSeriesSE2::getValuesInterpolate(double time, Eigen::Ref values, Interpolation interpolation, Extrapolation extrapolation, + double tolerance) const +{ + if (_time.empty()) return false; + + auto it = std::find_if(_time.begin(), _time.end(), [time](double val) { return val >= time; }); // find next time interval + if (it == _time.end()) + { + switch (extrapolation) + { + case Extrapolation::NoExtrapolation: + { + break; + } + case Extrapolation::ZeroOrderHold: + { + values = getValuesMap(getTimeDimension() - 1); + return true; + } + default: + { + PRINT_ERROR("TimeSeries::valuesInterpolate(): desired extrapolation method not implemented."); + return false; + } + } + } + // interpolate + // int n = (int)_time.size(); + int idx = (int)std::distance(_time.begin(), it); + if (idx >= getTimeDimension()) idx = getTimeDimension() - 1; // this might occure with floating point comparison + if (std::abs(time - _time[idx]) < tolerance) + { + // we are close to the next value, in particular idx + values = getValuesMap(idx); + return true; + } + if (idx < 1) + { + PRINT_ERROR("accessing a time idx in the past which is not this time series"); + } + // okay, so now we have the above case val > time -> so idx corresponds to the next sample. + double dt = time - _time[idx - 1]; + PRINT_ERROR_COND_NAMED(dt < 0, "dt < 0 in interpolation. This cannot be correct."); + + switch (interpolation) + { + case Interpolation::ZeroOrderHold: + { + if (idx < 1) idx = 1; + values = getValuesMap(idx - 1); // since dt > 0 -> we use the previous value + break; + } + case Interpolation::Linear: + { + if (idx < 1) + { + values = getValuesMap(0); + } + else + { + double dt_data = _time[idx] - _time[idx - 1]; + double dt_frac = dt / dt_data; + values.noalias() = getValuesMap(idx - 1) + dt_frac * (getValuesMap(idx) - getValuesMap(idx - 1)); + // overwrite third component with is an angle in [-pi, pi) + if (values.size() > 2) + { + values[2] = interpolate_angle(getValuesMap(idx - 1)[2], getValuesMap(idx)[2], dt_frac); + } + } + break; + } + default: + { + PRINT_ERROR("TimeSeries::valuesInterpolate(): desired interpolation method not implemented."); + return false; + } + } + return true; +} + +double TimeSeriesSE2::computeMeanOverall() +{ + PRINT_ERROR_NAMED("SE2 version not yet implemented."); + return getValuesMatrixView().mean(); +} + +void TimeSeriesSE2::computeMeanCwise(Eigen::Ref mean_values) +{ + PRINT_ERROR_NAMED("SE2 version Not yet implemented."); + if (mean_values.size() != getValueDimension()) + { + PRINT_ERROR("TimeSeries::computeMeanCwise(): provided mean_values vector does not match value dimension"); + return; + } + mean_values = getValuesMatrixView().rowwise().mean(); +} + +} // namespace mpc_local_planner diff --git a/mpc_local_planner_examples/CMakeLists.txt b/mpc_local_planner_examples/CMakeLists.txt new file mode 100644 index 0000000..dc04fc7 --- /dev/null +++ b/mpc_local_planner_examples/CMakeLists.txt @@ -0,0 +1,179 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mpc_local_planner_examples) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES teb_local_planner_tutorials +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) + +## Declare a C++ library +# add_library(teb_local_planner_tutorials +# src/${PROJECT_NAME}/teb_local_planner_tutorials.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(teb_local_planner_tutorials ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(teb_local_planner_tutorials_node src/teb_local_planner_tutorials_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(teb_local_planner_tutorials_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(teb_local_planner_tutorials_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS teb_local_planner_tutorials teb_local_planner_tutorials_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +install(DIRECTORY + launch cfg maps stage + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_teb_local_planner_tutorials.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/mpc_local_planner_examples/README.md b/mpc_local_planner_examples/README.md new file mode 100644 index 0000000..bae8f7f --- /dev/null +++ b/mpc_local_planner_examples/README.md @@ -0,0 +1,10 @@ +# mpc_local_planner_tutorials +This package contains supplementary material and examples for the [mpc_local_planner](http://wiki.ros.org/mpc_local_planner). + +The examples package mainly contains fully working robot navigation configurations in combination with the *mpc_local_planner*. + +**Dependencies:** + + * *navigation stack* and *mpc_local_planner* package + * *stage*: `sudo apt-get install ros-melodic-stage-ros` + diff --git a/mpc_local_planner_examples/cfg/amcl_params.yaml b/mpc_local_planner_examples/cfg/amcl_params.yaml new file mode 100644 index 0000000..5035266 --- /dev/null +++ b/mpc_local_planner_examples/cfg/amcl_params.yaml @@ -0,0 +1,38 @@ +use_map_topic: true + +odom_frame_id: "odom" +base_frame_id: "base_footprint" +global_frame_id: "map" + +## Publish scans from best pose at a max of 10 Hz +odom_model_type: "diff" +odom_alpha5: 0.1 +gui_publish_rate: 10.0 +laser_max_beams: 60 +laser_max_range: 12.0 +min_particles: 500 +max_particles: 2000 +kld_err: 0.05 +kld_z: 0.99 +odom_alpha1: 0.2 +odom_alpha2: 0.2 +## translation std dev, m +odom_alpha3: 0.2 +odom_alpha4: 0.2 +laser_z_hit: 0.5 +aser_z_short: 0.05 +laser_z_max: 0.05 +laser_z_rand: 0.5 +laser_sigma_hit: 0.2 +laser_lambda_short: 0.1 +laser_model_type: "likelihood_field" # "likelihood_field" or "beam" +laser_likelihood_max_dist: 2.0 +update_min_d: 0.25 +update_min_a: 0.2 + +resample_interval: 1 + +## Increase tolerance because the computer can get quite busy +transform_tolerance: 1.0 +recovery_alpha_slow: 0.001 +recovery_alpha_fast: 0.1 diff --git a/mpc_local_planner_examples/cfg/carlike/costmap_common_params.yaml b/mpc_local_planner_examples/cfg/carlike/costmap_common_params.yaml new file mode 100644 index 0000000..f863313 --- /dev/null +++ b/mpc_local_planner_examples/cfg/carlike/costmap_common_params.yaml @@ -0,0 +1,28 @@ + +#---standard pioneer footprint--- +#---(in meters)--- +#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] ] +footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ] + +transform_tolerance: 0.2 + +obstacle_layer: + enabled: true + obstacle_range: 3.0 + raytrace_range: 3.5 + inflation_radius: 0.2 + track_unknown_space: false + combination_method: 1 + + observation_sources: laser_scan_sensor + laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true} + + +inflation_layer: + enabled: true + cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) + inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. + +static_layer: + enabled: true + map_topic: "/map" diff --git a/mpc_local_planner_examples/cfg/carlike/global_costmap_params.yaml b/mpc_local_planner_examples/cfg/carlike/global_costmap_params.yaml new file mode 100644 index 0000000..ef4a230 --- /dev/null +++ b/mpc_local_planner_examples/cfg/carlike/global_costmap_params.yaml @@ -0,0 +1,15 @@ +global_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 1.0 + publish_frequency: 0.5 + + transform_tolerance: 0.5 + plugins: + - {name: static_layer, type: "costmap_2d::StaticLayer"} + - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} + + + + diff --git a/mpc_local_planner_examples/cfg/carlike/local_costmap_params.yaml b/mpc_local_planner_examples/cfg/carlike/local_costmap_params.yaml new file mode 100644 index 0000000..0cd41cf --- /dev/null +++ b/mpc_local_planner_examples/cfg/carlike/local_costmap_params.yaml @@ -0,0 +1,14 @@ +local_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 5.0 + publish_frequency: 2.0 + rolling_window: true + width: 5.5 + height: 5.5 + resolution: 0.1 + transform_tolerance: 0.5 + + plugins: + - {name: static_layer, type: "costmap_2d::StaticLayer"} + - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} diff --git a/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml b/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml new file mode 100644 index 0000000..656e4a1 --- /dev/null +++ b/mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml @@ -0,0 +1,104 @@ +MpcLocalPlannerROS: + + odom_topic: odom + + ## Robot settings + robot: + type: "simple_car" + simple_car: + wheelbase: 0.4 + front_wheel_driving: False + max_vel_x: 0.4 + max_vel_x_backwards: 0.2 + max_steering_angle: 1.4 + acc_lim_x: 0.5 # deactive bounds with zero + dec_lim_x: 0.5 # deactive bounds with zero + max_steering_rate: 0.5 # deactive bounds with zero + + ## Footprint model for collision avoidance + footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" + type: "line" + radius: 0.2 # for type "circular" + line_start: [0.0, 0.0] # for type "line" + line_end: [0.4, 0.0] # for type "line" + front_offset: 0.2 # for type "two_circles" + front_radius: 0.2 # for type "two_circles" + rear_offset: 0.2 # for type "two_circles" + rear_radius: 0.2 # for type "two_circles" + 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" + is_footprint_dynamic: False + + + ## Collision avoidance + collision_avoidance: + min_obstacle_dist: 0.25 # Note, this parameter must be chosen w.r.t. the footprint_model + enable_dynamic_obstacles: False + force_inclusion_factor: 1.5 + cutoff_factor: 5 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.0 + + ## Planning grid + grid: + type: "fd_grid" + grid_size_ref: 20 + dt_ref: 0.3 + xf_fixed: [True, True, True] + warm_start: True + collocation_method: "forward_differences" + cost_integration_method: "left_sum" + variable_grid: + enable: True + min_dt: 0.0; + max_dt: 10.0; + grid_adaptation: + enable: True + dt_hyst_ratio: 0.1 + min_grid_size: 2 + max_grid_size: 50 + + ## Planning options + planning: + objective: + type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly + terminal_cost: + type: "none" + terminal_constraint: + type: "none" + + ## Controller options + controller: + outer_ocp_iterations: 1 + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.1 + global_plan_overwrite_orientation: True + global_plan_prune_distance: 1.0 + allow_init_with_backward_motion: True + max_global_plan_lookahead_dist: 1.5 + force_reinit_new_goal_dist: 1.0 + force_reinit_new_goal_angular: 1.57 + prefer_x_feedback: False + publish_ocp_results: False + + ## Solver settings + solver: + type: "ipopt" + ipopt: + iterations: 100 + max_cpu_time: -1.0 + ipopt_numeric_options: + tol: 1e-4 + ipopt_string_options: + linear_solver: "mumps" + hessian_approximation: "limited-memory" # exact/limited-memory, WARNING 'exact' does currently not work well with the carlike model + lsq_lm: + iterations: 10 + weight_init_eq: 2 + weight_init_ineq: 2 + weight_init_bounds: 2 + weight_adapt_factor_eq: 1.5 + weight_adapt_factor_ineq: 1.5 + weight_adapt_factor_bounds: 1.5 + weight_adapt_max_eq: 500 + weight_adapt_max_ineq: 500 + weight_adapt_max_bounds: 500 diff --git a/mpc_local_planner_examples/cfg/diff_drive/costmap_common_params.yaml b/mpc_local_planner_examples/cfg/diff_drive/costmap_common_params.yaml new file mode 100644 index 0000000..41d52e6 --- /dev/null +++ b/mpc_local_planner_examples/cfg/diff_drive/costmap_common_params.yaml @@ -0,0 +1,30 @@ + +#---standard pioneer footprint--- +#---(in meters)--- +robot_radius: 0.17 +footprint_padding: 0.00 + +transform_tolerance: 0.2 + +always_send_full_costmap: true + +obstacle_layer: + enabled: true + obstacle_range: 3.0 + raytrace_range: 4.0 + inflation_radius: 0.2 + track_unknown_space: true + combination_method: 1 + + observation_sources: laser_scan_sensor + laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true} + + +inflation_layer: + enabled: true + cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) + inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. + +static_layer: + enabled: true + map_topic: "/map" diff --git a/mpc_local_planner_examples/cfg/diff_drive/costmap_converter_params.yaml b/mpc_local_planner_examples/cfg/diff_drive/costmap_converter_params.yaml new file mode 100644 index 0000000..b472791 --- /dev/null +++ b/mpc_local_planner_examples/cfg/diff_drive/costmap_converter_params.yaml @@ -0,0 +1,31 @@ +########################################################################################### +## NOTE: Costmap conversion is experimental. Its purpose is to combine many point ## +## obstales into clusters, computed in a separate thread in order to improve the overall ## +## efficiency of local planning. However, the implemented conversion algorithms are in a ## +## very early stage of development. Contributions are welcome! ## +########################################################################################### + +MpcLocalPlannerROS: + + ## Costmap converter plugin + #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" + costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC" + #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH" + #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull" + costmap_converter_spin_thread: True + costmap_converter_rate: 5 + + + ## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons) + ## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH + costmap_converter/CostmapToLinesDBSRANSAC: + cluster_max_distance: 0.4 + cluster_min_pts: 2 + ransac_inlier_distance: 0.15 + ransac_min_inliers: 10 + ransac_no_iterations: 1500 + ransac_remainig_outliers: 3 + ransac_convert_outlier_pts: True + ransac_filter_remaining_outlier_pts: False + convex_hull_min_pt_separation: 0.1 + diff --git a/mpc_local_planner_examples/cfg/diff_drive/global_costmap_params.yaml b/mpc_local_planner_examples/cfg/diff_drive/global_costmap_params.yaml new file mode 100644 index 0000000..ef4a230 --- /dev/null +++ b/mpc_local_planner_examples/cfg/diff_drive/global_costmap_params.yaml @@ -0,0 +1,15 @@ +global_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 1.0 + publish_frequency: 0.5 + + transform_tolerance: 0.5 + plugins: + - {name: static_layer, type: "costmap_2d::StaticLayer"} + - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} + + + + diff --git a/mpc_local_planner_examples/cfg/diff_drive/local_costmap_params.yaml b/mpc_local_planner_examples/cfg/diff_drive/local_costmap_params.yaml new file mode 100644 index 0000000..0cd41cf --- /dev/null +++ b/mpc_local_planner_examples/cfg/diff_drive/local_costmap_params.yaml @@ -0,0 +1,14 @@ +local_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 5.0 + publish_frequency: 2.0 + rolling_window: true + width: 5.5 + height: 5.5 + resolution: 0.1 + transform_tolerance: 0.5 + + plugins: + - {name: static_layer, type: "costmap_2d::StaticLayer"} + - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} diff --git a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml new file mode 100644 index 0000000..b1f6369 --- /dev/null +++ b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml @@ -0,0 +1,108 @@ +MpcLocalPlannerROS: + + odom_topic: odom + + ## Robot settings + robot: + type: "unicycle" + unicycle: + max_vel_x: 0.4 + max_vel_x_backwards: 0.2 + max_vel_theta: 0.3 + acc_lim_x: 0.2 # deactive bounds with zero + dec_lim_x: 0.2 # deactive bounds with zero + acc_lim_theta: 0.2 # deactivate bounds with zero + + ## Footprint model for collision avoidance + footprint_model: + type: "point" + is_footprint_dynamic: False + + ## Collision avoidance + collision_avoidance: + min_obstacle_dist: 0.2 # Note, this parameter must be chosen w.r.t. the footprint_model + enable_dynamic_obstacles: False + force_inclusion_factor: 1.5 + cutoff_factor: 5 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.5 + + ## Planning grid + grid: + type: "fd_grid" + grid_size_ref: 20 + dt_ref: 0.3 + xf_fixed: [True, True, True] + warm_start: True + collocation_method: "forward_differences" + cost_integration_method: "left_sum" + variable_grid: + enable: True + min_dt: 0.0; + max_dt: 10.0; + grid_adaptation: + enable: True + dt_hyst_ratio: 0.1 + min_grid_size: 2 + max_grid_size: 50 + + ## Planning options + planning: + objective: + type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly + quadratic_form: + state_weights: [2.0, 2.0, 2.0] + control_weights: [1.0, 1.0] + integral_form: False + minimum_time_via_points: + position_weight: 10.5 + orientation_weight: 0.0 + via_points_ordered: False + terminal_cost: + type: "quadratic" # can be "none" + quadratic: + final_state_weights: [2.0, 2.0, 2.0] + terminal_constraint: + type: "none" # can be "none" + l2_ball: + weight_matrix: [1.0, 1.0, 1.0] + radius: 5 + + ## Controller options + controller: + outer_ocp_iterations: 1 + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.1 + global_plan_overwrite_orientation: true + global_plan_prune_distance: 1.0 + allow_init_with_backward_motion: True + max_global_plan_lookahead_dist: 1.5 + global_plan_viapoint_sep: 0.5 + force_reinit_new_goal_dist: 1.0 + force_reinit_new_goal_angular: 1.57 + force_reinit_num_steps: 0 + prefer_x_feedback: False + publish_ocp_results: False + + ## Solver settings + solver: + type: "ipopt" + ipopt: + iterations: 100 + max_cpu_time: -1.0 + ipopt_numeric_options: + tol: 1e-4 + ipopt_string_options: + linear_solver: "mumps" + hessian_approximation: "exact" # exact or limited-memory + lsq_lm: + iterations: 10 + weight_init_eq: 2 + weight_init_ineq: 2 + weight_init_bounds: 2 + weight_adapt_factor_eq: 1.5 + weight_adapt_factor_ineq: 1.5 + weight_adapt_factor_bounds: 1.5 + weight_adapt_max_eq: 500 + weight_adapt_max_ineq: 500 + weight_adapt_max_bounds: 500 diff --git a/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml new file mode 100644 index 0000000..4b58ba5 --- /dev/null +++ b/mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml @@ -0,0 +1,103 @@ +MpcLocalPlannerROS: + + odom_topic: odom + + ## Robot settings + robot: + type: "unicycle" + unicycle: + max_vel_x: 0.4 + max_vel_x_backwards: 0.2 + max_vel_theta: 0.3 + acc_lim_x: 0.2 # deactive bounds with zero + dec_lim_x: 0.2 # deactive bounds with zero + acc_lim_theta: 0.2 # deactivate bounds with zero + + ## Footprint model for collision avoidance + footprint_model: + type: "point" + is_footprint_dynamic: False + + ## Collision avoidance + collision_avoidance: + min_obstacle_dist: 0.2 # Note, this parameter must be chosen w.r.t. the footprint_model + enable_dynamic_obstacles: False + force_inclusion_factor: 1.5 + cutoff_factor: 5 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.5 + + ## Planning grid + grid: + type: "fd_grid" + grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist + dt_ref: 0.3 # and here the corresponding temporal resolution + xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below + warm_start: True + collocation_method: "forward_differences" + cost_integration_method: "left_sum" + variable_grid: + enable: False # We want a fixed grid + min_dt: 0.0; + max_dt: 10.0; + grid_adaptation: + enable: True + dt_hyst_ratio: 0.1 + min_grid_size: 2 + max_grid_size: 50 + + ## Planning options + planning: + objective: + type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly + quadratic_form: + state_weights: [2.0, 2.0, 0.25] + control_weights: [0.1, 0.05] + integral_form: False + terminal_cost: + type: "quadratic" # can be "none" + quadratic: + final_state_weights: [10.0, 10.0, 0.5] + terminal_constraint: + type: "none" # can be "none" + l2_ball: + weight_matrix: [1.0, 1.0, 1.0] + radius: 5 + + ## Controller options + controller: + outer_ocp_iterations: 1 + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 0.1 + global_plan_overwrite_orientation: true + global_plan_prune_distance: 1.0 + allow_init_with_backward_motion: True + max_global_plan_lookahead_dist: 1.0 # Check horizon length + force_reinit_new_goal_dist: 1.0 + force_reinit_new_goal_angular: 1.57 + force_reinit_num_steps: 0 + prefer_x_feedback: False + publish_ocp_results: False + + ## Solver settings + solver: + type: "ipopt" + ipopt: + iterations: 100 + max_cpu_time: -1.0 + ipopt_numeric_options: + tol: 1e-4 + ipopt_string_options: + linear_solver: "mumps" + hessian_approximation: "exact" # exact or limited-memory + lsq_lm: + iterations: 10 + weight_init_eq: 2 + weight_init_ineq: 2 + weight_init_bounds: 2 + weight_adapt_factor_eq: 1.5 + weight_adapt_factor_ineq: 1.5 + weight_adapt_factor_bounds: 1.5 + weight_adapt_max_eq: 500 + weight_adapt_max_ineq: 500 + weight_adapt_max_bounds: 500 diff --git a/mpc_local_planner_examples/cfg/rviz_navigation.rviz b/mpc_local_planner_examples/cfg/rviz_navigation.rviz new file mode 100644 index 0000000..ac60e08 --- /dev/null +++ b/mpc_local_planner_examples/cfg/rviz_navigation.rviz @@ -0,0 +1,317 @@ +Panels: + - Class: rviz/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.6050959825515747 + Tree Height: 546 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 50 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /move_base/MpcLocalPlannerROS/mpc_markers + Name: MpcMarker + Namespaces: + PointObstacles: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.05000000074505806 + Head Length: 0.10000000149011612 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: LocalPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 164; 0; 0 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.02500000037252903 + Shaft Length: 0.10000000149011612 + Topic: /move_base/MpcLocalPlannerROS/local_plan + Unreliable: false + Value: true + - Alpha: 0.4000000059604645 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: LocalCostmap + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + Enabled: true + Name: Local Planner + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: GlobalPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/GlobalPlanner/plan + Unreliable: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: GlobalCostmap + Topic: /move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + Enabled: true + Name: Global Planner + - Class: rviz/Group + Displays: + - Alpha: 1 + Class: rviz/Polygon + Color: 85; 0; 255 + Enabled: true + Name: Robot Footprint + Topic: /move_base/local_costmap/footprint + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Arrow Length: 0.10000000149011612 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: AMCL Particles + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /particlecloud + Unreliable: false + Value: false + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /odom + Unreliable: false + Value: false + Enabled: true + Name: Robot + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 12.58323860168457 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 5.375269889831543 + Y: 5.046060085296631 + Z: -1.4013299942016602 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5648000240325928 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.710430145263672 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000363000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1215 + X: 705 + Y: 37 diff --git a/mpc_local_planner_examples/cfg/rviz_navigation_cc.rviz b/mpc_local_planner_examples/cfg/rviz_navigation_cc.rviz new file mode 100644 index 0000000..cb2402e --- /dev/null +++ b/mpc_local_planner_examples/cfg/rviz_navigation_cc.rviz @@ -0,0 +1,355 @@ +Panels: + - Class: rviz/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Local Planner1/TebPoses1 + - /Local Planner1/TebMarker1 + - /Local Planner1/LocalPath1 + - /Local Planner1/LocalCostmap1 + - /Global Planner1/GlobalPath1 + - /Global Planner1/GlobalCostmap1 + - /Robot1/Robot Footprint1 + - /Obstacle1 + - /Obstacle1/Footprint estimation1 + - /Obstacle1/Marker1 + Splitter Ratio: 0.6050959825515747 + Tree Height: 555 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 50 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: TebPoses + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /robot_0/move_base/TebLocalPlannerROS/teb_poses + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /robot_0/move_base/TebLocalPlannerROS/teb_markers + Name: TebMarker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: LocalPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /robot_0/move_base/TebLocalPlannerROS/local_plan + Unreliable: false + Value: true + - Alpha: 0.4000000059604645 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: LocalCostmap + Topic: /robot_0/move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + Enabled: true + Name: Local Planner + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: GlobalPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /robot_0/move_base/TebLocalPlannerROS/global_plan + Unreliable: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: GlobalCostmap + Topic: /robot_0/move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + Enabled: true + Name: Global Planner + - Class: rviz/Group + Displays: + - Alpha: 1 + Class: rviz/Polygon + Color: 85; 0; 255 + Enabled: true + Name: Robot Footprint + Topic: /robot_0/move_base/local_costmap/footprint + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /robot_0/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Arrow Length: 0.10000000149011612 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: AMCL Particles + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /robot_0/particlecloud + Unreliable: false + Value: false + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /robot_0/odom + Unreliable: false + Value: false + Enabled: true + Name: Robot + - Class: rviz/Group + Displays: + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint estimation + Topic: /costmap_polygons + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /standalone_converter/costmap_polygon_markers + Name: Marker + Namespaces: + Polygons: true + Queue Size: 100 + Value: true + Enabled: true + Name: Obstacle + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 8.012253761291504 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.049907684326172 + Y: 3.295278549194336 + Z: 8.21469497680664 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.715442657470703 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000176000002b6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000002b6000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c40000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf0000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000343000002b600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1215 + X: 705 + Y: 37 diff --git a/mpc_local_planner_examples/launch/carlike_minimum_time.launch b/mpc_local_planner_examples/launch/carlike_minimum_time.launch new file mode 100644 index 0000000..a98f828 --- /dev/null +++ b/mpc_local_planner_examples/launch/carlike_minimum_time.launch @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mpc_local_planner_examples/launch/diff_drive_minimum_time.launch b/mpc_local_planner_examples/launch/diff_drive_minimum_time.launch new file mode 100644 index 0000000..d5cccd9 --- /dev/null +++ b/mpc_local_planner_examples/launch/diff_drive_minimum_time.launch @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mpc_local_planner_examples/launch/diff_drive_minimum_time_costmap_conversion.launch b/mpc_local_planner_examples/launch/diff_drive_minimum_time_costmap_conversion.launch new file mode 100644 index 0000000..10cf8d2 --- /dev/null +++ b/mpc_local_planner_examples/launch/diff_drive_minimum_time_costmap_conversion.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mpc_local_planner_examples/launch/diff_drive_quadratic_form.launch b/mpc_local_planner_examples/launch/diff_drive_quadratic_form.launch new file mode 100644 index 0000000..fe40c0f --- /dev/null +++ b/mpc_local_planner_examples/launch/diff_drive_quadratic_form.launch @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mpc_local_planner_examples/maps/corridor.png b/mpc_local_planner_examples/maps/corridor.png new file mode 100644 index 0000000..05bcf9e Binary files /dev/null and b/mpc_local_planner_examples/maps/corridor.png differ diff --git a/mpc_local_planner_examples/maps/corridor.yaml b/mpc_local_planner_examples/maps/corridor.yaml new file mode 100644 index 0000000..7951e7c --- /dev/null +++ b/mpc_local_planner_examples/maps/corridor.yaml @@ -0,0 +1,7 @@ +image: corridor.png +resolution: 0.050000 +origin: [0.000000, 0.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/mpc_local_planner_examples/maps/empty_box.png b/mpc_local_planner_examples/maps/empty_box.png new file mode 100644 index 0000000..ba0568d Binary files /dev/null and b/mpc_local_planner_examples/maps/empty_box.png differ diff --git a/mpc_local_planner_examples/maps/empty_box.yaml b/mpc_local_planner_examples/maps/empty_box.yaml new file mode 100644 index 0000000..e4cb273 --- /dev/null +++ b/mpc_local_planner_examples/maps/empty_box.yaml @@ -0,0 +1,7 @@ +image: empty_box.png +resolution: 0.050000 +origin: [-3.0, -3.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/mpc_local_planner_examples/maps/maze.png b/mpc_local_planner_examples/maps/maze.png new file mode 100644 index 0000000..c5767e4 Binary files /dev/null and b/mpc_local_planner_examples/maps/maze.png differ diff --git a/mpc_local_planner_examples/maps/maze.yaml b/mpc_local_planner_examples/maps/maze.yaml new file mode 100644 index 0000000..79220e7 --- /dev/null +++ b/mpc_local_planner_examples/maps/maze.yaml @@ -0,0 +1,6 @@ +image: maze.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/mpc_local_planner_examples/package.xml b/mpc_local_planner_examples/package.xml new file mode 100644 index 0000000..6272f3d --- /dev/null +++ b/mpc_local_planner_examples/package.xml @@ -0,0 +1,29 @@ + + + mpc_local_planner_examples + 0.0.0 + The mpc_local_planner_examples package + + Christoph Rösmann + Christoph Rösmann + + BSD + + http://wiki.ros.org/mpc_local_planner_examples + + + catkin + + stage_ros + mpc_local_planner + move_base + map_server + amcl + + + + + + + + diff --git a/mpc_local_planner_examples/stage/corridor.world b/mpc_local_planner_examples/stage/corridor.world new file mode 100644 index 0000000..c2d4044 --- /dev/null +++ b/mpc_local_planner_examples/stage/corridor.world @@ -0,0 +1,123 @@ +include "robots/diff_drive_robot_gps.inc" +include "robots/obstacle.inc" + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 0 + + gui_nose 0 + gui_grid 0 + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +resolution 0.02 +interval_sim 100 # simulation timestep in milliseconds + +window +( + size [ 600.0 700.0 ] + center [ 0.0 0.0 ] + rotate [ 0.0 0.0 ] + scale 60 +) + +floorplan +( + name "corridor" + bitmap "../maps/corridor.png" + size [ 25.000 6.000 2.000 ] + pose [ 12.5 3.0 0.000 0.000 ] +) + +# throw in a robot +myrobot +( + pose [ 2.0 3.0 0.0 0.0 ] + name "turtlebot" +) + + +myobstacle +( + pose [ 4.0 3.77 0.0 0.0 ] + name "obstacle0" +) + +myobstacle +( + pose [ 5.5 4.09 0.0 0.0 ] + name "obstacle1" +) + +myobstacle +( + pose [ 7.0 5.38 0.0 0.0 ] + name "obstacle2" +) + +myobstacle +( + pose [ 8.5 1.47 0.0 0.0 ] + name "obstacle3" +) + +myobstacle +( + pose [ 10.0 2.14 0.0 0.0 ] + name "obstacle4" +) + +myobstacle +( + pose [ 11.5 3.58 0.0 0.0 ] + name "obstacle5" +) + +myobstacle +( + pose [ 13.0 0.26 0.0 0.0 ] + name "obstacle6" +) + +myobstacle +( + pose [ 14.5 1.09 0.0 0.0 ] + name "obstacle7" +) + +myobstacle +( + pose [ 16 2.94 0.0 0.0 ] + name "obstacle8" +) + +myobstacle +( + pose [ 17.5 2.76 0.0 0.0 ] + name "obstacle9" +) + +myobstacle +( + pose [ 19 0.79 0.0 0.0 ] + name "obstacle10" +) + +myobstacle +( + pose [ 20.5 5.16 0.0 0.0 ] + name "obstacle11" +) + +myobstacle +( + pose [ 22 3.62 0.0 0.0 ] + name "obstacle12" +) diff --git a/mpc_local_planner_examples/stage/empty_box.world b/mpc_local_planner_examples/stage/empty_box.world new file mode 100644 index 0000000..f3a8166 --- /dev/null +++ b/mpc_local_planner_examples/stage/empty_box.world @@ -0,0 +1,32 @@ +# include our robot and obstacle definitions +include "robots/diff_drive_robot_gps.inc" +include "robots/obstacle.inc" + +## Simulation settings +resolution 0.02 +interval_sim 100 # simulation timestep in milliseconds + +## Load a static map +model +( + # most maps will need a bounding box + name "emptyBox" + bitmap "../maps/empty_box.png" + size [ 6.0 6.0 2.0 ] + pose [ 0.0 0.0 0.0 0.0 ] + laser_return 1 + color "gray30" +) + +# throw in a robot +myrobot +( + pose [ -2.0 0.0 0.0 -90.0 ] + name "myRobot" +) + +myobstacle +( + pose [ 0.0 1.0 0.0 0.0 ] + name "obstacle" +) diff --git a/mpc_local_planner_examples/stage/maze_carlike.world b/mpc_local_planner_examples/stage/maze_carlike.world new file mode 100644 index 0000000..429b13b --- /dev/null +++ b/mpc_local_planner_examples/stage/maze_carlike.world @@ -0,0 +1,44 @@ +include "robots/carlike_robot.inc" + + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +resolution 0.02 +interval_sim 100 # simulation timestep in milliseconds + +window +( + size [ 600.0 700.0 ] + center [ 0.0 0.0 ] + rotate [ 0.0 0.0 ] + scale 60 +) + +floorplan +( + name "maze" + bitmap "../maps/maze.png" + size [ 10.0 10.0 2.0 ] + pose [ 5.0 5.0 0.0 0.0 ] +) + +# throw in a robot +carlike_robot +( + pose [ 2.0 2.0 0.0 0.0 ] + name "robot" +) diff --git a/mpc_local_planner_examples/stage/maze_diff_drive.world b/mpc_local_planner_examples/stage/maze_diff_drive.world new file mode 100644 index 0000000..7bffcd6 --- /dev/null +++ b/mpc_local_planner_examples/stage/maze_diff_drive.world @@ -0,0 +1,44 @@ +include "robots/diff_drive_robot.inc" + + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +resolution 0.02 +interval_sim 100 # simulation timestep in milliseconds + +window +( + size [ 600.0 700.0 ] + center [ 0.0 0.0 ] + rotate [ 0.0 0.0 ] + scale 60 +) + +floorplan +( + name "maze" + bitmap "../maps/maze.png" + size [ 10.0 10.0 2.0 ] + pose [ 5.0 5.0 0.0 0.0 ] +) + +# throw in a robot +diff_drive_robot +( + pose [ 2.0 2.0 0.0 0.0 ] + name "robot" +) diff --git a/mpc_local_planner_examples/stage/maze_omnidir.world b/mpc_local_planner_examples/stage/maze_omnidir.world new file mode 100644 index 0000000..e7675da --- /dev/null +++ b/mpc_local_planner_examples/stage/maze_omnidir.world @@ -0,0 +1,44 @@ +include "robots/omnidir_robot.inc" + + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +resolution 0.02 +interval_sim 100 # simulation timestep in milliseconds + +window +( + size [ 600.0 700.0 ] + center [ 0.0 0.0 ] + rotate [ 0.0 0.0 ] + scale 60 +) + +floorplan +( + name "maze" + bitmap "../maps/maze.png" + size [ 10.0 10.0 2.0 ] + pose [ 5.0 5.0 0.0 0.0 ] +) + +# throw in a robot +omnidir_robot +( + pose [ 2.0 2.0 0.0 0.0 ] + name "robot" +) diff --git a/mpc_local_planner_examples/stage/robots/carlike_robot.inc b/mpc_local_planner_examples/stage/robots/carlike_robot.inc new file mode 100644 index 0000000..14a2404 --- /dev/null +++ b/mpc_local_planner_examples/stage/robots/carlike_robot.inc @@ -0,0 +1,38 @@ +define laser ranger +( + sensor + ( + range_max 6.5 + fov 58.0 + samples 640 + ) + # generic model properties + color "black" + size [ 0.06 0.15 0.03 ] +) + +# +# Robot model: +# footprint (counter-clockwise): [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] +# center of rotation: [0,0] +# wheelbase: 0.4 + +define carlike_robot position +( + pose [ 0.0 0.0 0.0 0.0 ] + + odom_error [0.03 0.03 999999 999999 999999 0.02] + + 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] + # correct center of rotation: + origin [ 0.2 0.0 0.0 0.0] + gui_nose 1 + color "red" + + # kinematics + drive "car" + wheelbase 0.4 # distance between both axles + + # spawn sensors + laser(pose [ -0.1 0.0 -0.11 0.0 ]) +) diff --git a/mpc_local_planner_examples/stage/robots/diff_drive_robot.inc b/mpc_local_planner_examples/stage/robots/diff_drive_robot.inc new file mode 100644 index 0000000..b2cba16 --- /dev/null +++ b/mpc_local_planner_examples/stage/robots/diff_drive_robot.inc @@ -0,0 +1,30 @@ +define laser ranger +( + sensor + ( + range_max 6.5 + fov 58.0 + samples 640 + ) + # generic model properties + color "black" + size [ 0.06 0.15 0.03 ] +) + +define diff_drive_robot position +( + pose [ 0.0 0.0 0.0 0.0 ] + + odom_error [0.03 0.03 999999 999999 999999 0.02] + + size [ 0.25 0.25 0.40 ] + origin [ 0.0 0.0 0.0 0.0 ] + gui_nose 1 + color "red" + + # kinematics + drive "diff" + + # spawn sensors + laser(pose [ -0.1 0.0 -0.11 0.0 ]) +) diff --git a/mpc_local_planner_examples/stage/robots/diff_drive_robot_gps.inc b/mpc_local_planner_examples/stage/robots/diff_drive_robot_gps.inc new file mode 100644 index 0000000..7fd0e5b --- /dev/null +++ b/mpc_local_planner_examples/stage/robots/diff_drive_robot_gps.inc @@ -0,0 +1,21 @@ +define mylaser ranger +( + sensor + ( + # just for demonstration purposes + range [0.1 25] # minimum and maximum range + fov 360.0 # field of view + samples 1920 # number of samples + ) + size [ 0.06 0.15 0.03 ] +) + +define myrobot position +( + size [ 0.25 0.25 0.40 ] # (x,y,z) + localization "gps" # exact localization + gui_nose 1 # draw nose showing heading + drive "diff" # diff-drive + color "red" # red model + mylaser(pose [ -0.1 0.0 -0.11 0.0 ]) # has mylaser sensor +) diff --git a/mpc_local_planner_examples/stage/robots/obstacle.inc b/mpc_local_planner_examples/stage/robots/obstacle.inc new file mode 100644 index 0000000..4be8d0f --- /dev/null +++ b/mpc_local_planner_examples/stage/robots/obstacle.inc @@ -0,0 +1,8 @@ +define myobstacle position +( + localization "gps" + size [ 0.25 0.25 0.40 ] + gui_nose 1 + drive "omni" + color "blue" +) \ No newline at end of file diff --git a/mpc_local_planner_examples/stage/robots/omnidir_robot.inc b/mpc_local_planner_examples/stage/robots/omnidir_robot.inc new file mode 100644 index 0000000..92ac4b1 --- /dev/null +++ b/mpc_local_planner_examples/stage/robots/omnidir_robot.inc @@ -0,0 +1,30 @@ +define laser ranger +( + sensor + ( + range_max 6.5 + fov 58.0 + samples 640 + ) + # generic model properties + color "black" + size [ 0.06 0.15 0.03 ] +) + +define omnidir_robot position +( + pose [ 0.0 0.0 0.0 0.0 ] + + odom_error [0.03 0.03 999999 999999 999999 0.02] + + size [ 0.25 0.25 0.40 ] + origin [ 0.0 0.0 0.0 0.0 ] + gui_nose 1 + color "red" + + # kinematics + drive "omni" + + # spawn sensors + laser(pose [ -0.1 0.0 -0.11 0.0 ]) +) diff --git a/mpc_local_planner_msgs/CMakeLists.txt b/mpc_local_planner_msgs/CMakeLists.txt new file mode 100644 index 0000000..1ede96d --- /dev/null +++ b/mpc_local_planner_msgs/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mpc_local_planner_msgs) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + std_msgs + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder + add_message_files( + FILES + StateFeedback.msg + OptimalControlResult.msg + ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here + generate_messages( + DEPENDENCIES + std_msgs + ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES mpc_local_planner_msgs + CATKIN_DEPENDS std_msgs message_runtime +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +#include_directories( +# include +# ${catkin_INCLUDE_DIRS} +#) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/mpc_local_planner_msgs.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/mpc_local_planner_msgs_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_local_planner_msgs.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/mpc_local_planner_msgs/msg/OptimalControlResult.msg b/mpc_local_planner_msgs/msg/OptimalControlResult.msg new file mode 100644 index 0000000..f327809 --- /dev/null +++ b/mpc_local_planner_msgs/msg/OptimalControlResult.msg @@ -0,0 +1,12 @@ +std_msgs/Header header + +int64 dim_states +int64 dim_controls + +float64[] time_states +float64[] states # Column Major +float64[] time_controls +float64[] controls # Column Major + +bool optimal_solution_found +float64 cpu_time diff --git a/mpc_local_planner_msgs/msg/StateFeedback.msg b/mpc_local_planner_msgs/msg/StateFeedback.msg new file mode 100644 index 0000000..9508a20 --- /dev/null +++ b/mpc_local_planner_msgs/msg/StateFeedback.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +float64[] state diff --git a/mpc_local_planner_msgs/package.xml b/mpc_local_planner_msgs/package.xml new file mode 100644 index 0000000..6bbfc09 --- /dev/null +++ b/mpc_local_planner_msgs/package.xml @@ -0,0 +1,30 @@ + + + mpc_local_planner_msgs + 0.0.0 + This package provides message types that are used by the package mpc_local_planner + + Christoph Rösmann + Christoph Rösmann + + GPLv3 + + http://wiki.ros.org/mpc_local_planner_msgs + + catkin + + std_msgs + message_generation + + std_msgs + + std_msgs + message_runtime + + + + + + + +