├── .gitignore ├── .gitlab-ci.yml ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg ├── TebLocalPlannerReconfigure.cfg └── rviz_test_optim.rviz ├── cmake_modules ├── FindG2O.cmake └── FindSUITESPARSE.cmake ├── include └── teb_local_planner │ ├── distance_calculations.h │ ├── equivalence_relations.h │ ├── g2o_types │ ├── base_teb_edges.h │ ├── edge_acceleration.h │ ├── edge_dynamic_obstacle.h │ ├── edge_kinematics.h │ ├── edge_obstacle.h │ ├── edge_prefer_rotdir.h │ ├── edge_time_optimal.h │ ├── edge_velocity.h │ ├── edge_via_point.h │ ├── penalties.h │ ├── vertex_pose.h │ └── vertex_timediff.h │ ├── graph_search.h │ ├── h_signature.h │ ├── homotopy_class_planner.h │ ├── homotopy_class_planner.hpp │ ├── misc.h │ ├── obstacles.h │ ├── optimal_planner.h │ ├── planner_interface.h │ ├── pose_se2.h │ ├── recovery_behaviors.h │ ├── robot_footprint_model.h │ ├── teb_config.h │ ├── teb_local_planner_ros.h │ ├── timed_elastic_band.h │ ├── timed_elastic_band.hpp │ ├── visualization.h │ └── visualization.hpp ├── launch └── test_optim_node.launch ├── msg ├── FeedbackMsg.msg ├── TrajectoryMsg.msg └── TrajectoryPointMsg.msg ├── package.xml ├── scripts ├── cmd_vel_to_ackermann_drive.py ├── export_to_mat.py ├── export_to_svg.py ├── publish_dynamic_obstacle.py ├── publish_test_obstacles.py ├── publish_viapoints.py └── visualize_velocity_profile.py ├── src ├── graph_search.cpp ├── homotopy_class_planner.cpp ├── obstacles.cpp ├── optimal_planner.cpp ├── recovery_behaviors.cpp ├── teb_config.cpp ├── teb_local_planner_ros.cpp ├── test_optim_node.cpp ├── timed_elastic_band.cpp └── visualization.cpp ├── teb_local_planner_plugin.xml └── teb中的G2O问题的设置.md /.gitignore: -------------------------------------------------------------------------------- 1 | .svn 2 | .kdev4 3 | *.kdev4 4 | .vscode 5 | 6 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | build-kinetic: 2 | variables: 3 | ROSDISTRO: "kinetic" 4 | CATKIN_WS: "/root/catkin_ws" 5 | stage: build 6 | image: osrf/ros:$ROSDISTRO-desktop-full 7 | before_script: 8 | - apt-get -qq update 9 | - apt-get -qq install git-core python-catkin-tools curl 10 | - mkdir -p $CATKIN_WS/src 11 | - cp -a . $CATKIN_WS/src/ 12 | - cd $CATKIN_WS 13 | - rosdep update 14 | - rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false 15 | script: 16 | - source /ros_entrypoint.sh 17 | - cd $CATKIN_WS 18 | - catkin build -i -s --no-status 19 | only: 20 | - kinetic-devel 21 | tags: 22 | - ubuntu 23 | - docker -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Generic .travis.yml file for running continuous integration on Travis-CI with 2 | # any ROS package. 3 | # 4 | # This installs ROS on a clean Travis-CI virtual machine, creates a ROS 5 | # workspace, resolves all listed dependencies, and sets environment variables 6 | # (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are 7 | # no compilation errors), and runs all the tests. If any of the compilation/test 8 | # phases fail, the build is marked as a failure. 9 | # 10 | # We handle two types of package dependencies: 11 | # - packages (ros and otherwise) available through apt-get. These are installed 12 | # using rosdep, based on the information in the ROS package.xml. 13 | # - dependencies that must be checked out from source. These are handled by 14 | # 'wstool', and should be listed in a file named dependencies.rosinstall. 15 | # 16 | # There are two variables you may want to change: 17 | # - ROS_DISTRO (default is indigo). Note that packages must be available for 18 | # ubuntu 14.04 trusty. 19 | # - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo 20 | # root). This should list all necessary repositories in wstool format (see 21 | # the ros wiki). If the file does not exists then nothing happens. 22 | # 23 | # See the README.md for more information. 24 | # 25 | # Author: Felix Duvallet 26 | 27 | # NOTE: The build lifecycle on Travis.ci is something like this: 28 | # before_install 29 | # install 30 | # before_script 31 | # script 32 | # after_success or after_failure 33 | # after_script 34 | # OPTIONAL before_deploy 35 | # OPTIONAL deploy 36 | # OPTIONAL after_deploy 37 | 38 | ################################################################################ 39 | 40 | # Use ubuntu trusty (14.04) with sudo privileges. 41 | dist: trusty 42 | sudo: required 43 | language: 44 | - generic 45 | cache: 46 | - apt 47 | 48 | # Configuration variables. All variables are global now, but this can be used to 49 | # trigger a build matrix for different ROS distributions if desired. 50 | env: 51 | global: 52 | - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] 53 | - CI_SOURCE_PATH=$(pwd) 54 | - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall 55 | - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options 56 | - ROS_PARALLEL_JOBS='-j8 -l6' 57 | matrix: 58 | - ROS_DISTRO=jade 59 | 60 | 61 | ################################################################################ 62 | 63 | # Install system dependencies, namely a very barebones ROS setup. 64 | before_install: 65 | - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 66 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 67 | - sudo apt-get update -qq 68 | - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin 69 | - source /opt/ros/$ROS_DISTRO/setup.bash 70 | # Prepare rosdep to install dependencies. 71 | - sudo rosdep init 72 | - rosdep update 73 | 74 | # Create a catkin workspace with the package under integration. 75 | install: 76 | - mkdir -p ~/catkin_ws/src 77 | - cd ~/catkin_ws/src 78 | - catkin_init_workspace 79 | # Create the devel/setup.bash (run catkin_make with an empty workspace) and 80 | # source it to set the path variables. 81 | - cd ~/catkin_ws 82 | - catkin_make 83 | - source devel/setup.bash 84 | # Add the package under integration to the workspace using a symlink. 85 | - cd ~/catkin_ws/src 86 | - ln -s $CI_SOURCE_PATH . 87 | 88 | # Install all dependencies, using wstool and rosdep. 89 | # wstool looks for a ROSINSTALL_FILE defined in the environment variables. 90 | before_script: 91 | # source dependencies: install using wstool. 92 | - cd ~/catkin_ws/src 93 | - wstool init 94 | - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi 95 | - wstool up 96 | # package depdencies: install using rosdep. 97 | - cd ~/catkin_ws 98 | - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 99 | 100 | # Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to 101 | # catkin_make. 102 | script: 103 | - cd ~/catkin_ws 104 | - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) 105 | # Testing: Use both run_tests (to see the output) and test (to error out). 106 | - catkin_make run_tests # This always returns 0, but looks pretty. 107 | - catkin_make test # This will return non-zero if a test fails. 108 | 109 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(teb_local_planner) 3 | 4 | # Set to Release in order to speed up the program significantly 5 | set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel 6 | 7 | ## Find catkin macros and libraries 8 | find_package(catkin REQUIRED COMPONENTS 9 | base_local_planner 10 | costmap_2d 11 | costmap_converter 12 | cmake_modules 13 | dynamic_reconfigure 14 | geometry_msgs 15 | interactive_markers 16 | message_generation 17 | nav_core 18 | nav_msgs 19 | roscpp 20 | std_msgs 21 | pluginlib 22 | tf 23 | tf_conversions 24 | visualization_msgs 25 | ) 26 | message(STATUS "System: ${CMAKE_SYSTEM}") 27 | ## System dependencies are found with CMake's conventions 28 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules) 29 | message(STATUS "${CMAKE_MODULE_PATH}") 30 | find_package(Boost REQUIRED COMPONENTS system thread graph) 31 | find_package(SUITESPARSE REQUIRED) 32 | find_package(G2O REQUIRED) 33 | 34 | # Eigen3 FindScript Backward compatibility (ubuntu saucy) 35 | # Since FindEigen.cmake is deprecated starting from jade. 36 | if (EXISTS "FindEigen3.cmake") 37 | find_package(Eigen3 REQUIRED) 38 | set(Eigen_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS}) 39 | elseif (EXISTS "FindEigen.cmake") 40 | find_package(Eigen REQUIRED) 41 | elseif (EXISTS "FindEigen.cmake") 42 | message(WARNING "No findEigen cmake script found. You must provde one of them, 43 | e.g. by adding it to ${PROJECT_SOURCE_DIR}/cmake_modules.") 44 | endif (EXISTS "FindEigen3.cmake") 45 | 46 | set(EXTERNAL_INCLUDE_DIRS ${Eigen_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ${G2O_INCLUDE_DIR}) 47 | set(EXTERNAL_LIBS ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES}) 48 | 49 | ## Uncomment this if the package has a setup.py. This macro ensures 50 | ## modules and global scripts declared therein get installed 51 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 52 | # catkin_python_setup() 53 | 54 | 55 | ## C++11 support 56 | ## Unfortunately, the 3d-party dependency libg2o requires c++11 starting from ROS Jade. 57 | ## Even if the ROS Jade specifications do not want c++11-only packages, 58 | ## we cannot compile without c++11 enabled. Another option would be to downgrade 59 | ## libg2o third-party package. 60 | ## By now, if you do not want c++11, please refer to the ros indigo version. 61 | IF(NOT MSVC) 62 | include(CheckCXXCompilerFlag) 63 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 64 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 65 | if(COMPILER_SUPPORTS_CXX11) 66 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 67 | elseif(COMPILER_SUPPORTS_CXX0X) 68 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 69 | else() 70 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support which is required 71 | by linked third party packages starting from ROS Jade. Ignore this message for ROS Indigo.") 72 | endif() 73 | endif() 74 | 75 | ################################################ 76 | ## Declare ROS messages, services and actions ## 77 | ################################################ 78 | 79 | ## To declare and build messages, services or actions from within this 80 | ## package, follow these steps: 81 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 82 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 85 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 86 | ## pulled in transitively but can be declared for certainty nonetheless: 87 | ## * add a build_depend tag for "message_generation" 88 | ## * add a run_depend tag for "message_runtime" 89 | ## * In this file (CMakeLists.txt): 90 | ## * add "message_generation" and every package in MSG_DEP_SET to 91 | ## find_package(catkin REQUIRED COMPONENTS ...) 92 | ## * add "message_runtime" and every package in MSG_DEP_SET to 93 | ## catkin_package(CATKIN_DEPENDS ...) 94 | ## * uncomment the add_*_files sections below as needed 95 | ## and list every .msg/.srv/.action file to be processed 96 | ## * uncomment the generate_messages entry below 97 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 98 | 99 | ## Generate messages in the 'msg' folder 100 | add_message_files( 101 | FILES 102 | TrajectoryPointMsg.msg 103 | TrajectoryMsg.msg 104 | FeedbackMsg.msg 105 | ) 106 | 107 | ## Generate services in the 'srv' folder 108 | # add_service_files( 109 | # FILES 110 | # Service1.srv 111 | # Service2.srv 112 | # ) 113 | 114 | ## Generate actions in the 'action' folder 115 | # add_action_files( 116 | # FILES 117 | # Action1.action 118 | # Action2.action 119 | # ) 120 | 121 | ## Generate added messages and services with any dependencies listed here 122 | generate_messages( 123 | DEPENDENCIES 124 | geometry_msgs std_msgs costmap_converter 125 | ) 126 | 127 | #add dynamic reconfigure api 128 | #find_package(catkin REQUIRED dynamic_reconfigure) 129 | generate_dynamic_reconfigure_options( 130 | cfg/TebLocalPlannerReconfigure.cfg 131 | ) 132 | 133 | 134 | ################################### 135 | ## catkin specific configuration ## 136 | ################################### 137 | ## The catkin_package macro generates cmake config files for your package 138 | ## Declare things to be passed to dependent projects 139 | ## INCLUDE_DIRS: uncomment this if you package contains header files 140 | ## LIBRARIES: libraries you create in this project that dependent projects also need 141 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 142 | ## DEPENDS: system dependencies of this project that dependent projects also need 143 | catkin_package( 144 | INCLUDE_DIRS include ${EXTERNAL_INCLUDE_DIRS} 145 | LIBRARIES teb_local_planner ${EXTERNAL_LIBS} 146 | CATKIN_DEPENDS 147 | base_local_planner 148 | costmap_2d 149 | costmap_converter 150 | dynamic_reconfigure 151 | geometry_msgs 152 | interactive_markers 153 | message_runtime 154 | nav_core 155 | nav_msgs 156 | pluginlib 157 | roscpp 158 | std_msgs 159 | tf 160 | tf_conversions 161 | visualization_msgs 162 | DEPENDS SUITESPARSE G2O 163 | ) 164 | 165 | ########### 166 | ## Build ## 167 | ########### 168 | 169 | ## Specify additional locations of header files 170 | ## Your package locations should be listed before other locations 171 | # include_directories(include) 172 | include_directories( 173 | include 174 | ${EXTERNAL_INCLUDE_DIRS} 175 | ${catkin_INCLUDE_DIRS} 176 | ) 177 | 178 | ## Build the teb_local_planner library 179 | 180 | add_library(teb_local_planner 181 | src/timed_elastic_band.cpp 182 | src/optimal_planner.cpp 183 | src/obstacles.cpp 184 | src/visualization.cpp 185 | src/recovery_behaviors.cpp 186 | src/teb_config.cpp 187 | src/homotopy_class_planner.cpp 188 | src/teb_local_planner_ros.cpp 189 | src/graph_search.cpp 190 | ) 191 | 192 | # Dynamic reconfigure: make sure configure headers are built before any node using them 193 | add_dependencies(teb_local_planner ${PROJECT_NAME}_gencfg) 194 | # Generate messages before compiling the lib 195 | add_dependencies(teb_local_planner ${PROJECT_NAME}_generate_messages_cpp) 196 | 197 | target_link_libraries(teb_local_planner 198 | ${EXTERNAL_LIBS} 199 | ${catkin_LIBRARIES} 200 | ) 201 | 202 | 203 | 204 | ## Build additional executables 205 | 206 | add_executable(test_optim_node src/test_optim_node.cpp) 207 | 208 | target_link_libraries(test_optim_node 209 | teb_local_planner 210 | ${EXTERNAL_LIBS} 211 | ${catkin_LIBRARIES} 212 | ) 213 | 214 | 215 | ############# 216 | ## Install ## 217 | ############# 218 | 219 | # all install targets should use catkin DESTINATION variables 220 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 221 | 222 | ## Mark executable scripts (Python etc.) for installation 223 | ## in contrast to setup.py, you can choose the destination 224 | # install(PROGRAMS 225 | # scripts/my_python_script 226 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 227 | # ) 228 | 229 | ## Mark executables and/or libraries for installation 230 | install(TARGETS teb_local_planner 231 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 232 | ) 233 | install(TARGETS test_optim_node 234 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 235 | ) 236 | 237 | ## Mark cpp header files for installation 238 | install(DIRECTORY include/${PROJECT_NAME}/ 239 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 240 | #FILES_MATCHING PATTERN "*.h" 241 | PATTERN ".svn" EXCLUDE 242 | ) 243 | 244 | ## Mark other files for installation (e.g. launch and bag files, etc.) 245 | install(FILES 246 | teb_local_planner_plugin.xml 247 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 248 | ) 249 | 250 | install(DIRECTORY 251 | launch cfg scripts 252 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 253 | PATTERN ".svn" EXCLUDE 254 | ) 255 | 256 | ############# 257 | ## Testing ## 258 | ############# 259 | 260 | ## Add gtest based cpp test target and link libraries 261 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_teb_local_planner.cpp) 262 | # if(TARGET ${PROJECT_NAME}-test) 263 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 264 | # endif() 265 | 266 | ## Add folders to be run by python nosetests 267 | # catkin_add_nosetests(test) 268 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, TU Dortmund - Lehrstuhl RST 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of teb_local_planner nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | teb_local_planner 代码中文注释 2 | ============================= 3 | 4 | ROS wiki地址　：http://wiki.ros.org/teb_local_planner 5 | 6 | 7 | 8 | github 仓库　：https://github.com/rst-tu-dortmund/teb_local_planner 9 | 10 | 11 | ## 相关paper 12 | 13 | - C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153. 14 | - C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, May 2012, pp 74–79. 15 | - C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, Sept. 2013, pp. 138–143. 16 | - C. Rösmann, F. Hoffmann and T. Bertram: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015. 17 | - C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017. 18 | 19 | ## 详见文章 20 | 21 | [teb中的g2o问题设置](https://epsavlc.github.io/2019/08/06/teb_g2o.html) 22 | 23 | -------------------------------------------------------------------------------- /cfg/rviz_test_optim.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 562 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.0299999993 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 0.100000001 51 | Cell Size: 0.100000001 52 | Class: rviz/Grid 53 | Color: 160; 160; 164 54 | Enabled: true 55 | Line Style: 56 | Line Width: 0.0299999993 57 | Value: Lines 58 | Name: Fine Grid 59 | Normal Cell Count: 0 60 | Offset: 61 | X: 0 62 | Y: 0 63 | Z: 0 64 | Plane: XY 65 | Plane Cell Count: 100 66 | Reference Frame: 67 | Value: true 68 | - Alpha: 1 69 | Buffer Length: 1 70 | Class: rviz/Path 71 | Color: 25; 255; 0 72 | Enabled: true 73 | Head Diameter: 0.300000012 74 | Head Length: 0.200000003 75 | Length: 0.300000012 76 | Line Style: Lines 77 | Line Width: 0.0299999993 78 | Name: Path 79 | Offset: 80 | X: 0 81 | Y: 0 82 | Z: 0 83 | Pose Style: None 84 | Radius: 0.0299999993 85 | Shaft Diameter: 0.100000001 86 | Shaft Length: 0.100000001 87 | Topic: /test_optim_node/local_plan 88 | Unreliable: false 89 | Value: true 90 | - Alpha: 1 91 | Arrow Length: 0.300000012 92 | Axes Length: 0.300000012 93 | Axes Radius: 0.00999999978 94 | Class: rviz/PoseArray 95 | Color: 255; 25; 0 96 | Enabled: true 97 | Head Length: 0.0700000003 98 | Head Radius: 0.0299999993 99 | Name: PoseArray 100 | Shaft Length: 0.230000004 101 | Shaft Radius: 0.00999999978 102 | Shape: Arrow (Flat) 103 | Topic: /test_optim_node/teb_poses 104 | Unreliable: false 105 | Value: true 106 | - Class: rviz/Marker 107 | Enabled: true 108 | Marker Topic: /test_optim_node/teb_markers 109 | Name: Marker 110 | Namespaces: 111 | PointObstacles: true 112 | Queue Size: 100 113 | Value: true 114 | - Class: rviz/InteractiveMarkers 115 | Enable Transparency: true 116 | Enabled: true 117 | Name: InteractiveMarkers 118 | Show Axes: false 119 | Show Descriptions: true 120 | Show Visual Aids: false 121 | Update Topic: /marker_obstacles/update 122 | Value: true 123 | Enabled: true 124 | Global Options: 125 | Background Color: 48; 48; 48 126 | Fixed Frame: odom 127 | Frame Rate: 30 128 | Name: root 129 | Tools: 130 | - Class: rviz/Interact 131 | Hide Inactive Objects: true 132 | - Class: rviz/MoveCamera 133 | - Class: rviz/Select 134 | - Class: rviz/FocusCamera 135 | - Class: rviz/Measure 136 | - Class: rviz/SetInitialPose 137 | Topic: /initialpose 138 | - Class: rviz/SetGoal 139 | Topic: /move_base_simple/goal 140 | - Class: rviz/PublishPoint 141 | Single click: true 142 | Topic: /clicked_point 143 | Value: true 144 | Views: 145 | Current: 146 | Class: rviz/Orbit 147 | Distance: 7.77247 148 | Enable Stereo Rendering: 149 | Stereo Eye Separation: 0.0599999987 150 | Stereo Focal Distance: 1 151 | Swap Stereo Eyes: false 152 | Value: false 153 | Focal Point: 154 | X: 0 155 | Y: 0 156 | Z: 0 157 | Focal Shape Fixed Size: true 158 | Focal Shape Size: 0.0500000007 159 | Name: Current View 160 | Near Clip Distance: 0.00999999978 161 | Pitch: 1.56979632 162 | Target Frame: 163 | Value: Orbit (rviz) 164 | Yaw: 4.71043873 165 | Saved: ~ 166 | Window Geometry: 167 | Displays: 168 | collapsed: false 169 | Height: 846 170 | Hide Left Dock: false 171 | Hide Right Dock: true 172 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 173 | Selection: 174 | collapsed: false 175 | Time: 176 | collapsed: false 177 | Tool Properties: 178 | collapsed: false 179 | Views: 180 | collapsed: true 181 | Width: 1354 182 | X: 137 183 | Y: 50 184 | -------------------------------------------------------------------------------- /cmake_modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Locate the g2o libraries 2 | # A general framework for graph optimization. 3 | # 4 | # This module defines 5 | # G2O_FOUND, if false, do not try to link against g2o 6 | # G2O_LIBRARIES, path to the libg2o 7 | # G2O_INCLUDE_DIR, where to find the g2o header files 8 | # 9 | # Niko Suenderhauf 10 | # Adapted by Felix Endres 11 | 12 | IF(UNIX) 13 | 14 | #IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) 15 | # in cache already 16 | # SET(G2O_FIND_QUIETLY TRUE) 17 | #ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) 18 | 19 | MESSAGE(STATUS "Searching for g2o ...") 20 | FIND_PATH(G2O_INCLUDE_DIR 21 | NAMES core math_groups types 22 | PATHS /usr/local /usr 23 | PATH_SUFFIXES include/g2o include) 24 | 25 | IF (G2O_INCLUDE_DIR) 26 | MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}") 27 | ENDIF (G2O_INCLUDE_DIR) 28 | 29 | FIND_LIBRARY(G2O_CORE_LIB 30 | NAMES g2o_core g2o_core_rd 31 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 32 | PATH_SUFFIXES lib) 33 | FIND_LIBRARY(G2O_STUFF_LIB 34 | NAMES g2o_stuff g2o_stuff_rd 35 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 36 | PATH_SUFFIXES lib) 37 | FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB 38 | NAMES g2o_types_slam2d g2o_types_slam2d_rd 39 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 40 | PATH_SUFFIXES lib) 41 | FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB 42 | NAMES g2o_types_slam3d g2o_types_slam3d_rd 43 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 44 | PATH_SUFFIXES lib) 45 | FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB 46 | NAMES g2o_solver_cholmod g2o_solver_cholmod_rd 47 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 48 | PATH_SUFFIXES lib) 49 | FIND_LIBRARY(G2O_SOLVER_PCG_LIB 50 | NAMES g2o_solver_pcg g2o_solver_pcg_rd 51 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 52 | PATH_SUFFIXES lib) 53 | FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB 54 | NAMES g2o_solver_csparse g2o_solver_csparse_rd 55 | PATHS /usr/local /usr 56 | PATH_SUFFIXES lib) 57 | FIND_LIBRARY(G2O_INCREMENTAL_LIB 58 | NAMES g2o_incremental g2o_incremental_rd 59 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 60 | PATH_SUFFIXES lib) 61 | FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB 62 | NAMES g2o_csparse_extension g2o_csparse_extension_rd 63 | PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} 64 | PATH_SUFFIXES lib) 65 | 66 | SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB} 67 | ${G2O_CORE_LIB} 68 | ${G2O_STUFF_LIB} 69 | ${G2O_TYPES_SLAM2D_LIB} 70 | ${G2O_TYPES_SLAM3D_LIB} 71 | ${G2O_SOLVER_CHOLMOD_LIB} 72 | ${G2O_SOLVER_PCG_LIB} 73 | ${G2O_SOLVER_CSPARSE_LIB} 74 | ${G2O_INCREMENTAL_LIB} 75 | ) 76 | 77 | IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 78 | SET(G2O_FOUND "YES") 79 | IF(NOT G2O_FIND_QUIETLY) 80 | MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}") 81 | ENDIF(NOT G2O_FIND_QUIETLY) 82 | ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 83 | IF(NOT G2O_LIBRARIES) 84 | IF(G2O_FIND_REQUIRED) 85 | message(FATAL_ERROR "Could not find libg2o!") 86 | ENDIF(G2O_FIND_REQUIRED) 87 | ENDIF(NOT G2O_LIBRARIES) 88 | 89 | IF(NOT G2O_INCLUDE_DIR) 90 | IF(G2O_FIND_REQUIRED) 91 | message(FATAL_ERROR "Could not find g2o include directory!") 92 | ENDIF(G2O_FIND_REQUIRED) 93 | ENDIF(NOT G2O_INCLUDE_DIR) 94 | ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 95 | 96 | ENDIF(UNIX) 97 | 98 | -------------------------------------------------------------------------------- /cmake_modules/FindSUITESPARSE.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find SUITESPARSE 2 | # Once done this will define 3 | # 4 | # SUITESPARSE_FOUND - system has SUITESPARSE 5 | # SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory 6 | # SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE 7 | # SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package) 8 | # SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package) 9 | # SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs 10 | # SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs 11 | # SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly 12 | 13 | IF (SUITESPARSE_INCLUDE_DIRS) 14 | # Already in cache, be silent 15 | SET(SUITESPARSE_FIND_QUIETLY TRUE) 16 | ENDIF (SUITESPARSE_INCLUDE_DIRS) 17 | 18 | if( WIN32 ) 19 | # Find cholmod part of the suitesparse library collection 20 | 21 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 22 | PATHS "C:\\libs\\win32\\SuiteSparse\\Include" ) 23 | 24 | # Add cholmod include directory to collection include directories 25 | IF ( CHOLMOD_INCLUDE_DIR ) 26 | list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) 27 | ENDIF( CHOLMOD_INCLUDE_DIR ) 28 | 29 | 30 | # find path suitesparse library 31 | FIND_PATH( SUITESPARSE_LIBRARY_DIRS 32 | amd.lib 33 | PATHS "C:\\libs\\win32\\SuiteSparse\\libs" ) 34 | 35 | # if we found the library, add it to the defined libraries 36 | IF ( SUITESPARSE_LIBRARY_DIRS ) 37 | list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd ) 38 | ENDIF( SUITESPARSE_LIBRARY_DIRS ) 39 | 40 | else( WIN32 ) 41 | IF(APPLE) 42 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 43 | PATHS /opt/local/include/ufsparse 44 | /usr/local/include ) 45 | 46 | FIND_PATH( SUITESPARSE_LIBRARY_DIR 47 | NAMES libcholmod.a 48 | PATHS /opt/local/lib 49 | /usr/local/lib ) 50 | ELSE(APPLE) 51 | FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h 52 | PATHS /usr/local/include 53 | /usr/include 54 | /usr/include/suitesparse/ 55 | ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod 56 | PATH_SUFFIXES cholmod/ CHOLMOD/ ) 57 | 58 | FIND_PATH( SUITESPARSE_LIBRARY_DIR 59 | NAMES libcholmod.so libcholmod.a 60 | PATHS /usr/lib 61 | /usr/lib64 62 | /usr/lib/x86_64-linux-gnu 63 | /usr/lib/i386-linux-gnu 64 | /usr/local/lib 65 | /usr/lib/arm-linux-gnueabihf/ 66 | /usr/lib/aarch64-linux-gnu/ 67 | /usr/lib/arm-linux-gnueabi/ 68 | /usr/lib/arm-linux-gnu) 69 | ENDIF(APPLE) 70 | 71 | # Add cholmod include directory to collection include directories 72 | IF ( CHOLMOD_INCLUDE_DIR ) 73 | list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) 74 | ENDIF( CHOLMOD_INCLUDE_DIR ) 75 | 76 | # if we found the library, add it to the defined libraries 77 | IF ( SUITESPARSE_LIBRARY_DIR ) 78 | 79 | list ( APPEND SUITESPARSE_LIBRARIES amd) 80 | list ( APPEND SUITESPARSE_LIBRARIES btf) 81 | list ( APPEND SUITESPARSE_LIBRARIES camd) 82 | list ( APPEND SUITESPARSE_LIBRARIES ccolamd) 83 | list ( APPEND SUITESPARSE_LIBRARIES cholmod) 84 | list ( APPEND SUITESPARSE_LIBRARIES colamd) 85 | # list ( APPEND SUITESPARSE_LIBRARIES csparse) 86 | list ( APPEND SUITESPARSE_LIBRARIES cxsparse) 87 | list ( APPEND SUITESPARSE_LIBRARIES klu) 88 | # list ( APPEND SUITESPARSE_LIBRARIES spqr) 89 | list ( APPEND SUITESPARSE_LIBRARIES umfpack) 90 | 91 | IF (APPLE) 92 | list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig) 93 | ENDIF (APPLE) 94 | 95 | # Metis and spqr are optional 96 | FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY 97 | NAMES metis 98 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 99 | IF (SUITESPARSE_METIS_LIBRARY) 100 | list ( APPEND SUITESPARSE_LIBRARIES metis) 101 | ENDIF(SUITESPARSE_METIS_LIBRARY) 102 | 103 | if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp") 104 | SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid") 105 | else() 106 | SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid") 107 | endif() 108 | 109 | if(SUITESPARSE_SPQR_VALID) 110 | FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY 111 | NAMES spqr 112 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 113 | IF (SUITESPARSE_SPQR_LIBRARY) 114 | list ( APPEND SUITESPARSE_LIBRARIES spqr) 115 | ENDIF (SUITESPARSE_SPQR_LIBRARY) 116 | endif() 117 | 118 | ENDIF( SUITESPARSE_LIBRARY_DIR ) 119 | 120 | endif( WIN32 ) 121 | 122 | 123 | IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 124 | IF(WIN32) 125 | list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig ) 126 | ENDIF(WIN32) 127 | SET(SUITESPARSE_FOUND TRUE) 128 | MESSAGE(STATUS "Found SuiteSparse") 129 | ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 130 | SET( SUITESPARSE_FOUND FALSE ) 131 | MESSAGE(FATAL_ERROR "Unable to find SuiteSparse") 132 | ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 133 | 134 | -------------------------------------------------------------------------------- /include/teb_local_planner/equivalence_relations.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #ifndef EQUIVALENCE_RELATIONS_H_ 40 | #define EQUIVALENCE_RELATIONS_H_ 41 | 42 | #include 43 | 44 | namespace teb_local_planner 45 | { 46 | 47 | /** 48 | * @class EquivalenceClass 49 | * @brief Abstract class that defines an interface for computing and comparing equivalence classes 50 | * 该类是一个基类，用来定义在拓扑意义上等价的路径 51 | * 52 | * Equivalence relations are utilized in order to test if two trajectories are belonging to the same 53 | * equivalence class w.r.t. the current obstacle configurations. A common equivalence relation is 54 | * the concept of homotopy classes. All trajectories belonging to the same homotopy class 55 | * can CONTINUOUSLY be deformed into each other without intersecting any obstacle. Hence they likely 56 | * share the same local minimum after invoking (local) trajectory optimization. A weaker equivalence relation 57 | * is defined by the concept of homology classes (e.g. refer to HSignature). 58 | * 59 | * Each EquivalenceClass object (or subclass) stores a candidate value which might be compared to another EquivalenceClass object. 60 | * 61 | * @remarks Currently, the computeEquivalenceClass method is not available in the generic interface EquivalenceClass. 62 | * Call the "compute"-methods directly on the subclass. 63 | */ 64 | class EquivalenceClass 65 | { 66 | public: 67 | 68 | /** 69 | * @brief Default constructor 70 | */ 71 | EquivalenceClass() {} 72 | 73 | /** 74 | * @brief virtual destructor 75 | */ 76 | virtual ~EquivalenceClass() {} 77 | 78 | /** 79 | * @brief Check if two candidate classes are equivalent 80 | * @param other The other equivalence class to test with 81 | */ 82 | virtual bool isEqual(const EquivalenceClass& other) const = 0; 83 | 84 | /** 85 | * @brief Check if the equivalence value is detected correctly 86 | * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur. 87 | */ 88 | virtual bool isValid() const = 0; 89 | 90 | /** 91 | * @brief 检测一条路径是否围绕着某个障碍物（而无法离开） 92 | * @return Returns false, if the trajectory loops around an obstacle 93 | */ 94 | virtual bool isReasonable() const = 0; 95 | 96 | }; 97 | 98 | using EquivalenceClassPtr = boost::shared_ptr; 99 | 100 | 101 | } // namespace teb_local_planner 102 | 103 | 104 | #endif /* EQUIVALENCE_RELATIONS_H_ */ 105 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/base_teb_edges.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann 42 | *********************************************************************/ 43 | 44 | #ifndef _BASE_TEB_EDGES_H_ 45 | #define _BASE_TEB_EDGES_H_ 46 | 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | 55 | namespace teb_local_planner 56 | { 57 | 58 | 59 | /** 60 | * @class BaseTebUnaryEdge 61 | * @brief Base edge connecting a single vertex in the TEB optimization problem 62 | * 63 | * This edge defines a base edge type for the TEB optimization problem. 64 | * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). 65 | * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. 66 | * Memory of edges should be freed by calling the clearEdge method of the g2o optimzier class. 67 | * @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge 68 | */ 69 | template 70 | class BaseTebUnaryEdge : public g2o::BaseUnaryEdge 71 | { 72 | public: 73 | 74 | using typename g2o::BaseUnaryEdge::ErrorVector; 75 | using g2o::BaseUnaryEdge::computeError; 76 | 77 | /** 78 | * @brief Construct edge. 79 | */ 80 | BaseTebUnaryEdge() 81 | { 82 | _vertices[0] = NULL; 83 | } 84 | 85 | /** 86 | * @brief Destruct edge. 87 | * 88 | * We need to erase vertices manually, since we want to keep them even if TebOptimalPlanner::clearGraph() is called. 89 | * This is necessary since the vertices are managed by the Timed_Elastic_Band class. 90 | */ 91 | virtual ~BaseTebUnaryEdge() 92 | { 93 | if(_vertices[0]) 94 | _vertices[0]->edges().erase(this); 95 | } 96 | 97 | /** 98 | * @brief Compute and return error / cost value. 99 | * 100 | * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. 101 | * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T 102 | */ 103 | ErrorVector& getError() 104 | { 105 | computeError(); 106 | return _error; 107 | } 108 | 109 | /** 110 | * @brief Read values from input stream 111 | */ 112 | virtual bool read(std::istream& is) 113 | { 114 | // TODO generic read 115 | return true; 116 | } 117 | 118 | /** 119 | * @brief Write values to an output stream 120 | */ 121 | virtual bool write(std::ostream& os) const 122 | { 123 | // TODO generic write 124 | return os.good(); 125 | } 126 | 127 | /** 128 | * @brief Assign the TebConfig class for parameters. 129 | * @param cfg TebConfig class 130 | */ 131 | void setTebConfig(const TebConfig& cfg) 132 | { 133 | cfg_ = &cfg; 134 | } 135 | 136 | protected: 137 | 138 | using g2o::BaseUnaryEdge::_error; 139 | using g2o::BaseUnaryEdge::_vertices; 140 | 141 | const TebConfig* cfg_; //!< Store TebConfig class for parameters 142 | 143 | public: 144 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 145 | }; 146 | 147 | /** 148 | * @class BaseTebBinaryEdge 149 | * @brief Base edge connecting two vertices in the TEB optimization problem 150 | * 151 | * This edge defines a base edge type for the TEB optimization problem. 152 | * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). 153 | * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. 154 | * Memory of edges should be freed by calling the clearEdge method of the g2o optimzier class. 155 | * @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge 156 | */ 157 | template 158 | class BaseTebBinaryEdge : public g2o::BaseBinaryEdge 159 | { 160 | public: 161 | 162 | using typename g2o::BaseBinaryEdge::ErrorVector; 163 | using g2o::BaseBinaryEdge::computeError; 164 | 165 | /** 166 | * @brief Construct edge. 167 | */ 168 | BaseTebBinaryEdge() 169 | { 170 | _vertices[0] = _vertices[1] = NULL; 171 | } 172 | 173 | /** 174 | * @brief Destruct edge. 175 | * 176 | * We need to erase vertices manually, since we want to keep them even if TebOptimalPlanner::clearGraph() is called. 177 | * This is necessary since the vertices are managed by the Timed_Elastic_Band class. 178 | */ 179 | virtual ~BaseTebBinaryEdge() 180 | { 181 | if(_vertices[0]) 182 | _vertices[0]->edges().erase(this); 183 | if(_vertices[1]) 184 | _vertices[1]->edges().erase(this); 185 | } 186 | 187 | /** 188 | * @brief Compute and return error / cost value. 189 | * 190 | * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. 191 | * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T 192 | */ 193 | ErrorVector& getError() 194 | { 195 | computeError(); 196 | return _error; 197 | } 198 | 199 | /** 200 | * @brief Read values from input stream 201 | */ 202 | virtual bool read(std::istream& is) 203 | { 204 | // TODO generic read 205 | return true; 206 | } 207 | 208 | /** 209 | * @brief Write values to an output stream 210 | */ 211 | virtual bool write(std::ostream& os) const 212 | { 213 | // TODO generic write 214 | return os.good(); 215 | } 216 | 217 | /** 218 | * @brief Assign the TebConfig class for parameters. 219 | * @param cfg TebConfig class 220 | */ 221 | void setTebConfig(const TebConfig& cfg) 222 | { 223 | cfg_ = &cfg; 224 | } 225 | 226 | protected: 227 | 228 | using g2o::BaseBinaryEdge::_error; 229 | using g2o::BaseBinaryEdge::_vertices; 230 | 231 | const TebConfig* cfg_; //!< Store TebConfig class for parameters 232 | 233 | public: 234 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 235 | }; 236 | 237 | 238 | /** 239 | * @class BaseTebMultiEdge 240 | * @brief Base edge connecting two vertices in the TEB optimization problem 241 | * 242 | * This edge defines a base edge type for the TEB optimization problem. 243 | * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). 244 | * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. 245 | * Memory of edges should be freed by calling the clearEdge method of the g2o optimzier class. 246 | * @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge 247 | */ 248 | template 249 | class BaseTebMultiEdge : public g2o::BaseMultiEdge 250 | { 251 | public: 252 | 253 | using typename g2o::BaseMultiEdge::ErrorVector; 254 | using g2o::BaseMultiEdge::computeError; 255 | 256 | /** 257 | * @brief Construct edge. 258 | */ 259 | BaseTebMultiEdge() 260 | { 261 | // for(std::size_t i=0; i<_vertices.size(); ++i) 262 | // _vertices[i] = NULL; 263 | } 264 | 265 | /** 266 | * @brief Destruct edge. 267 | * 268 | * We need to erase vertices manually, since we want to keep them even if TebOptimalPlanner::clearGraph() is called. 269 | * This is necessary since the vertices are managed by the Timed_Elastic_Band class. 270 | */ 271 | virtual ~BaseTebMultiEdge() 272 | { 273 | for(std::size_t i=0; i<_vertices.size(); ++i) 274 | { 275 | if(_vertices[i]) 276 | _vertices[i]->edges().erase(this); 277 | } 278 | } 279 | 280 | // Overwrites resize() from the parent class 281 | virtual void resize(size_t size) 282 | { 283 | g2o::BaseMultiEdge::resize(size); 284 | 285 | for(std::size_t i=0; i<_vertices.size(); ++i) 286 | _vertices[i] = NULL; 287 | } 288 | 289 | /** 290 | * @brief Compute and return error / cost value. 291 | * 292 | * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. 293 | * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T 294 | */ 295 | ErrorVector& getError() 296 | { 297 | computeError(); 298 | return _error; 299 | } 300 | 301 | /** 302 | * @brief Read values from input stream 303 | */ 304 | virtual bool read(std::istream& is) 305 | { 306 | // TODO generic read 307 | return true; 308 | } 309 | 310 | /** 311 | * @brief Write values to an output stream 312 | */ 313 | virtual bool write(std::ostream& os) const 314 | { 315 | // TODO generic write 316 | return os.good(); 317 | } 318 | 319 | /** 320 | * @brief Assign the TebConfig class for parameters. 321 | * @param cfg TebConfig class 322 | */ 323 | void setTebConfig(const TebConfig& cfg) 324 | { 325 | cfg_ = &cfg; 326 | } 327 | 328 | protected: 329 | 330 | using g2o::BaseMultiEdge::_error; 331 | using g2o::BaseMultiEdge::_vertices; 332 | 333 | const TebConfig* cfg_; //!< Store TebConfig class for parameters 334 | 335 | public: 336 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 337 | }; 338 | 339 | 340 | 341 | 342 | 343 | 344 | } // end namespace 345 | 346 | #endif 347 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann, Franz Albers 42 | *********************************************************************/ 43 | 44 | #ifndef EDGE_DYNAMICOBSTACLE_H 45 | #define EDGE_DYNAMICOBSTACLE_H 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | namespace teb_local_planner 56 | { 57 | 58 | /** 59 | * @class EdgeDynamicObstacle 60 | * @brief Edge defining the cost function for keeping a distance from dynamic (moving) obstacles. 61 | * 62 | * The edge depends on two vertices \f$ \mathbf{s}_i, \Delta T_i \f$ and minimizes: \n 63 | * \f$ \min \textrm{penaltyBelow}( dist2obstacle) \cdot weight \f$. \n 64 | * \e dist2obstacle denotes the minimum distance to the obstacle trajectory (spatial and temporal). \n 65 | * \e weight can be set using setInformation(). \n 66 | * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow(). \n 67 | * @see TebOptimalPlanner::AddEdgesDynamicObstacles 68 | * @remarks Do not forget to call setTebConfig(), setVertexIdx() and 69 | * @warning Experimental 70 | */ 71 | class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose> 72 | { 73 | public: 74 | 75 | /** 76 | * @brief Construct edge. 77 | */ 78 | EdgeDynamicObstacle() : t_(0) 79 | { 80 | } 81 | 82 | /** 83 | * @brief Construct edge and specify the time for its associated pose (neccessary for computeError). 84 | * @param t_ Estimated time until current pose is reached 85 | */ 86 | EdgeDynamicObstacle(double t) : t_(t) 87 | { 88 | } 89 | 90 | /** 91 | * @brief Actual cost function 92 | */ 93 | void computeError() 94 | { 95 | ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()"); 96 | const VertexPose* bandpt = static_cast(_vertices[0]); 97 | 98 | double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_); 99 | 100 | _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); 101 | _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0); 102 | 103 | ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]); 104 | } 105 | 106 | 107 | /** 108 | * @brief Set Obstacle for the underlying cost function 109 | * @param obstacle Const pointer to an Obstacle or derived Obstacle 110 | */ 111 | void setObstacle(const Obstacle* obstacle) 112 | { 113 | _measurement = obstacle; 114 | } 115 | 116 | /** 117 | * @brief Set pointer to the robot model 118 | * @param robot_model Robot model required for distance calculation 119 | */ 120 | void setRobotModel(const BaseRobotFootprintModel* robot_model) 121 | { 122 | robot_model_ = robot_model; 123 | } 124 | 125 | /** 126 | * @brief Set all parameters at once 127 | * @param cfg TebConfig class 128 | * @param robot_model Robot model required for distance calculation 129 | * @param obstacle 2D position vector containing the position of the obstacle 130 | */ 131 | void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) 132 | { 133 | cfg_ = &cfg; 134 | robot_model_ = robot_model; 135 | _measurement = obstacle; 136 | } 137 | 138 | protected: 139 | 140 | const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model 141 | double t_; //!< Estimated time until current pose is reached 142 | 143 | public: 144 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 145 | 146 | }; 147 | 148 | 149 | 150 | 151 | } // end namespace 152 | 153 | #endif 154 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/edge_kinematics.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann 42 | *********************************************************************/ 43 | 44 | #ifndef _EDGE_KINEMATICS_H 45 | #define _EDGE_KINEMATICS_H 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | 54 | namespace teb_local_planner 55 | { 56 | 57 | /** 58 | * @class EdgeKinematicsDiffDrive 59 | * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot. 60 | * 61 | * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation 62 | * of the non-holonomic constraint: 63 | * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. 64 | * 65 | * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n 66 | * A second equation is implemented to penalize backward motions (second element of the error /cost vector). \n 67 | * The \e weight can be set using setInformation(): Matrix element 2,2: (A value ~1 allows backward driving, but penalizes it slighly). \n 68 | * The dimension of the error / cost vector is 2: the first component represents the nonholonomic constraint cost, 69 | * the second one backward-drive cost. 70 | * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsCarlike 71 | * @remarks Do not forget to call setTebConfig() 72 | */ 73 | class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose> 74 | { 75 | public: 76 | 77 | /** 78 | * @brief Construct edge. 79 | */ 80 | EdgeKinematicsDiffDrive() 81 | { 82 | this->setMeasurement(0.); 83 | } 84 | 85 | /** 86 | * @brief Actual cost function 87 | */ 88 | void computeError() 89 | { 90 | ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()"); 91 | const VertexPose* conf1 = static_cast(_vertices[0]); 92 | const VertexPose* conf2 = static_cast(_vertices[1]); 93 | 94 | Eigen::Vector2d deltaS = conf2->position() - conf1->position(); 95 | 96 | // non holonomic constraint 97 | _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); 98 | 99 | // positive-drive-direction constraint 100 | Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) ); 101 | _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0); 102 | // epsilon=0, otherwise it pushes the first bandpoints away from start 103 | 104 | ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); 105 | } 106 | 107 | #ifdef USE_ANALYTIC_JACOBI 108 | #if 1 109 | /** 110 | * @brief Jacobi matrix of the cost function specified in computeError(). 111 | */ 112 | void linearizeOplus() 113 | { 114 | ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()"); 115 | const VertexPose* conf1 = static_cast(_vertices[0]); 116 | const VertexPose* conf2 = static_cast(_vertices[1]); 117 | 118 | Eigen::Vector2d deltaS = conf2->position() - conf1->position(); 119 | 120 | double cos1 = cos(conf1->theta()); 121 | double cos2 = cos(conf2->theta()); 122 | double sin1 = sin(conf1->theta()); 123 | double sin2 = sin(conf2->theta()); 124 | double aux1 = sin1 + sin2; 125 | double aux2 = cos1 + cos2; 126 | 127 | double dd_error_1 = deltaS[0]*cos1; 128 | double dd_error_2 = deltaS[1]*sin1; 129 | double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0); 130 | 131 | double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - 132 | ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); 133 | 134 | // conf1 135 | _jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1 136 | _jacobianOplusXi(0,1) = -aux2 * dev_nh_abs; // nh y1 137 | _jacobianOplusXi(1,0) = -cos1 * dd_dev; // drive-dir x1 138 | _jacobianOplusXi(1,1) = -sin1 * dd_dev; // drive-dir y1 139 | _jacobianOplusXi(0,2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle 140 | _jacobianOplusXi(1,2) = ( -sin1*deltaS[0] + cos1*deltaS[1] ) * dd_dev; // drive-dir angle1 141 | 142 | // conf2 143 | _jacobianOplusXj(0,0) = -aux1 * dev_nh_abs; // nh x2 144 | _jacobianOplusXj(0,1) = aux2 * dev_nh_abs; // nh y2 145 | _jacobianOplusXj(1,0) = cos1 * dd_dev; // drive-dir x2 146 | _jacobianOplusXj(1,1) = sin1 * dd_dev; // drive-dir y2 147 | _jacobianOplusXj(0,2) = (-sin2*deltaS[1] - cos2*deltaS[0]) * dev_nh_abs; // nh angle 148 | _jacobianOplusXj(1,2) = 0; // drive-dir angle1 149 | } 150 | #endif 151 | #endif 152 | 153 | public: 154 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 155 | }; 156 | 157 | 158 | 159 | 160 | /** 161 | * @class EdgeKinematicsCarlike 162 | * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot. 163 | * 164 | * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation 165 | * of the non-holonomic constraint: 166 | * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. 167 | * 168 | * The definition is identically to the one of the differential drive robot. 169 | * Additionally, this edge incorporates a minimum turning radius that is required by carlike robots. 170 | * The turning radius is defined by \f$ r=v/omega \f$. 171 | * 172 | * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n 173 | * The second equation enforces a minimum turning radius. 174 | * The \e weight can be set using setInformation(): Matrix element 2,2. \n 175 | * The dimension of the error / cost vector is 3: the first component represents the nonholonomic constraint cost, 176 | * the second one backward-drive cost and the third one the minimum turning radius 177 | * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsDiffDrive 178 | * @remarks Bounding the turning radius from below is not affected by the penalty_epsilon parameter, 179 | * the user might add an extra margin to the min_turning_radius param. 180 | * @remarks Do not forget to call setTebConfig() 181 | */ 182 | class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose> 183 | { 184 | public: 185 | 186 | /** 187 | * @brief Construct edge. 188 | */ 189 | EdgeKinematicsCarlike() 190 | { 191 | this->setMeasurement(0.); 192 | } 193 | 194 | /** 195 | * @brief Actual cost function 196 | */ 197 | void computeError() 198 | { 199 | ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()"); 200 | const VertexPose* conf1 = static_cast(_vertices[0]); 201 | const VertexPose* conf2 = static_cast(_vertices[1]); 202 | 203 | Eigen::Vector2d deltaS = conf2->position() - conf1->position(); 204 | 205 | // non holonomic constraint 206 | _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); 207 | 208 | // limit minimum turning radius 209 | double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); 210 | if (angle_diff == 0) 211 | _error[1] = 0; // straight line motion 212 | else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius 213 | _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0); 214 | else 215 | _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0); 216 | // This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter. 217 | 218 | ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); 219 | } 220 | 221 | public: 222 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 223 | }; 224 | 225 | 226 | 227 | 228 | } // end namespace 229 | 230 | #endif 231 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/edge_obstacle.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann 42 | *********************************************************************/ 43 | #ifndef EDGE_OBSTACLE_H_ 44 | #define EDGE_OBSTACLE_H_ 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | 54 | 55 | namespace teb_local_planner 56 | { 57 | 58 | /** 59 | * @class EdgeObstacle 60 | * @brief Edge defining the cost function for keeping a minimum distance from obstacles. 61 | * 62 | * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n 63 | * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n 64 | * \e dist2point denotes the minimum distance to the point obstacle. \n 65 | * \e weight can be set using setInformation(). \n 66 | * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n 67 | * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle 68 | * @remarks Do not forget to call setTebConfig() and setObstacle() 69 | */ 70 | class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose> 71 | { 72 | public: 73 | 74 | /** 75 | * @brief Construct edge. 76 | */ 77 | EdgeObstacle() 78 | { 79 | _measurement = NULL; 80 | } 81 | 82 | /** 83 | * @brief Actual cost function 84 | */ 85 | void computeError() 86 | { 87 | ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()"); 88 | const VertexPose* bandpt = static_cast(_vertices[0]); 89 | 90 | double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); 91 | 92 | _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); 93 | 94 | ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]); 95 | } 96 | 97 | #ifdef USE_ANALYTIC_JACOBI 98 | #if 0 99 | 100 | /** 101 | * @brief Jacobi matrix of the cost function specified in computeError(). 102 | */ 103 | void linearizeOplus() 104 | { 105 | ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgePointObstacle()"); 106 | const VertexPose* bandpt = static_cast(_vertices[0]); 107 | 108 | Eigen::Vector2d deltaS = *_measurement - bandpt->position(); 109 | double angdiff = atan2(deltaS[1],deltaS[0])-bandpt->theta(); 110 | 111 | double dist_squared = deltaS.squaredNorm(); 112 | double dist = sqrt(dist_squared); 113 | 114 | double aux0 = sin(angdiff); 115 | double dev_left_border = penaltyBoundFromBelowDerivative(dist*fabs(aux0),cfg_->obstacles.min_obstacle_dist,cfg_->optim.penalty_epsilon); 116 | 117 | if (dev_left_border==0) 118 | { 119 | _jacobianOplusXi( 0 , 0 ) = 0; 120 | _jacobianOplusXi( 0 , 1 ) = 0; 121 | _jacobianOplusXi( 0 , 2 ) = 0; 122 | return; 123 | } 124 | 125 | double aux1 = -fabs(aux0) / dist; 126 | double dev_norm_x = deltaS[0]*aux1; 127 | double dev_norm_y = deltaS[1]*aux1; 128 | 129 | double aux2 = cos(angdiff) * g2o::sign(aux0); 130 | double aux3 = aux2 / dist_squared; 131 | double dev_proj_x = aux3 * deltaS[1] * dist; 132 | double dev_proj_y = -aux3 * deltaS[0] * dist; 133 | double dev_proj_angle = -aux2; 134 | 135 | _jacobianOplusXi( 0 , 0 ) = dev_left_border * ( dev_norm_x + dev_proj_x ); 136 | _jacobianOplusXi( 0 , 1 ) = dev_left_border * ( dev_norm_y + dev_proj_y ); 137 | _jacobianOplusXi( 0 , 2 ) = dev_left_border * dev_proj_angle; 138 | } 139 | #endif 140 | #endif 141 | 142 | /** 143 | * @brief Set pointer to associated obstacle for the underlying cost function 144 | * @param obstacle 2D position vector containing the position of the obstacle 145 | */ 146 | void setObstacle(const Obstacle* obstacle) 147 | { 148 | _measurement = obstacle; 149 | } 150 | 151 | /** 152 | * @brief Set pointer to the robot model 153 | * @param robot_model Robot model required for distance calculation 154 | */ 155 | void setRobotModel(const BaseRobotFootprintModel* robot_model) 156 | { 157 | robot_model_ = robot_model; 158 | } 159 | 160 | /** 161 | * @brief Set all parameters at once 162 | * @param cfg TebConfig class 163 | * @param robot_model Robot model required for distance calculation 164 | * @param obstacle 2D position vector containing the position of the obstacle 165 | */ 166 | void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) 167 | { 168 | cfg_ = &cfg; 169 | robot_model_ = robot_model; 170 | _measurement = obstacle; 171 | } 172 | 173 | protected: 174 | 175 | const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model 176 | 177 | public: 178 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 179 | 180 | }; 181 | 182 | 183 | 184 | /** 185 | * @class EdgeInflatedObstacle 186 | * @brief Edge defining the cost function for keeping a minimum distance from inflated obstacles. 187 | * 带膨胀层的障碍物 188 | * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n 189 | * \f$ \min \textrm{penaltyBelow}( dist2point, min_obstacle_dist ) \cdot weight_inflation \f$. \n 190 | * Additional, a second penalty is provided with \n 191 | * \f$ \min \textrm{penaltyBelow}( dist2point, inflation_dist ) \cdot weight_inflation \f$. 192 | * It is assumed that inflation_dist > min_obstacle_dist and weight_inflation << weight_inflation. 193 | * \e dist2point denotes the minimum distance to the point obstacle. \n 194 | * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n 195 | * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeObstacle 196 | * @remarks Do not forget to call setTebConfig() and setObstacle() 197 | */ 198 | class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose> 199 | { 200 | public: 201 | 202 | /** 203 | * @brief Construct edge. 204 | */ 205 | EdgeInflatedObstacle() 206 | { 207 | _measurement = NULL; 208 | } 209 | 210 | /** 211 | * @brief Actual cost function 212 | */ 213 | void computeError() 214 | { 215 | ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()"); 216 | const VertexPose* bandpt = static_cast(_vertices[0]); 217 | 218 | double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); 219 | 220 | _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); 221 | _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0); 222 | 223 | 224 | ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeObstacle::computeError() _error[0]=%f, _error[1]=%f\n",_error[0], _error[1]); 225 | } 226 | 227 | /** 228 | * @brief Set pointer to associated obstacle for the underlying cost function 229 | * @param obstacle 2D position vector containing the position of the obstacle 230 | */ 231 | void setObstacle(const Obstacle* obstacle) 232 | { 233 | _measurement = obstacle; 234 | } 235 | 236 | /** 237 | * @brief Set pointer to the robot model 238 | * @param robot_model Robot model required for distance calculation 239 | */ 240 | void setRobotModel(const BaseRobotFootprintModel* robot_model) 241 | { 242 | robot_model_ = robot_model; 243 | } 244 | 245 | /** 246 | * @brief Set all parameters at once 247 | * @param cfg TebConfig class 248 | * @param robot_model Robot model required for distance calculation 249 | * @param obstacle 2D position vector containing the position of the obstacle 250 | */ 251 | void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) 252 | { 253 | cfg_ = &cfg; 254 | robot_model_ = robot_model; 255 | _measurement = obstacle; 256 | } 257 | 258 | protected: 259 | 260 | const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model 261 | 262 | public: 263 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 264 | 265 | }; 266 | 267 | 268 | } // end namespace 269 | 270 | #endif 271 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/edge_prefer_rotdir.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann 42 | *********************************************************************/ 43 | #ifndef EDGE_PREFER_ROTDIR_H_ 44 | #define EDGE_PREFER_ROTDIR_H_ 45 | 46 | #include 47 | #include 48 | #include 49 | #include "g2o/core/base_unary_edge.h" 50 | 51 | 52 | namespace teb_local_planner 53 | { 54 | 55 | /** 56 | * @class EdgePreferRotDir 57 | * @brief Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns 58 | * 59 | * The edge depends on two consecutive vertices \f$ \mathbf{s}_i, \mathbf{s}_{i+1} \f$ and penalizes a given rotation direction 60 | * based on the \e weight and \e dir (\f$ dir \in \{-1,1\} \f$) 61 | * \e dir should be +1 to prefer left rotations and -1 to prefer right rotations \n 62 | * \e weight can be set using setInformation(). \n 63 | * @see TebOptimalPlanner::AddEdgePreferRotDir 64 | */ 65 | class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> 66 | { 67 | public: 68 | 69 | /** 70 | * @brief Construct edge. 71 | */ 72 | EdgePreferRotDir() 73 | { 74 | _measurement = 1; 75 | } 76 | 77 | /** 78 | * @brief Actual cost function 79 | */ 80 | void computeError() 81 | { 82 | const VertexPose* conf1 = static_cast(_vertices[0]); 83 | const VertexPose* conf2 = static_cast(_vertices[1]); 84 | 85 | _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0); 86 | 87 | ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]); 88 | } 89 | 90 | /** 91 | * @brief Specify the prefered direction of rotation 92 | * @param dir +1 to prefer the left side, -1 to prefer the right side 93 | */ 94 | void setRotDir(double dir) 95 | { 96 | _measurement = dir; 97 | } 98 | 99 | /** Prefer rotations to the right */ 100 | void preferRight() {_measurement = -1;} 101 | 102 | /** Prefer rotations to the right */ 103 | void preferLeft() {_measurement = 1;} 104 | 105 | 106 | public: 107 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 108 | 109 | }; 110 | 111 | 112 | 113 | } // end namespace 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/edge_time_optimal.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann 42 | *********************************************************************/ 43 | 44 | #ifndef EDGE_TIMEOPTIMAL_H_ 45 | #define EDGE_TIMEOPTIMAL_H_ 46 | 47 | #include 48 | 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | #include 57 | 58 | namespace teb_local_planner 59 | { 60 | 61 | 62 | /** 63 | * @class EdgeTimeOptimal 64 | * @brief Edge defining the cost function for minimizing transition time of the trajectory. 65 | * 66 | * The edge depends on a single vertex \f$ \Delta T_i \f$ and minimizes: \n 67 | * \f$ \min \Delta T_i^2 \cdot scale \cdot weight \f$. \n 68 | * \e scale is determined using the penaltyEquality() function, since we experiences good convergence speeds with it. \n 69 | * \e weight can be set using setInformation() (something around 1.0 seems to be fine). \n 70 | * @see TebOptimalPlanner::AddEdgesTimeOptimal 71 | * @remarks Do not forget to call setTebConfig() 72 | */ 73 | class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff> 74 | { 75 | public: 76 | 77 | /** 78 | * @brief Construct edge. 79 | */ 80 | EdgeTimeOptimal() 81 | { 82 | this->setMeasurement(0.); 83 | } 84 | 85 | /** 86 | * @brief Actual cost function 87 | */ 88 | void computeError() 89 | { 90 | ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()"); 91 | const VertexTimeDiff* timediff = static_cast(_vertices[0]); 92 | 93 | _error[0] = timediff->dt(); 94 | 95 | ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]); 96 | } 97 | 98 | #ifdef USE_ANALYTIC_JACOBI 99 | /** 100 | * @brief Jacobi matrix of the cost function specified in computeError(). 101 | */ 102 | void linearizeOplus() 103 | { 104 | ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()"); 105 | _jacobianOplusXi( 0 , 0 ) = 1; 106 | } 107 | #endif 108 | 109 | 110 | public: 111 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 112 | }; 113 | 114 | }; // end namespace 115 | 116 | #endif /* EDGE_TIMEOPTIMAL_H_ */ 117 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/edge_via_point.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann 42 | *********************************************************************/ 43 | #ifndef EDGE_VIA_POINT_H_ 44 | #define EDGE_VIA_POINT_H_ 45 | 46 | #include 47 | #include 48 | 49 | #include "g2o/core/base_unary_edge.h" 50 | 51 | 52 | namespace teb_local_planner 53 | { 54 | 55 | /** 56 | * @class EdgeViaPoint 57 | * @brief Edge defining the cost function for pushing a configuration towards a via point 58 | * 59 | * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n 60 | * \f$ \min dist2point \cdot weight \f$. \n 61 | * \e dist2point denotes the distance to the via point. \n 62 | * \e weight can be set using setInformation(). \n 63 | * @see TebOptimalPlanner::AddEdgesViaPoints 64 | * @remarks Do not forget to call setTebConfig() and setViaPoint() 65 | */ 66 | class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose> 67 | { 68 | public: 69 | 70 | /** 71 | * @brief Construct edge. 72 | */ 73 | EdgeViaPoint() 74 | { 75 | _measurement = NULL; 76 | } 77 | 78 | /** 79 | * @brief Actual cost function 80 | */ 81 | void computeError() 82 | { 83 | ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()"); 84 | const VertexPose* bandpt = static_cast(_vertices[0]); 85 | 86 | _error[0] = (bandpt->position() - *_measurement).norm(); 87 | 88 | ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]); 89 | } 90 | 91 | /** 92 | * @brief Set pointer to associated via point for the underlying cost function 93 | * @param via_point 2D position vector containing the position of the via point 94 | */ 95 | void setViaPoint(const Eigen::Vector2d* via_point) 96 | { 97 | _measurement = via_point; 98 | } 99 | 100 | /** 101 | * @brief Set all parameters at once 102 | * @param cfg TebConfig class 103 | * @param via_point 2D position vector containing the position of the via point 104 | */ 105 | void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point) 106 | { 107 | cfg_ = &cfg; 108 | _measurement = via_point; 109 | } 110 | 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 113 | 114 | }; 115 | 116 | 117 | 118 | } // end namespace 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/penalties.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #ifndef PENALTIES_H 40 | #define PENALTIES_H 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | namespace teb_local_planner 47 | { 48 | 49 | /** 50 | * @brief Linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$ 51 | * @param var The scalar that should be bounded 52 | * @param a lower and upper absolute bound 53 | * @param epsilon safty margin (move bound to the interior of the interval) 54 | * @see penaltyBoundToIntervalDerivative 55 | * @return Penalty / cost value that is nonzero if the constraint is not satisfied 56 | */ 57 | inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon) 58 | { 59 | if (var < -a+epsilon) 60 | { 61 | return (-var - (a - epsilon)); 62 | } 63 | if (var <= a-epsilon) 64 | { 65 | return 0.; 66 | } 67 | else 68 | { 69 | return (var - (a - epsilon)); 70 | } 71 | } 72 | 73 | /** 74 | * @brief Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ 75 | * @param var The scalar that should be bounded 76 | * @param a lower bound 77 | * @param b upper bound 78 | * @param epsilon safty margin (move bound to the interior of the interval) 79 | * @see penaltyBoundToIntervalDerivative 80 | * @return Penalty / cost value that is nonzero if the constraint is not satisfied 81 | */ 82 | inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon) 83 | { 84 | if (var < a+epsilon) 85 | { 86 | return (-var + (a + epsilon)); 87 | } 88 | if (var <= b-epsilon) 89 | { 90 | return 0.; 91 | } 92 | else 93 | { 94 | return (var - (b - epsilon)); 95 | } 96 | } 97 | 98 | 99 | /** 100 | * @brief Linear penalty function for bounding \c var from below: \f$ a < var \f$ 101 | * @param var The scalar that should be bounded 102 | * @param a lower bound 103 | * @param epsilon safty margin (move bound to the interior of the interval) 104 | * @see penaltyBoundFromBelowDerivative 105 | * @return Penalty / cost value that is nonzero if the constraint is not satisfied 106 | */ 107 | inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon) 108 | { 109 | if (var >= a+epsilon) 110 | { 111 | return 0.; 112 | } 113 | else 114 | { 115 | return (-var + (a+epsilon)); 116 | } 117 | } 118 | 119 | /** 120 | * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$ 121 | * @param var The scalar that should be bounded 122 | * @param a lower and upper absolute bound 123 | * @param epsilon safty margin (move bound to the interior of the interval) 124 | * @see penaltyBoundToInterval 125 | * @return Derivative of the penalty function w.r.t. \c var 126 | */ 127 | inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon) 128 | { 129 | if (var < -a+epsilon) 130 | { 131 | return -1; 132 | } 133 | if (var <= a-epsilon) 134 | { 135 | return 0.; 136 | } 137 | else 138 | { 139 | return 1; 140 | } 141 | } 142 | 143 | /** 144 | * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ 145 | * @param var The scalar that should be bounded 146 | * @param a lower bound 147 | * @param b upper bound 148 | * @param epsilon safty margin (move bound to the interior of the interval) 149 | * @see penaltyBoundToInterval 150 | * @return Derivative of the penalty function w.r.t. \c var 151 | */ 152 | inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& b, const double& epsilon) 153 | { 154 | if (var < a+epsilon) 155 | { 156 | return -1; 157 | } 158 | if (var <= b-epsilon) 159 | { 160 | return 0.; 161 | } 162 | else 163 | { 164 | return 1; 165 | } 166 | } 167 | 168 | 169 | /** 170 | * @brief Derivative of the linear penalty function for bounding \c var from below: \f$ a < var \f$ 171 | * @param var The scalar that should be bounded 172 | * @param a lower bound 173 | * @param epsilon safty margin (move bound to the interior of the interval) 174 | * @see penaltyBoundFromBelow 175 | * @return Derivative of the penalty function w.r.t. \c var 176 | */ 177 | inline double penaltyBoundFromBelowDerivative(const double& var, const double& a,const double& epsilon) 178 | { 179 | if (var >= a+epsilon) 180 | { 181 | return 0.; 182 | } 183 | else 184 | { 185 | return -1; 186 | } 187 | } 188 | 189 | 190 | } // namespace teb_local_planner 191 | 192 | 193 | #endif // PENALTIES_H 194 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/vertex_pose.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann 42 | *********************************************************************/ 43 | 44 | #ifndef VERTEX_POSE_H_ 45 | #define VERTEX_POSE_H_ 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | 54 | namespace teb_local_planner 55 | { 56 | 57 | /** 58 | * @class VertexPose 59 | * @brief This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o 60 | * @see PoseSE2 61 | * @see VertexTimeDiff 62 | */ 63 | // 优化变量的维度为3， 数据类型为PoseSE2 64 | class VertexPose : public g2o::BaseVertex<3, PoseSE2 > 65 | { 66 | public: 67 | 68 | /** 69 | * @brief Default constructor 70 | * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] 71 | */ 72 | VertexPose(bool fixed = false) 73 | { 74 | setToOriginImpl(); 75 | setFixed(fixed); 76 | } 77 | 78 | /** 79 | * @brief Construct pose using a given PoseSE2 80 | * @param pose PoseSE2 defining the pose [x, y, angle_rad] 81 | * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] 82 | */ 83 | VertexPose(const PoseSE2& pose, bool fixed = false) 84 | { 85 | _estimate = pose; 86 | setFixed(fixed); 87 | } 88 | 89 | /** 90 | * @brief Construct pose using a given 2D position vector and orientation 91 | * @param position Eigen::Vector2d containing x and y coordinates 92 | * @param theta yaw-angle 93 | * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] 94 | */ 95 | VertexPose(const Eigen::Ref& position, double theta, bool fixed = false) 96 | { 97 | _estimate.position() = position; 98 | _estimate.theta() = theta; 99 | setFixed(fixed); 100 | } 101 | 102 | /** 103 | * @brief Construct pose using single components x, y, and the yaw angle 104 | * @param x x-coordinate 105 | * @param y y-coordinate 106 | * @param theta yaw angle in rad 107 | * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] 108 | */ 109 | VertexPose(double x, double y, double theta, bool fixed = false) 110 | { 111 | _estimate.x() = x; 112 | _estimate.y() = y; 113 | _estimate.theta() = theta; 114 | setFixed(fixed); 115 | } 116 | 117 | /** 118 | * @brief Destructs the VertexPose 119 | */ 120 | ~VertexPose() {} 121 | 122 | /** 123 | * @brief Access the pose 124 | * @see estimate 125 | * @return reference to the PoseSE2 estimate 126 | */ 127 | PoseSE2& pose() {return _estimate;} 128 | 129 | /** 130 | * @brief Access the pose (read-only) 131 | * @see estimate 132 | * @return const reference to the PoseSE2 estimate 133 | */ 134 | const PoseSE2& pose() const {return _estimate;} 135 | 136 | 137 | /** 138 | * @brief Access the 2D position part 139 | * @see estimate 140 | * @return reference to the 2D position part 141 | */ 142 | Eigen::Vector2d& position() {return _estimate.position();} 143 | 144 | /** 145 | * @brief Access the 2D position part (read-only) 146 | * @see estimate 147 | * @return const reference to the 2D position part 148 | */ 149 | const Eigen::Vector2d& position() const {return _estimate.position();} 150 | 151 | /** 152 | * @brief Access the x-coordinate the pose 153 | * @return reference to the x-coordinate 154 | */ 155 | double& x() {return _estimate.x();} 156 | 157 | /** 158 | * @brief Access the x-coordinate the pose (read-only) 159 | * @return const reference to the x-coordinate 160 | */ 161 | const double& x() const {return _estimate.x();} 162 | 163 | /** 164 | * @brief Access the y-coordinate the pose 165 | * @return reference to the y-coordinate 166 | */ 167 | double& y() {return _estimate.y();} 168 | 169 | /** 170 | * @brief Access the y-coordinate the pose (read-only) 171 | * @return const reference to the y-coordinate 172 | */ 173 | const double& y() const {return _estimate.y();} 174 | 175 | /** 176 | * @brief Access the orientation part (yaw angle) of the pose 177 | * @return reference to the yaw angle 178 | */ 179 | double& theta() {return _estimate.theta();} 180 | 181 | /** 182 | * @brief Access the orientation part (yaw angle) of the pose (read-only) 183 | * @return const reference to the yaw angle 184 | */ 185 | const double& theta() const {return _estimate.theta();} 186 | 187 | /** 188 | * @brief Set the underlying estimate (2D vector) to zero. 189 | */ 190 | virtual void setToOriginImpl() 191 | { 192 | _estimate.setZero(); 193 | } 194 | 195 | /** 196 | * @brief Define the update increment \f$ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} \f$. 197 | * A simple addition for the position. 198 | * The angle is first added to the previous estimated angle and afterwards normalized to the interval \f$ [-\pi \pi] \f$ 199 | * @param update increment that should be added to the previous esimate 200 | */ 201 | virtual void oplusImpl(const double* update) 202 | { 203 | _estimate.plus(update); 204 | } 205 | 206 | /** 207 | * @brief Read an estimate from an input stream. 208 | * First the x-coordinate followed by y and the yaw angle. 209 | * @param is input stream 210 | * @return always \c true 211 | */ 212 | virtual bool read(std::istream& is) 213 | { 214 | is >> _estimate.x() >> _estimate.y() >> _estimate.theta(); 215 | return true; 216 | } 217 | 218 | /** 219 | * @brief Write the estimate to an output stream 220 | * First the x-coordinate followed by y and the yaw angle. 221 | * @param os output stream 222 | * @return \c true if the export was successful, otherwise \c false 223 | */ 224 | virtual bool write(std::ostream& os) const 225 | { 226 | os << _estimate.x() << " " << _estimate.y() << _estimate.theta(); 227 | return os.good(); 228 | } 229 | 230 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 231 | }; 232 | 233 | } 234 | 235 | #endif // VERTEX_POSE_H_ 236 | -------------------------------------------------------------------------------- /include/teb_local_planner/g2o_types/vertex_timediff.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Notes: 37 | * The following class is derived from a class defined by the 38 | * g2o-framework. g2o is licensed under the terms of the BSD License. 39 | * Refer to the base class source for detailed licensing information. 40 | * 41 | * Author: Christoph Rösmann 42 | *********************************************************************/ 43 | 44 | #ifndef VERTEX_TIMEDIFF_H 45 | #define VERTEX_TIMEDIFF_H 46 | 47 | 48 | #include "g2o/config.h" 49 | #include "g2o/core/base_vertex.h" 50 | #include "g2o/core/hyper_graph_action.h" 51 | 52 | #include "ros/console.h" 53 | 54 | #include 55 | 56 | namespace teb_local_planner 57 | { 58 | 59 | /** 60 | * @class VertexTimeDiff 61 | * @brief This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o 62 | * @see VertexPointXY 63 | * @see VertexOrientation 64 | */ 65 | // 时间差分顶点 66 | class VertexTimeDiff : public g2o::BaseVertex<1, double> 67 | { 68 | public: 69 | 70 | /** 71 | * @brief Default constructor 72 | * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] 73 | */ 74 | VertexTimeDiff(bool fixed = false) 75 | { 76 | setToOriginImpl(); 77 | setFixed(fixed); 78 | } 79 | 80 | /** 81 | * @brief Construct the TimeDiff vertex with a value 82 | * @param dt time difference value of the vertex 83 | * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] 84 | */ 85 | VertexTimeDiff(double dt, bool fixed = false) 86 | { 87 | _estimate = dt; 88 | setFixed(fixed); 89 | } 90 | 91 | /** 92 | * @brief Destructs the VertexTimeDiff 93 | */ 94 | ~VertexTimeDiff() 95 | {} 96 | 97 | /** 98 | * @brief Access the timediff value of the vertex 99 | * @see estimate 100 | * @return reference to dt 101 | */ 102 | double& dt() {return _estimate;} 103 | 104 | /** 105 | * @brief Access the timediff value of the vertex (read-only) 106 | * @see estimate 107 | * @return const reference to dt 108 | */ 109 | const double& dt() const {return _estimate;} 110 | 111 | /** 112 | * @brief Set the underlying TimeDiff estimate \f$ \Delta T \f$ to default. 113 | */ 114 | virtual void setToOriginImpl() 115 | { 116 | _estimate = 0.1; 117 | } 118 | 119 | /** 120 | * @brief Define the update increment \f$ \Delta T_{k+1} = \Delta T_k + update \f$. 121 | * A simple addition implements what we want. 122 | * @param update increment that should be added to the previous esimate 123 | */ 124 | virtual void oplusImpl(const double* update) 125 | { 126 | _estimate += *update; 127 | } 128 | 129 | /** 130 | * @brief Read an estimate of \f$ \Delta T \f$ from an input stream 131 | * @param is input stream 132 | * @return always \c true 133 | */ 134 | virtual bool read(std::istream& is) 135 | { 136 | is >> _estimate; 137 | return true; 138 | } 139 | 140 | /** 141 | * @brief Write the estimate \f$ \Delta T \f$ to an output stream 142 | * @param os output stream 143 | * @return \c true if the export was successful, otherwise \c false 144 | */ 145 | virtual bool write(std::ostream& os) const 146 | { 147 | os << estimate(); 148 | return os.good(); 149 | } 150 | 151 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 152 | }; 153 | 154 | } 155 | 156 | #endif 157 | -------------------------------------------------------------------------------- /include/teb_local_planner/graph_search.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2017, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Christoph Rösmann, Franz Albers 37 | *********************************************************************/ 38 | 39 | #ifndef GRAPH_SEARCH_INTERFACE_H 40 | #define GRAPH_SEARCH_INTERFACE_H 41 | 42 | #ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS 43 | #include 44 | #else 45 | // Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48: 46 | // boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated, 47 | // but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now. 48 | #define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS 49 | #include 50 | #undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS 51 | #endif 52 | 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | #include 59 | 60 | #include 61 | 62 | #include 63 | #include 64 | #include 65 | 66 | namespace teb_local_planner 67 | { 68 | 69 | class HomotopyClassPlanner; // Forward declaration 70 | 71 | //! Vertex in the graph that is used to find homotopy classes (only stores 2D positions) 72 | struct HcGraphVertex 73 | { 74 | public: 75 | Eigen::Vector2d pos; // position of vertices in the map 76 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 77 | }; 78 | 79 | //! Abbrev. for the homotopy class search-graph type @see HcGraphVertex 80 | typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph; 81 | //! Abbrev. for vertex type descriptors in the homotopy class search-graph 82 | typedef boost::graph_traits::vertex_descriptor HcGraphVertexType; 83 | //! Abbrev. for edge type descriptors in the homotopy class search-graph 84 | typedef boost::graph_traits::edge_descriptor HcGraphEdgeType; 85 | //! Abbrev. for the vertices iterator of the homotopy class search-graph 86 | typedef boost::graph_traits::vertex_iterator HcGraphVertexIterator; 87 | //! Abbrev. for the edges iterator of the homotopy class search-graph 88 | typedef boost::graph_traits::edge_iterator HcGraphEdgeIterator; 89 | //! Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one 90 | typedef boost::graph_traits::adjacency_iterator HcGraphAdjecencyIterator; 91 | 92 | //!< Inline function used for calculateHSignature() in combination with HCP graph vertex descriptors 93 | inline std::complex getCplxFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph) 94 | { 95 | return std::complex(graph[vert_descriptor].pos.x(), graph[vert_descriptor].pos.y()); 96 | } 97 | 98 | //!< Inline function used for initializing the TEB in combination with HCP graph vertex descriptors 99 | inline const Eigen::Vector2d& getVector2dFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph) 100 | { 101 | return graph[vert_descriptor].pos; 102 | } 103 | 104 | /** 105 | * @brief Base class for graph based path planning / homotopy class sampling 106 | */ 107 | class GraphSearchInterface 108 | { 109 | public: 110 | 111 | virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity) = 0; 112 | 113 | /** 114 | * @brief Clear any existing graph of the homotopy class search 115 | */ 116 | void clearGraph() {graph_.clear();} 117 | 118 | // HcGraph graph() const {return graph_;} 119 | // Workaround. graph_ is public for now, beacuse otherwise the compilation fails with the same boost bug mentioned above. 120 | HcGraph graph_; //!< Store the graph that is utilized to find alternative homotopy classes. 121 | 122 | protected: 123 | /** 124 | * @brief Protected constructor that should be called by subclasses 125 | */ 126 | GraphSearchInterface(const TebConfig& cfg, HomotopyClassPlanner* hcp) : cfg_(&cfg), hcp_(hcp){} 127 | 128 | /** 129 | * @brief Depth First Search implementation to find all paths between the start and the specified goal vertex. 130 | * 深度优先搜索，来找到可能的路径 131 | * Complete paths are stored to the internal path container. 132 | * @sa http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/ 133 | * @param g Graph on which the depth first should be performed 134 | * @param visited A container that stores visited vertices (pass an empty container, it will be filled inside during recursion). 135 | * @param goal Desired goal vertex 136 | * @param start_orientation Orientation of the first trajectory pose, required to initialize the trajectory/TEB 137 | * @param goal_orientation Orientation of the goal trajectory pose, required to initialize the trajectory/TEB 138 | * @param start_velocity start velocity (optional) 139 | */ 140 | void DepthFirst(HcGraph& g, std::vector& visited, const HcGraphVertexType& goal, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity); 141 | 142 | 143 | protected: 144 | const TebConfig* cfg_; //!< Config class that stores and manages all related parameters 145 | HomotopyClassPlanner* const hcp_; //!< Raw pointer to the HomotopyClassPlanner. The HomotopyClassPlanner itself is guaranteed to outlive the graph search class it is holding. 146 | 147 | public: 148 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 149 | }; 150 | 151 | 152 | 153 | class lrKeyPointGraph : public GraphSearchInterface 154 | { 155 | public: 156 | lrKeyPointGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){} 157 | 158 | virtual ~lrKeyPointGraph(){} 159 | 160 | /** 161 | * @brief Create a graph containing points in the global frame that can be used to explore new possible paths between start and goal. 162 | * 163 | * This version of the graph creation places a keypoint on the left and right side of each obstacle w.r.t to the goal heading. \n 164 | * All feasible paths between start and goal point are extracted using a Depth First Search afterwards. \n 165 | * This version works very well for small point obstacles. For more complex obstacles call the createProbRoadmapGraph() 166 | * method that samples keypoints in a predefined area and hopefully finds all relevant alternative paths. 167 | * 168 | * @see createProbRoadmapGraph 169 | * @param start Start pose from wich to start on (e.g. the current robot pose). 170 | * @param goal Goal pose to find paths to (e.g. the robot's goal). 171 | * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization). 172 | * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1] 173 | * @param start_velocity start velocity (optional) 174 | */ 175 | virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity); 176 | }; 177 | 178 | 179 | 180 | 181 | class ProbRoadmapGraph : public GraphSearchInterface 182 | { 183 | public: 184 | ProbRoadmapGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){} 185 | 186 | virtual ~ProbRoadmapGraph(){} 187 | 188 | 189 | /** 190 | * @brief Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal. 191 | * 创建图，并撒点，寻找可能的路径。\n 192 | * This version of the graph samples keypoints in a predefined area (config) in the current frame between start and goal. \n 193 | * Afterwards all feasible paths between start and goal point are extracted using a Depth First Search. \n 194 | * Use the sampling method for complex, non-point or huge obstacles. \n 195 | * You may call createGraph() instead. 196 | * 197 | * @see createGraph 198 | * @param start Start pose from wich to start on (e.g. the current robot pose). 199 | * @param goal Goal pose to find paths to (e.g. the robot's goal). 200 | * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization). 201 | * @param no_samples number of random samples 202 | * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1] 203 | * @param start_velocity start velocity (optional) 204 | */ 205 | virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity); 206 | 207 | private: 208 | boost::random::mt19937 rnd_generator_; //!< Random number generator used by createProbRoadmapGraph to sample graph keypoints. 209 | }; 210 | } // end namespace 211 | 212 | #endif // GRAPH_SEARCH_INTERFACE_H 213 | -------------------------------------------------------------------------------- /include/teb_local_planner/homotopy_class_planner.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2017, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #include "teb_local_planner/homotopy_class_planner.h" 40 | #include "teb_local_planner/h_signature.h" 41 | 42 | namespace teb_local_planner 43 | { 44 | 45 | 46 | template 47 | EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles, 48 | boost::optional timediff_start, boost::optional timediff_end) 49 | { 50 | if(cfg_->obstacles.include_dynamic_obstacles) 51 | { 52 | HSignature3d* H = new HSignature3d(*cfg_); 53 | H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles, timediff_start, timediff_end); 54 | return EquivalenceClassPtr(H); 55 | } 56 | else 57 | { 58 | HSignature* H = new HSignature(*cfg_); 59 | H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles); 60 | return EquivalenceClassPtr(H); 61 | } 62 | } 63 | 64 | 65 | template 66 | TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity) 67 | { 68 | TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(*cfg_, obstacles_, robot_model_)); 69 | 70 | candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, 71 | cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples, 72 | cfg_->trajectory.allow_init_with_backwards_motion); 73 | 74 | if (start_velocity) 75 | candidate->setVelocityStart(*start_velocity); 76 | 77 | EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_, 78 | candidate->teb().timediffs().begin(), candidate->teb().timediffs().end()); 79 | 80 | if(addEquivalenceClassIfNew(H)) 81 | { 82 | tebs_.push_back(candidate); 83 | return tebs_.back(); 84 | } 85 | 86 | // If the candidate constitutes no new equivalence class, return a null pointer 87 | return TebOptimalPlannerPtr(); 88 | } 89 | 90 | } // namespace teb_local_planner 91 | 92 | -------------------------------------------------------------------------------- /include/teb_local_planner/misc.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #ifndef MISC_H 40 | #define MISC_H 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | 47 | namespace teb_local_planner 48 | { 49 | 50 | #define SMALL_NUM 0.00000001 51 | 52 | //! Symbols for left/none/right rotations 53 | enum class RotType { left, none, right }; 54 | 55 | /** 56 | * @brief Check whether two variables (double) are close to each other 57 | * @param a the first value to compare 58 | * @param b the second value to compare 59 | * @param epsilon precision threshold 60 | * @return \c true if |a-b| < epsilon, false otherwise 61 | */ 62 | inline bool is_close(double a, double b, double epsilon = 1e-4) 63 | { 64 | return std::fabs(a - b) < epsilon; 65 | } 66 | 67 | /** 68 | * @brief Return the average angle of an arbitrary number of given angles [rad] 69 | * @param angles vector containing all angles 70 | * @return average / mean angle, that is normalized to [-pi, pi] 71 | */ 72 | inline double average_angles(const std::vector& angles) 73 | { 74 | double x=0, y=0; 75 | for (std::vector::const_iterator it = angles.begin(); it!=angles.end(); ++it) 76 | { 77 | x += cos(*it); 78 | y += sin(*it); 79 | } 80 | if(x == 0 && y == 0) 81 | return 0; 82 | else 83 | return std::atan2(y, x); 84 | } 85 | 86 | /** @brief Small helper function: check if |a|<|b| */ 87 | inline bool smaller_than_abs(double i, double j) {return std::fabs(i) 107 | inline double distance_points2d(const P1& point1, const P2& point2) 108 | { 109 | return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) ); 110 | } 111 | 112 | 113 | /** 114 | * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) 115 | * @param v1 object containing public methods x() and y() 116 | * @param v2 object containing fields x() and y() 117 | * @return magnitude that would result in the 3D case (along the z-axis) 118 | */ 119 | template 120 | inline double cross2d(const V1& v1, const V2& v2) 121 | { 122 | return v1.x()*v2.y() - v2.x()*v1.y(); 123 | } 124 | 125 | /** 126 | * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. 127 | * 128 | * Return a constant reference for boths input variants (pointer or reference). 129 | * @remarks Makes only sense in combination with the overload getConstReference(const T& val). 130 | * @param ptr pointer of type T 131 | * @tparam T arbitrary type 132 | * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion 133 | */ 134 | template 135 | inline const T& get_const_reference(const T* ptr) {return *ptr;} 136 | 137 | /** 138 | * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. 139 | * 140 | * Return a constant reference for boths input variants (pointer or reference). 141 | * @remarks Makes only sense in combination with the overload getConstReference(const T* val). 142 | * @param val 143 | * @param dummy SFINAE helper variable 144 | * @tparam T arbitrary type 145 | * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion 146 | */ 147 | template 148 | inline const T& get_const_reference(const T& val, typename boost::disable_if >::type* dummy = 0) {return val;} 149 | 150 | } // namespace teb_local_planner 151 | 152 | #endif /* MISC_H */ 153 | -------------------------------------------------------------------------------- /include/teb_local_planner/planner_interface.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #ifndef PLANNER_INTERFACE_H_ 40 | #define PLANNER_INTERFACE_H_ 41 | 42 | // boost 43 | #include 44 | 45 | // ros 46 | #include 47 | #include 48 | 49 | // this package 50 | #include 51 | 52 | // messages 53 | #include 54 | #include 55 | #include 56 | 57 | 58 | namespace teb_local_planner 59 | { 60 | 61 | 62 | /** 63 | * @class PlannerInterface 64 | * @brief This abstract class defines an interface for local planners 65 | */ 66 | class PlannerInterface 67 | { 68 | public: 69 | 70 | /** 71 | * @brief Default constructor 72 | */ 73 | PlannerInterface() 74 | { 75 | } 76 | /** 77 | * @brief Virtual destructor. 78 | */ 79 | virtual ~PlannerInterface() 80 | { 81 | } 82 | 83 | 84 | /** @name Plan a trajectory */ 85 | //@{ 86 | 87 | /** 88 | * @brief Plan a trajectory based on an initial reference plan. 89 | * 90 | * Provide this method to create and optimize a trajectory that is initialized 91 | * according to an initial reference plan (given as a container of poses). 92 | * @param initial_plan vector of geometry_msgs::PoseStamped 93 | * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used) 94 | * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, 95 | * otherwise the final velocity will be zero (default: false) 96 | * @return \c true if planning was successful, \c false otherwise 97 | */ 98 | virtual bool plan(const std::vector& initial_plan, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false) = 0; 99 | 100 | /** 101 | * @brief Plan a trajectory between a given start and goal pose (tf::Pose version). 102 | * 103 | * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose. 104 | * @param start tf::Pose containing the start pose of the trajectory 105 | * @param goal tf::Pose containing the goal pose of the trajectory 106 | * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used) 107 | * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, 108 | * otherwise the final velocity will be zero (default: false) 109 | * @return \c true if planning was successful, \c false otherwise 110 | */ 111 | virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false) = 0; 112 | 113 | /** 114 | * @brief Plan a trajectory between a given start and goal pose. 115 | * 116 | * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose. 117 | * @param start PoseSE2 containing the start pose of the trajectory 118 | * @param goal PoseSE2 containing the goal pose of the trajectory 119 | * @param start_vel Initial velocity at the start pose (twist msg containing the translational and angular velocity). 120 | * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, 121 | * otherwise the final velocity will be zero (default: false) 122 | * @return \c true if planning was successful, \c false otherwise 123 | */ 124 | virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false) = 0; 125 | 126 | /** 127 | * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval. 128 | * @warning Call plan() first and check if the generated plan is feasible. 129 | * @param[out] vx translational velocity [m/s] 130 | * @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s] 131 | * @param[out] omega rotational velocity [rad/s] 132 | * @return \c true if command is valid, \c false otherwise 133 | */ 134 | virtual bool getVelocityCommand(double& vx, double& vy, double& omega) const = 0; 135 | 136 | //@} 137 | 138 | 139 | /** 140 | * @brief Reset the planner. 141 | */ 142 | virtual void clearPlanner() = 0; 143 | 144 | /** 145 | * @brief Prefer a desired initial turning direction (by penalizing the opposing one) 146 | * 147 | * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two 148 | * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. 149 | * Initial means that the penalty is applied only to the first few poses of the trajectory. 150 | * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none) 151 | */ 152 | virtual void setPreferredTurningDir(RotType dir) {ROS_WARN("setPreferredTurningDir() not implemented for this planner.");} 153 | 154 | /** 155 | * @brief Visualize planner specific stuff. 156 | * Overwrite this method to provide an interface to perform all planner related visualizations at once. 157 | */ 158 | virtual void visualize() 159 | { 160 | } 161 | 162 | /** 163 | * @brief Check whether the planned trajectory is feasible or not. 164 | * 165 | * This method currently checks only that the trajectory, or a part of the trajectory is collision free. 166 | * Obstacles are here represented as costmap instead of the internal ObstacleContainer. 167 | * @param costmap_model Pointer to the costmap model 168 | * @param footprint_spec The specification of the footprint of the robot in world coordinates 169 | * @param inscribed_radius The radius of the inscribed circle of the robot 170 | * @param circumscribed_radius The radius of the circumscribed circle of the robot 171 | * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked. 172 | * @return \c true, if the robot footprint along the first part of the trajectory intersects with 173 | * any obstacle in the costmap, \c false otherwise. 174 | */ 175 | virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector& footprint_spec, 176 | double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1) = 0; 177 | 178 | 179 | /** 180 | * @brief Implement this method to check if the planner suggests a shorter horizon (e.g. to resolve problems) 181 | * 182 | * This method is intendend to be called after determining that a trajectory provided by the planner is infeasible. 183 | * In some cases a reduction of the horizon length might resolve problems. E.g. if a planned trajectory cut corners. 184 | * Since the trajectory representation is managed by the planner, it is part of the base planner_interface. 185 | * The implementation is optional. If not specified, the method returns \c false. 186 | * @param initial_plan The intial and transformed plan (part of the local map and pruned up to the robot position) 187 | * @return \c true, if the planner suggests a shorter horizon, \c false otherwise. 188 | */ 189 | virtual bool isHorizonReductionAppropriate(const std::vector& initial_plan) const {return false;} 190 | 191 | /** 192 | * Compute and return the cost of the current optimization graph (supports multiple trajectories) 193 | * @param[out] cost current cost value for each trajectory 194 | * [for a planner with just a single trajectory: size=1, vector will not be cleared] 195 | * @param obst_cost_scale Specify extra scaling for obstacle costs 196 | * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time 197 | */ 198 | virtual void computeCurrentCost(std::vector& cost, double obst_cost_scale=1.0, bool alternative_time_cost=false) 199 | { 200 | } 201 | 202 | }; 203 | 204 | //! Abbrev. for shared instances of PlannerInterface or it's subclasses 205 | typedef boost::shared_ptr PlannerInterfacePtr; 206 | 207 | 208 | } // namespace teb_local_planner 209 | 210 | #endif /* PLANNER_INTERFACE_H__ */ 211 | -------------------------------------------------------------------------------- /include/teb_local_planner/recovery_behaviors.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #ifndef RECOVERY_BEHAVIORS_H__ 40 | #define RECOVERY_BEHAVIORS_H__ 41 | 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | namespace teb_local_planner 48 | { 49 | 50 | 51 | /** 52 | * @class FailureDetector 53 | * @brief This class implements methods in order to detect if the robot got stucked or is oscillating 54 | * 55 | * The StuckDetector analyzes the last N commanded velocities in order to detect whether the robot 56 | * might got stucked or oscillates between left/right/forward/backwards motions. 57 | */ 58 | class FailureDetector 59 | { 60 | public: 61 | 62 | /** 63 | * @brief Default constructor 64 | */ 65 | FailureDetector() {} 66 | 67 | /** 68 | * @brief destructor. 69 | */ 70 | ~FailureDetector() {} 71 | 72 | /** 73 | * @brief Set buffer length (measurement history) 74 | * @param length number of measurements to be kept 75 | */ 76 | void setBufferLength(int length) {buffer_.set_capacity(length);} 77 | 78 | /** 79 | * @brief Add a new twist measurement to the internal buffer and compute a new decision 80 | * @param twist geometry_msgs::Twist velocity information 81 | * @param v_max maximum forward translational velocity 82 | * @param v_backwards_max maximum backward translational velocity 83 | * @param omega_max maximum angular velocity 84 | * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1) 85 | * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1) 86 | */ 87 | void update(const geometry_msgs::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps); 88 | 89 | /** 90 | * @brief Check if the robot got stucked 91 | * 92 | * This call does not compute the actual decision, 93 | * since it is computed within each update() invocation. 94 | * @return true if the robot got stucked, false otherwise. 95 | */ 96 | bool isOscillating() const; 97 | 98 | /** 99 | * @brief Clear the current internal state 100 | * 101 | * This call also resets the internal buffer 102 | */ 103 | void clear(); 104 | 105 | protected: 106 | 107 | /** Variables to be monitored */ 108 | struct VelMeasurement 109 | { 110 | double v = 0; 111 | double omega = 0; 112 | }; 113 | 114 | /** 115 | * @brief Detect if the robot got stucked based on the current buffer content 116 | * 117 | * Afterwards the status might be checked using gotStucked(); 118 | * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1) 119 | * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1) 120 | * @return true if the robot got stucked, false otherwise 121 | */ 122 | bool detect(double v_eps, double omega_eps); 123 | 124 | private: 125 | 126 | boost::circular_buffer buffer_; //!< Circular buffer to store the last measurements @see setBufferLength 127 | bool oscillating_ = false; //!< Current state: true if robot is oscillating 128 | 129 | }; 130 | 131 | 132 | } // namespace teb_local_planner 133 | 134 | #endif /* RECOVERY_BEHAVIORS_H__ */ 135 | -------------------------------------------------------------------------------- /include/teb_local_planner/timed_elastic_band.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | namespace teb_local_planner 42 | { 43 | 44 | 45 | 46 | template 47 | bool TimedElasticBand::initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta, 48 | boost::optional max_acc_x, boost::optional max_acc_theta, 49 | boost::optional start_orientation, boost::optional goal_orientation, int min_samples, bool guess_backwards_motion) 50 | { 51 | Eigen::Vector2d start_position = fun_position( *path_start ); 52 | Eigen::Vector2d goal_position = fun_position( *boost::prior(path_end) ); 53 | 54 | bool backwards = false; 55 | 56 | double start_orient, goal_orient; 57 | if (start_orientation) 58 | { 59 | start_orient = *start_orientation; 60 | 61 | // check if the goal is behind the start pose (w.r.t. start orientation) 62 | if (guess_backwards_motion && (goal_position-start_position).dot(Eigen::Vector2d(std::cos(start_orient), std::sin(start_orient))) < 0) 63 | backwards = true; 64 | } 65 | else 66 | { 67 | Eigen::Vector2d start2goal = goal_position - start_position; 68 | start_orient = atan2(start2goal[1],start2goal[0]); 69 | } 70 | 71 | double timestep = 1; // TODO: time 72 | 73 | if (goal_orientation) 74 | { 75 | goal_orient = *goal_orientation; 76 | } 77 | else 78 | { 79 | goal_orient = start_orient; 80 | } 81 | 82 | if (!isInit()) 83 | { 84 | addPose(start_position, start_orient, true); // add starting point and mark it as fixed for optimization 85 | 86 | // we insert middle points now (increase start by 1 and decrease goal by 1) 87 | std::advance(path_start,1); 88 | std::advance(path_end,-1); 89 | int idx=0; 90 | for (; path_start != path_end; ++path_start) // insert middle-points 91 | { 92 | //Eigen::Vector2d point_to_goal = path.back()-*it; 93 | //double dir_to_goal = atan2(point_to_goal[1],point_to_goal[0]); // direction to goal 94 | // Alternative: Direction from last path 95 | Eigen::Vector2d curr_point = fun_position(*path_start); 96 | Eigen::Vector2d diff_last = curr_point - Pose(idx).position(); // we do not use boost::prior(*path_start) for those cases, 97 | // where fun_position() does not return a reference or is expensive. 98 | double diff_norm = diff_last.norm(); 99 | 100 | double timestep_vel = diff_norm/max_vel_x; // constant velocity 101 | double timestep_acc; 102 | 103 | if (max_acc_x) 104 | { 105 | timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration 106 | if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc; 107 | else timestep = timestep_vel; 108 | } 109 | else timestep = timestep_vel; 110 | 111 | if (timestep<0) timestep=0.2; // TODO: this is an assumption 112 | 113 | double yaw = atan2(diff_last[1],diff_last[0]); 114 | if (backwards) 115 | yaw = g2o::normalize_theta(yaw + M_PI); 116 | addPoseAndTimeDiff(curr_point, yaw ,timestep); 117 | 118 | /* 119 | // TODO: the following code does not seem to hot-start the optimizer. Instead it recudes convergence time. 120 | 121 | Eigen::Vector2d diff_next = fun_position(*boost::next(path_start))-curr_point; // TODO maybe store the boost::next for the following iteration 122 | double ang_diff = std::abs( g2o::normalize_theta( atan2(diff_next[1],diff_next[0]) 123 | -atan2(diff_last[1],diff_last[0]) ) ); 124 | 125 | timestep_vel = ang_diff/max_vel_theta; // constant velocity 126 | if (max_acc_theta) 127 | { 128 | timestep_acc = sqrt(2*ang_diff/(*max_acc_theta)); // constant acceleration 129 | if (timestep_vel < timestep_acc) timestep = timestep_acc; 130 | else timestep = timestep_vel; 131 | } 132 | else timestep = timestep_vel; 133 | 134 | if (timestep<0) timestep=0.2; // TODO: this is an assumption 135 | 136 | yaw = atan2(diff_last[1],diff_last[0]); // TODO redundant right now, not yet finished 137 | if (backwards) 138 | yaw = g2o::normalize_theta(yaw + M_PI); 139 | addPoseAndTimeDiff(curr_point, yaw ,timestep); 140 | 141 | */ 142 | 143 | ++idx; 144 | } 145 | Eigen::Vector2d diff = goal_position-Pose(idx).position(); 146 | double diff_norm = diff.norm(); 147 | double timestep_vel = diff_norm/max_vel_x; // constant velocity 148 | if (max_acc_x) 149 | { 150 | double timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration 151 | if (timestep_vel < timestep_acc) timestep = timestep_acc; 152 | else timestep = timestep_vel; 153 | } 154 | else timestep = timestep_vel; 155 | 156 | 157 | PoseSE2 goal(goal_position, goal_orient); 158 | 159 | // if number of samples is not larger than min_samples, insert manually 160 | if ( sizePoses() < min_samples-1 ) 161 | { 162 | ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples..."); 163 | while (sizePoses() < min_samples-1) // subtract goal point that will be added later 164 | { 165 | // Each inserted point bisects the remaining distance. Thus the timestep is also bisected. 166 | timestep /= 2; 167 | // simple strategy: interpolate between the current pose and the goal 168 | addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep ); // let the optimier correct the timestep (TODO: better initialization 169 | } 170 | } 171 | 172 | // now add goal 173 | addPoseAndTimeDiff(goal, timestep); // add goal point 174 | setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization 175 | } 176 | else // size!=0 177 | { 178 | ROS_WARN("Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!"); 179 | ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs()); 180 | return false; 181 | } 182 | return true; 183 | } 184 | 185 | 186 | } // namespace teb_local_planner 187 | 188 | 189 | 190 | -------------------------------------------------------------------------------- /include/teb_local_planner/visualization.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #ifndef VISUALIZATION_H_ 40 | #define VISUALIZATION_H_ 41 | 42 | 43 | 44 | // teb stuff 45 | #include 46 | #include 47 | #include 48 | 49 | // ros stuff 50 | #include 51 | #include 52 | 53 | // boost 54 | #include 55 | #include 56 | 57 | // std 58 | #include 59 | 60 | // messages 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | 69 | namespace teb_local_planner 70 | { 71 | 72 | class TebOptimalPlanner; //!< Forward Declaration 73 | 74 | 75 | /** 76 | * @class TebVisualization 77 | * @brief Visualize stuff from the teb_local_planner 78 | */ 79 | class TebVisualization 80 | { 81 | public: 82 | 83 | /** 84 | * @brief Default constructor 85 | * @remarks do not forget to call initialize() 86 | */ 87 | TebVisualization(); 88 | 89 | /** 90 | * @brief Constructor that initializes the class and registers topics 91 | * @param nh local ros::NodeHandle 92 | * @param cfg const reference to the TebConfig class for parameters 93 | */ 94 | TebVisualization(ros::NodeHandle& nh, const TebConfig& cfg); 95 | 96 | /** 97 | * @brief Initializes the class and registers topics. 98 | * 99 | * Call this function if only the default constructor has been called before. 100 | * @param nh local ros::NodeHandle 101 | * @param cfg const reference to the TebConfig class for parameters 102 | */ 103 | void initialize(ros::NodeHandle& nh, const TebConfig& cfg); 104 | 105 | 106 | /** @name Publish to topics */ 107 | //@{ 108 | 109 | /** 110 | * @brief Publish a given global plan to the ros topic \e ../../global_plan 111 | * @param global_plan Pose array describing the global plan 112 | */ 113 | void publishGlobalPlan(const std::vector& global_plan) const; 114 | 115 | /** 116 | * @brief Publish a given local plan to the ros topic \e ../../local_plan 117 | * @param local_plan Pose array describing the local plan 118 | */ 119 | void publishLocalPlan(const std::vector& local_plan) const; 120 | 121 | /** 122 | * @brief Publish Timed_Elastic_Band related stuff (local plan, pose sequence). 123 | * 124 | * Given a Timed_Elastic_Band instance, publish the local plan to \e ../../local_plan 125 | * and the pose sequence to \e ../../teb_poses. 126 | * @param teb const reference to a Timed_Elastic_Band 127 | */ 128 | void publishLocalPlanAndPoses(const TimedElasticBand& teb) const; 129 | 130 | /** 131 | * @brief Publish the visualization of the robot model 132 | * 133 | * @param current_pose Current pose of the robot 134 | * @param robot_model Subclass of BaseRobotFootprintModel 135 | * @param ns Namespace for the marker objects 136 | */ 137 | void publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns = "RobotFootprintModel"); 138 | 139 | /** 140 | * @brief Publish obstacle positions to the ros topic \e ../../teb_markers 141 | * @todo Move filling of the marker message to polygon class in order to avoid checking types. 142 | * @param obstacles Obstacle container 143 | */ 144 | void publishObstacles(const ObstContainer& obstacles) const; 145 | 146 | /** 147 | * @brief Publish via-points to the ros topic \e ../../teb_markers 148 | * @param via_points via-point container 149 | */ 150 | void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator >& via_points, const std::string& ns = "ViaPoints") const; 151 | 152 | /** 153 | * @brief Publish a boost::adjacency_list (boost's graph datatype) via markers. 154 | * @remarks Make sure that vertices of the graph contain a member \c pos as \c Eigen::Vector2d type 155 | * to query metric position values. 156 | * @param graph Const reference to the boost::adjacency_list (graph) 157 | * @param ns_prefix Namespace prefix for the marker objects (the strings "Edges" and "Vertices" will be appended) 158 | * @tparam GraphType boost::graph object in which vertices has the field/member \c pos. 159 | */ 160 | template 161 | void publishGraph(const GraphType& graph, const std::string& ns_prefix = "Graph"); 162 | 163 | /** 164 | * @brief Publish multiple 2D paths (each path given as a point sequence) from a container class. 165 | * 166 | * Provide a std::vector< std::vector< T > > in which T.x() and T.y() exist 167 | * and std::vector could be individually substituded by std::list / std::deque /... 168 | * 169 | * A common point-type for object T could be Eigen::Vector2d. 170 | * 171 | * T could be also a raw pointer std::vector< std::vector< T* > >. 172 | * 173 | * @code 174 | * typedef std::vector > PathType; // could be a list or deque as well ... 175 | * std::vector path_container(2); // init 2 empty paths; the container could be a list or deque as well ... 176 | * // Fill path_container.at(0) with Eigen::Vector2d elements, we skip that here 177 | * // Fill path_container.at(1) with Eigen::Vector2d elements, we skip that here 178 | * publishPathContainer( path_container.begin(), path_container.end() ); 179 | * @endcode 180 | * 181 | * @remarks Actually the underlying path does not necessarily need to be a Eigen::Vector2d sequence. 182 | * Eigen::Vector2d can be replaced with any datatype that implement public x() and y() methods.\n 183 | * @param first Bidirectional iterator pointing to the begin of the path 184 | * @param last Bidirectional iterator pointing to the end of the path 185 | * @param ns Namespace for the marker objects (the strings "Edges" and "Vertices" will be appended) 186 | * @tparam BidirIter Bidirectional iterator to a 2D path (sequence of Eigen::Vector2d elements) in a container 187 | */ 188 | template 189 | void publishPathContainer(BidirIter first, BidirIter last, const std::string& ns = "PathContainer"); 190 | 191 | /** 192 | * @brief Publish multiple Tebs from a container class (publish as marker message). 193 | * 194 | * @param teb_planner Container of boost::shared_ptr< TebOptPlannerPtr > 195 | * @param ns Namespace for the marker objects 196 | */ 197 | void publishTebContainer(const std::vector< boost::shared_ptr >& teb_planner, const std::string& ns = "TebContainer"); 198 | 199 | /** 200 | * @brief Publish a feedback message (multiple trajectory version) 201 | * 202 | * The feedback message contains the all planned trajectory candidates (e.g. if planning in distinctive topologies is turned on). 203 | * Each trajectory is composed of the sequence of poses, the velocity profile and temporal information. 204 | * The feedback message also contains a list of active obstacles. 205 | * @param teb_planners container with multiple tebs (resp. their planner instances) 206 | * @param selected_trajectory_idx Idx of the currently selected trajectory in \c teb_planners 207 | * @param obstacles Container of obstacles 208 | */ 209 | void publishFeedbackMessage(const std::vector< boost::shared_ptr >& teb_planners, unsigned int selected_trajectory_idx, const ObstContainer& obstacles); 210 | 211 | /** 212 | * @brief Publish a feedback message (single trajectory overload) 213 | * 214 | * The feedback message contains the planned trajectory 215 | * that is composed of the sequence of poses, the velocity profile and temporal information. 216 | * The feedback message also contains a list of active obstacles. 217 | * @param teb_planner the planning instance 218 | * @param obstacles Container of obstacles 219 | */ 220 | void publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles); 221 | 222 | //@} 223 | 224 | protected: 225 | 226 | /** 227 | * @brief Small helper function that checks if initialize() has been called and prints an error message if not. 228 | * @return \c true if not initialized, \c false if everything is ok 229 | */ 230 | bool printErrorWhenNotInitialized() const; 231 | 232 | ros::Publisher global_plan_pub_; //!< Publisher for the global plan 233 | ros::Publisher local_plan_pub_; //!< Publisher for the local plan 234 | ros::Publisher teb_poses_pub_; //!< Publisher for the trajectory pose sequence 235 | ros::Publisher teb_marker_pub_; //!< Publisher for visualization markers 236 | ros::Publisher feedback_pub_; //!< Publisher for the feedback message for analysis and debug purposes 237 | 238 | const TebConfig* cfg_; //!< Config class that stores and manages all related parameters 239 | 240 | bool initialized_; //!< Keeps track about the correct initialization of this class 241 | 242 | 243 | public: 244 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 245 | }; 246 | 247 | //! Abbrev. for shared instances of the TebVisualization 248 | typedef boost::shared_ptr TebVisualizationPtr; 249 | 250 | //! Abbrev. for shared instances of the TebVisualization (read-only) 251 | typedef boost::shared_ptr TebVisualizationConstPtr; 252 | 253 | 254 | } // namespace teb_local_planner 255 | 256 | 257 | // Include template method implementations / definitions 258 | #include 259 | 260 | #endif /* VISUALIZATION_H_ */ 261 | -------------------------------------------------------------------------------- /include/teb_local_planner/visualization.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | 42 | 43 | namespace teb_local_planner 44 | { 45 | 46 | 47 | template 48 | void TebVisualization::publishGraph(const GraphType& graph, const std::string& ns_prefix) 49 | { 50 | if ( printErrorWhenNotInitialized() ) 51 | return; 52 | 53 | typedef typename boost::graph_traits::vertex_iterator GraphVertexIterator; 54 | typedef typename boost::graph_traits::edge_iterator GraphEdgeIterator; 55 | 56 | // Visualize Edges 57 | visualization_msgs::Marker marker; 58 | marker.header.frame_id = cfg_->map_frame; 59 | marker.header.stamp = ros::Time::now(); 60 | marker.ns = ns_prefix + "Edges"; 61 | marker.id = 0; 62 | // #define TRIANGLE 63 | #ifdef TRIANGLE 64 | marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 65 | #else 66 | marker.type = visualization_msgs::Marker::LINE_LIST; 67 | #endif 68 | marker.action = visualization_msgs::Marker::ADD; 69 | 70 | GraphEdgeIterator it_edge, end_edges; 71 | for (boost::tie(it_edge,end_edges) = boost::edges(graph); it_edge!=end_edges; ++it_edge) 72 | { 73 | #ifdef TRIANGLE 74 | geometry_msgs::Point point_start1; 75 | point_start1.x = graph[boost::source(*it_edge,graph)].pos[0]+0.05; 76 | point_start1.y = graph[boost::source(*it_edge,graph)].pos[1]-0.05; 77 | point_start1.z = 0; 78 | marker.points.push_back(point_start1); 79 | geometry_msgs::Point point_start2; 80 | point_start2.x = graph[boost::source(*it_edge,graph)].pos[0]-0.05; 81 | point_start2.y = graph[boost::source(*it_edge,graph)].pos[1]+0.05; 82 | point_start2.z = 0; 83 | marker.points.push_back(point_start2); 84 | 85 | #else 86 | geometry_msgs::Point point_start; 87 | point_start.x = graph[boost::source(*it_edge,graph)].pos[0]; 88 | point_start.y = graph[boost::source(*it_edge,graph)].pos[1]; 89 | point_start.z = 0; 90 | marker.points.push_back(point_start); 91 | #endif 92 | geometry_msgs::Point point_end; 93 | point_end.x = graph[boost::target(*it_edge,graph)].pos[0]; 94 | point_end.y = graph[boost::target(*it_edge,graph)].pos[1]; 95 | point_end.z = 0; 96 | marker.points.push_back(point_end); 97 | 98 | // add color 99 | std_msgs::ColorRGBA color; 100 | color.a = 1.0; 101 | color.r = 0; 102 | color.g = 0; 103 | color.b = 1; 104 | marker.colors.push_back(color); 105 | marker.colors.push_back(color); 106 | #ifdef TRIANGLE 107 | marker.colors.push_back(color); 108 | #endif 109 | } 110 | 111 | #ifdef TRIANGLE 112 | marker.scale.x = 1; 113 | marker.scale.y = 1; 114 | marker.scale.z = 1; 115 | #else 116 | marker.scale.x = 0.01; 117 | #endif 118 | marker.color.a = 1.0; 119 | marker.color.r = 0.0; 120 | marker.color.g = 1.0; 121 | marker.color.b = 0.0; 122 | 123 | // Now publish edge markers 124 | teb_marker_pub_.publish( marker ); 125 | 126 | // Visualize vertices 127 | marker.header.frame_id = cfg_->map_frame; 128 | marker.header.stamp = ros::Time::now(); 129 | marker.ns = ns_prefix + "Vertices"; 130 | marker.id = 0; 131 | marker.type = visualization_msgs::Marker::POINTS; 132 | marker.action = visualization_msgs::Marker::ADD; 133 | 134 | GraphVertexIterator it_vert, end_vert; 135 | for (boost::tie(it_vert,end_vert) = boost::vertices(graph); it_vert!=end_vert; ++it_vert) 136 | { 137 | geometry_msgs::Point point; 138 | point.x = graph[*it_vert].pos[0]; 139 | point.y = graph[*it_vert].pos[1]; 140 | point.z = 0; 141 | marker.points.push_back(point); 142 | // add color 143 | 144 | std_msgs::ColorRGBA color; 145 | color.a = 1.0; 146 | if (it_vert==end_vert-1) 147 | { 148 | color.r = 1; 149 | color.g = 0; 150 | color.b = 0; 151 | } 152 | else 153 | { 154 | color.r = 0; 155 | color.g = 1; 156 | color.b = 0; 157 | } 158 | marker.colors.push_back(color); 159 | } 160 | // set first color (start vertix) to blue 161 | if (!marker.colors.empty()) 162 | { 163 | marker.colors.front().b = 1; 164 | marker.colors.front().g = 0; 165 | } 166 | 167 | marker.scale.x = 0.1; 168 | marker.scale.y = 0.1; 169 | marker.color.a = 1.0; 170 | marker.color.r = 0.0; 171 | marker.color.g = 1.0; 172 | marker.color.b = 0.0; 173 | 174 | // Now publish vertex markers 175 | teb_marker_pub_.publish( marker ); 176 | } 177 | 178 | template 179 | void TebVisualization::publishPathContainer(BidirIter first, BidirIter last, const std::string& ns) 180 | { 181 | if ( printErrorWhenNotInitialized() ) 182 | return; 183 | 184 | visualization_msgs::Marker marker; 185 | marker.header.frame_id = cfg_->map_frame; 186 | marker.header.stamp = ros::Time::now(); 187 | marker.ns = ns; 188 | marker.id = 0; 189 | marker.type = visualization_msgs::Marker::LINE_LIST; 190 | marker.action = visualization_msgs::Marker::ADD; 191 | 192 | typedef typename std::iterator_traits::value_type PathType; // Get type of the path (point container) 193 | 194 | // Iterate through path container 195 | while(first != last) 196 | { 197 | // iterate single path points 198 | typename PathType::const_iterator it_point, end_point; 199 | for (it_point = first->begin(), end_point = boost::prior(first->end()); it_point != end_point; ++it_point) 200 | { 201 | geometry_msgs::Point point_start; 202 | point_start.x = get_const_reference(*it_point).x(); 203 | point_start.y = get_const_reference(*it_point).y(); 204 | point_start.z = 0; 205 | marker.points.push_back(point_start); 206 | 207 | geometry_msgs::Point point_end; 208 | point_end.x = get_const_reference(*boost::next(it_point)).x(); 209 | point_end.y = get_const_reference(*boost::next(it_point)).y(); 210 | point_end.z = 0; 211 | marker.points.push_back(point_end); 212 | } 213 | ++first; 214 | } 215 | marker.scale.x = 0.01; 216 | marker.color.a = 1.0; 217 | marker.color.r = 0.0; 218 | marker.color.g = 1.0; 219 | marker.color.b = 0.0; 220 | 221 | teb_marker_pub_.publish( marker ); 222 | } 223 | 224 | 225 | } // namespace teb_local_planner -------------------------------------------------------------------------------- /launch/test_optim_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /msg/FeedbackMsg.msg: -------------------------------------------------------------------------------- 1 | # Message that contains intermediate results 2 | # and diagnostics of the (predictive) planner. 3 | 4 | std_msgs/Header header 5 | 6 | # The planned trajectory (or if multiple plans exist, all of them) 7 | teb_local_planner/TrajectoryMsg[] trajectories 8 | 9 | # Index of the trajectory in 'trajectories' that is selected currently 10 | uint16 selected_trajectory_idx 11 | 12 | # List of active obstacles 13 | costmap_converter/ObstacleArrayMsg obstacles_msg 14 | 15 | 16 | -------------------------------------------------------------------------------- /msg/TrajectoryMsg.msg: -------------------------------------------------------------------------------- 1 | # Message that contains a trajectory for mobile robot navigation 2 | 3 | std_msgs/Header header 4 | teb_local_planner/TrajectoryPointMsg[] trajectory 5 | 6 | 7 | -------------------------------------------------------------------------------- /msg/TrajectoryPointMsg.msg: -------------------------------------------------------------------------------- 1 | # Message that contains single point on a trajectory suited for mobile navigation. 2 | # The trajectory is described by a sequence of poses, velocities, 3 | # accelerations and temporal information. 4 | 5 | # Why this message type? 6 | # nav_msgs/Path describes only a path without temporal information. 7 | # trajectory_msgs package contains only messages for joint trajectories. 8 | 9 | # The pose of the robot 10 | geometry_msgs/Pose pose 11 | 12 | # Corresponding velocity 13 | geometry_msgs/Twist velocity 14 | 15 | # Corresponding acceleration 16 | geometry_msgs/Twist acceleration 17 | 18 | duration time_from_start 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | teb_local_planner 4 | 0.6.11 5 | 6 | The teb_local_planner package implements a plugin 7 | to the base_local_planner of the 2D navigation stack. 8 | The underlying method called Timed Elastic Band locally optimizes 9 | the robot's trajectory with respect to trajectory execution time, 10 | separation from obstacles and compliance with kinodynamic constraints at runtime. 11 | 12 | 13 | 14 | Christoph Rösmann 15 | 16 | BSD 17 | 18 | http://wiki.ros.org/teb_local_planner 19 | 20 | Christoph Rösmann 21 | 22 | catkin 23 | 24 | cmake_modules 25 | message_generation 26 | 27 | message_runtime 28 | message_runtime 29 | 30 | base_local_planner 31 | costmap_2d 32 | costmap_converter 33 | 34 | dynamic_reconfigure 35 | geometry_msgs 36 | interactive_markers 37 | libg2o 38 | nav_core 39 | nav_msgs 40 | pluginlib 41 | roscpp 42 | std_msgs 43 | tf 44 | tf_conversions 45 | visualization_msgs 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /scripts/cmd_vel_to_ackermann_drive.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Author: christoph.roesmann@tu-dortmund.de 4 | 5 | import rospy, math 6 | from geometry_msgs.msg import Twist 7 | from ackermann_msgs.msg import AckermannDriveStamped 8 | 9 | 10 | def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase): 11 | if omega == 0 or v == 0: 12 | return 0 13 | 14 | radius = v / omega 15 | return math.atan(wheelbase / radius) 16 | 17 | 18 | def cmd_callback(data): 19 | global wheelbase 20 | global ackermann_cmd_topic 21 | global frame_id 22 | global pub 23 | global cmd_angle_instead_rotvel 24 | 25 | v = data.linear.x 26 | # if cmd_angle_instead_rotvel is true, the rotational velocity is already converted in the C++ node 27 | # in this case this script only needs to do the msg conversion from twist to Ackermann drive 28 | if cmd_angle_instead_rotvel: 29 | steering = data.angular.z 30 | else: 31 | steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase) 32 | 33 | msg = AckermannDriveStamped() 34 | msg.header.stamp = rospy.Time.now() 35 | msg.header.frame_id = frame_id 36 | msg.drive.steering_angle = steering 37 | msg.drive.speed = v 38 | 39 | pub.publish(msg) 40 | 41 | 42 | 43 | 44 | 45 | if __name__ == '__main__': 46 | try: 47 | 48 | rospy.init_node('cmd_vel_to_ackermann_drive') 49 | 50 | twist_cmd_topic = rospy.get_param('~twist_cmd_topic', '/cmd_vel') 51 | ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic', '/ackermann_cmd') 52 | wheelbase = rospy.get_param('~wheelbase', 1.0) 53 | frame_id = rospy.get_param('~frame_id', 'odom') 54 | cmd_angle_instead_rotvel = rospy.get_param('/move_base/TebLocalPlannerROS/cmd_angle_instead_rotvel', False) 55 | 56 | rospy.Subscriber(twist_cmd_topic, Twist, cmd_callback, queue_size=1) 57 | pub = rospy.Publisher(ackermann_cmd_topic, AckermannDriveStamped, queue_size=1) 58 | 59 | rospy.loginfo("Node 'cmd_vel_to_ackermann_drive' started.\nListening to %s, publishing to %s. Frame id: %s, wheelbase: %f", "/cmd_vel", ackermann_cmd_topic, frame_id, wheelbase) 60 | 61 | rospy.spin() 62 | 63 | except rospy.ROSInterruptException: 64 | pass 65 | 66 | -------------------------------------------------------------------------------- /scripts/export_to_mat.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # This small script subscribes to the FeedbackMsg message of teb_local_planner 4 | # and exports data to a mat file. 5 | # publish_feedback must be turned on such that the planner publishes this information. 6 | # Author: christoph.roesmann@tu-dortmund.de 7 | 8 | import rospy, math 9 | from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg 10 | from geometry_msgs.msg import PolygonStamped, Point32, Quaternion 11 | from tf.transformations import euler_from_quaternion 12 | import numpy as np 13 | import scipy.io as sio 14 | import time 15 | 16 | def feedback_callback(data): 17 | global got_data 18 | 19 | if not data.trajectories: # empty 20 | trajectory = [] 21 | return 22 | 23 | if got_data: 24 | return 25 | 26 | got_data = True 27 | 28 | # copy trajectory 29 | trajectories = [] 30 | for traj in data.trajectories: 31 | trajectory = [] 32 | # # store as struct and cell array 33 | # for point in traj.trajectory: 34 | # (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w]) 35 | # pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw} 36 | # velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z} 37 | # time_from_start = point.time_from_start.to_sec() 38 | # trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start}) 39 | 40 | # store as all-in-one mat 41 | arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t 42 | for index, point in enumerate(traj.trajectory): 43 | arr[0,index] = point.pose.position.x 44 | arr[1,index] = point.pose.position.y 45 | (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w]) 46 | arr[2,index] = yaw 47 | arr[3,index] = point.velocity.linear.x 48 | arr[4,index] = point.velocity.angular.z 49 | arr[5,index] = point.time_from_start.to_sec() 50 | 51 | # trajectories.append({'raw': trajectory, 'mat': arr}) 52 | trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']}) 53 | 54 | # copy obstacles 55 | obstacles = [] 56 | for obst_id, obst in enumerate(data.obstacle_msg.obstacles): 57 | #polygon = [] 58 | #for point in obst.polygon.points: 59 | # polygon.append({'x': point.x, 'y': point.y, 'z': point.z}) 60 | obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y 61 | for index, point in enumerate(obst.polygon.points): 62 | obst_arr[0, index] = point.x 63 | obst_arr[1, index] = point.y 64 | obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x 65 | obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y 66 | 67 | #obstacles.append(polygon) 68 | obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']}) 69 | 70 | 71 | # create main struct: 72 | mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles} 73 | 74 | timestr = time.strftime("%Y%m%d_%H%M%S") 75 | filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat' 76 | 77 | rospy.loginfo("Saving mat-file '%s'.", filename) 78 | sio.savemat(filename, mat) 79 | 80 | 81 | 82 | 83 | 84 | def feedback_exporter(): 85 | global got_data 86 | 87 | rospy.init_node("export_to_mat", anonymous=True) 88 | 89 | 90 | topic_name = "/test_optim_node/teb_feedback" # define feedback topic here! 91 | 92 | rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) 93 | 94 | rospy.loginfo("Waiting for feedback message on topic %s.", topic_name) 95 | 96 | r = rospy.Rate(2) # define rate here 97 | while not rospy.is_shutdown(): 98 | 99 | if got_data: 100 | rospy.loginfo("Data export completed.") 101 | return 102 | 103 | r.sleep() 104 | 105 | if __name__ == '__main__': 106 | try: 107 | global got_data 108 | got_data = False 109 | feedback_exporter() 110 | except rospy.ROSInterruptException: 111 | pass 112 | 113 | -------------------------------------------------------------------------------- /scripts/export_to_svg.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | ======================================================================================== 5 | # This small script subscribes to the FeedbackMsg message of teb_local_planner 6 | # and converts the current scene to a svg-image 7 | # publish_feedback must be turned on such that the planner publishes this information. 8 | # Author: christoph.roesmann@tu-dortmund.de 9 | 10 | It is recommendable to start this node after initialization of TEB is completed. 11 | 12 | Requirements: 13 | svgwrite: A Python library to create SVG drawings. http://pypi.python.org/pypi/svgwrite 14 | ======================================================================================= 15 | """ 16 | import roslib; 17 | import rospy 18 | import svgwrite 19 | import math 20 | import time 21 | import random 22 | from svgwrite import cm, mm 23 | from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg 24 | from geometry_msgs.msg import PolygonStamped, Point32, Quaternion 25 | 26 | 27 | # ================= PARAMETERS ================== 28 | # TODO: In case of a more general node, change parameter to ros-parameter 29 | # Drawing parameters: 30 | SCALE = 200 # Overall scaling: 100 pixel = 1 m 31 | MIN_POSE_DISTANCE = 0.3 # Distance between two consecutive poses in SVG-image 32 | SCALE_VELOCITY_VEC = 0.4 # Scaling of velocity vectors -> 1 cell = 1/SCALE_VELOCITY_VEC m/s 33 | GRID_X_MIN = -2 # Define, how many cells your grid should contain in each direction. 34 | GRID_X_MAX = 2 35 | GRID_Y_MIN = -2 36 | GRID_Y_MAX = 1 37 | 38 | # TEB parameters: 39 | OBSTACLE_DIST = 50 *SCALE/100 # cm 40 | 41 | 42 | # ================= FUNCTIONS =================== 43 | 44 | def sign(number): 45 | """ 46 | Signum function: get sign of a number 47 | 48 | @param number: get sign of this number 49 | @type number: numeric type (eg. integer) 50 | @return: sign of number 51 | @rtype: integer {1, -1, 0} 52 | """ 53 | return cmp(number,0) 54 | 55 | def arrowMarker(color='green', orientation='auto'): 56 | """ 57 | Create an arrow marker with svgwrite 58 | 59 | @return: arrow marker 60 | @rtype: svg_write marker object 61 | """ 62 | arrow = svg.marker(insert=(1,5), size=(4,3), orient=orientation) 63 | arrow.viewbox(width=10, height=10) 64 | arrow.add(svg.polyline([(0,0),(10,5),(0,10),(1,5)], fill=color, opacity=1.0)) 65 | svg.defs.add(arrow) 66 | return arrow 67 | 68 | def quaternion2YawDegree(orientation): 69 | """ 70 | Get yaw angle [degree] from quaternion representation 71 | 72 | @param orientation: orientation in quaternions to read from 73 | @type orientation: geometry_msgs/Quaternion 74 | @return: yaw angle [degree] 75 | @rtype: float 76 | """ 77 | yawRad = math.atan2(2*(orientation.x*orientation.y+orientation.z*orientation.w),1-2*(pow(orientation.y,2)+pow(orientation.z,2))) 78 | return yawRad*180/math.pi 79 | 80 | 81 | def feedback_callback(data): 82 | """ 83 | Callback for receiving TEB and obstacle information 84 | 85 | @param data: Received feedback message 86 | @type data: visualization_msgs/Marker 87 | 88 | @globalparam tebList: Received TEB List 89 | @globaltype tebList: teb_local_planner/FeedbackMsg 90 | """ 91 | # TODO: Remove global variables 92 | global feedbackMsg 93 | 94 | if not feedbackMsg: 95 | feedbackMsg = data 96 | rospy.loginfo("TEB feedback message received...") 97 | 98 | 99 | # ================ MAIN FUNCTION ================ 100 | 101 | if __name__ == '__main__': 102 | rospy.init_node('export_to_svg', anonymous=True) 103 | 104 | topic_name = "/test_optim_node/teb_feedback" # define feedback topic here! 105 | 106 | rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) 107 | 108 | rospy.loginfo("Waiting for feedback message on topic %s.", topic_name) 109 | 110 | rate = rospy.Rate(10.0) 111 | feedbackMsg = [] 112 | 113 | timestr = time.strftime("%Y%m%d_%H%M%S") 114 | filename_string = "teb_svg_" + timestr + '.svg' 115 | 116 | rospy.loginfo("SVG will be written to '%s'.", filename_string) 117 | 118 | random.seed(0) 119 | 120 | svg=svgwrite.Drawing(filename=filename_string, debug=True) 121 | 122 | # Create viewbox -> this box defines the size of the visible drawing 123 | svg.viewbox(GRID_X_MIN*SCALE-1*SCALE,GRID_Y_MIN*SCALE-1*SCALE,GRID_X_MAX*SCALE-GRID_X_MIN*SCALE+2*SCALE,GRID_Y_MAX*SCALE-GRID_Y_MIN*SCALE+2*SCALE) 124 | 125 | # Draw grid: 126 | hLines = svg.add(svg.g(id='hLines', stroke='black')) 127 | hLines.add(svg.line(start=(GRID_X_MIN*SCALE, 0), end=(GRID_X_MAX*SCALE, 0))) 128 | for y in range(GRID_Y_MAX): 129 | hLines.add(svg.line(start=(GRID_X_MIN*SCALE, SCALE+y*SCALE), end=(GRID_X_MAX*SCALE, SCALE+y*SCALE))) 130 | for y in range(-GRID_Y_MIN): 131 | hLines.add(svg.line(start=(GRID_X_MIN*SCALE, -SCALE-y*SCALE), end=(GRID_X_MAX*SCALE, -SCALE-y*SCALE))) 132 | vLines = svg.add(svg.g(id='vline', stroke='black')) 133 | vLines.add(svg.line(start=(0, GRID_Y_MIN*SCALE), end=(0, GRID_Y_MAX*SCALE))) 134 | for x in range(GRID_X_MAX): 135 | vLines.add(svg.line(start=(SCALE+x*SCALE, GRID_Y_MIN*SCALE), end=(SCALE+x*SCALE, GRID_Y_MAX*SCALE))) 136 | for x in range(-GRID_X_MIN): 137 | vLines.add(svg.line(start=(-SCALE-x*SCALE, GRID_Y_MIN*SCALE), end=(-SCALE-x*SCALE, GRID_Y_MAX*SCALE))) 138 | 139 | 140 | # Draw legend: 141 | legend = svg.g(id='legend', font_size=25) 142 | stringGeometry = "Geometry: 1 Unit = 1.0m" 143 | legendGeometry = svg.text(stringGeometry) 144 | legend.add(legendGeometry) 145 | legend.translate(tx=GRID_X_MIN*SCALE, ty=GRID_Y_MAX*SCALE + 30) # Move legend to buttom left corner 146 | svg.add(legend) 147 | 148 | 149 | #arrow = arrowMarker() # Init arrow marker 150 | 151 | rospy.loginfo("Initialization completed.\nWaiting for feedback message...") 152 | 153 | # -------------------- WAIT FOR CALLBACKS -------------------------- 154 | while not rospy.is_shutdown(): 155 | if feedbackMsg: 156 | break # Leave loop after receiving all necessary TEB information (see callbacks) to finish drawing 157 | rate.sleep() 158 | # ------------------------------------------------------------------ 159 | 160 | if not feedbackMsg.trajectories: 161 | rospy.loginfo("Received message does not contain trajectories. Shutting down...") 162 | sys.exit() 163 | 164 | if len(feedbackMsg.trajectories[0].trajectory) < 2: 165 | rospy.loginfo("Received message does not contain trajectories with at least two states (start and goal). Shutting down...") 166 | sys.exit() 167 | 168 | # iterate trajectories 169 | for index, traj in enumerate(feedbackMsg.trajectories): 170 | 171 | #color 172 | traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB') 173 | 174 | # Iterate through TEB positions -> Draw Paths 175 | points = [] 176 | for point in traj.trajectory: 177 | points.append( (point.pose.position.x*SCALE,-point.pose.position.y*SCALE) ) # y is negative in image coordinates 178 | # svgwrite rotates clockwise! 179 | 180 | 181 | if index == feedbackMsg.selected_trajectory_idx: # highlight currently selected teb 182 | line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \ 183 | stroke_linejoin='round', opacity=1.0 ) ) 184 | else: 185 | line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='butt', \ 186 | stroke_linejoin='round', stroke_dasharray='10,3', opacity=1.0 ) ) 187 | #marker_points = points[::7] 188 | #markerline = svg.add( svg.polyline(points=marker_points, fill='none', stroke=traj_color, stroke_width=10, opacity=0.0 ) ) 189 | #arrow = arrowMarker(traj_color) 190 | #markerline.set_markers( (arrow, arrow, arrow) ) 191 | #line.set_markers( (arrow, arrow, arrow) ) 192 | #line['marker-start'] = arrow.get_funciri() 193 | 194 | 195 | # Add Start and Goal Point 196 | start_pose = feedbackMsg.trajectories[0].trajectory[0].pose 197 | goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose 198 | start_position = start_pose.position 199 | goal_position = goal_pose.position 200 | svg.add(svg.circle(center=(start_position.x*SCALE,-start_position.y*SCALE), r=10, stroke_width=1, stroke='blue', fill ='blue')) 201 | svg.add(svg.text("Start", (start_position.x*SCALE-70, -start_position.y*SCALE+45), font_size=35)) # Add label 202 | svg.add(svg.circle(center=(goal_position.x*SCALE,-goal_position.y*SCALE), r=10, stroke_width=1, stroke='red', fill ='red')) 203 | svg.add(svg.text("Goal", (goal_position.x*SCALE-40, -goal_position.y*SCALE+45), font_size=35)) # Add label 204 | 205 | # draw start arrow 206 | start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0) 207 | start_arrow.translate(start_position.x*SCALE,-start_position.y*SCALE) 208 | start_arrow.rotate( quaternion2YawDegree(start_pose.orientation) ) 209 | start_arrow.scale(3) 210 | svg.add(start_arrow) 211 | 212 | # draw goal arrow 213 | goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0) 214 | goal_arrow.translate(goal_position.x*SCALE,-goal_position.y*SCALE) 215 | goal_arrow.rotate( quaternion2YawDegree(goal_pose.orientation) ) 216 | goal_arrow.scale(3) 217 | svg.add(goal_arrow) 218 | 219 | # Draw obstacles 220 | for obstacle in feedbackMsg.obstacles: 221 | if len(obstacle.polygon.points) == 1: # point obstacle 222 | point = obstacle.polygon.points[0] 223 | svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=OBSTACLE_DIST, stroke_width=1, stroke='grey', fill ='grey', opacity=0.3)) 224 | svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=15, stroke_width=1, stroke='black', fill ='black')) 225 | svg.add(svg.text("Obstacle", (point.x*SCALE-70, -point.y*SCALE+45), font_size=35)) # Add label 226 | if len(obstacle.polygon.points) == 2: # line obstacle 227 | line_start = obstacle.polygon.points[0] 228 | line_end = obstacle.polygon.points[1] 229 | svg.add(svg.line(start=(line_start.x*SCALE,-line_start.y*SCALE), end=(line_end.x*SCALE,-line_end.y*SCALE), stroke='black', fill='gray', stroke_width=1, opacity=1.0)) 230 | svg.add(svg.text("Obstacle", (line_start.x*SCALE-70, -line_start.y*SCALE+45), font_size=35)) # Add label 231 | if len(obstacle.polygon.points) > 2: # polygon obstacle 232 | vertices = [] 233 | for point in obstacle.polygon.points: 234 | vertices.append((point.x*SCALE, -point.y*SCALE)) 235 | svg.add(svg.polygon(points=vertices, stroke='black', fill='gray', stroke_width=1, opacity=1.0)) 236 | svg.add(svg.text("Obstacle", (obstacle.polygon.points[0].x*SCALE-70, -obstacle.polygon.points.y*SCALE+45), font_size=35)) # Add label 237 | 238 | 239 | 240 | # Save svg to file (svg_output.svg) and exit node 241 | svg.save() 242 | 243 | rospy.loginfo("Drawing completed.") -------------------------------------------------------------------------------- /scripts/publish_dynamic_obstacle.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Author: franz.albers@tu-dortmund.de 4 | 5 | import rospy, math, tf 6 | from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg 7 | from geometry_msgs.msg import PolygonStamped, Point32, QuaternionStamped, Quaternion, TwistWithCovariance 8 | from tf.transformations import quaternion_from_euler 9 | 10 | 11 | def publish_obstacle_msg(): 12 | pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) 13 | #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) 14 | rospy.init_node("test_obstacle_msg") 15 | 16 | y_0 = -3.0 17 | vel_x = 0.0 18 | vel_y = 0.3 19 | range_y = 6.0 20 | 21 | obstacle_msg = ObstacleArrayMsg() 22 | obstacle_msg.header.stamp = rospy.Time.now() 23 | obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map 24 | 25 | # Add point obstacle 26 | obstacle_msg.obstacles.append(ObstacleMsg()) 27 | obstacle_msg.obstacles[0].id = 99 28 | obstacle_msg.obstacles[0].polygon.points = [Point32()] 29 | obstacle_msg.obstacles[0].polygon.points[0].x = -1.5 30 | obstacle_msg.obstacles[0].polygon.points[0].y = 0 31 | obstacle_msg.obstacles[0].polygon.points[0].z = 0 32 | 33 | yaw = math.atan2(vel_y, vel_x) 34 | q = tf.transformations.quaternion_from_euler(0,0,yaw) 35 | obstacle_msg.obstacles[0].orientation = Quaternion(*q) 36 | 37 | obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x 38 | obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y 39 | obstacle_msg.obstacles[0].velocities.twist.linear.z = 0 40 | obstacle_msg.obstacles[0].velocities.twist.angular.x = 0 41 | obstacle_msg.obstacles[0].velocities.twist.angular.y = 0 42 | obstacle_msg.obstacles[0].velocities.twist.angular.z = 0 43 | 44 | r = rospy.Rate(10) # 10hz 45 | t = 0.0 46 | while not rospy.is_shutdown(): 47 | 48 | # Vary y component of the point obstacle 49 | if (vel_y >= 0): 50 | obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y 51 | else: 52 | obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y 53 | 54 | t = t + 0.1 55 | 56 | pub.publish(obstacle_msg) 57 | 58 | r.sleep() 59 | 60 | 61 | 62 | if __name__ == '__main__': 63 | try: 64 | publish_obstacle_msg() 65 | except rospy.ROSInterruptException: 66 | pass 67 | 68 | -------------------------------------------------------------------------------- /scripts/publish_test_obstacles.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Author: christoph.roesmann@tu-dortmund.de 4 | 5 | import rospy, math 6 | from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg 7 | from geometry_msgs.msg import PolygonStamped, Point32 8 | 9 | 10 | def publish_obstacle_msg(): 11 | pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) 12 | #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) 13 | rospy.init_node("test_obstacle_msg") 14 | 15 | 16 | obstacle_msg = ObstacleArrayMsg() 17 | obstacle_msg.header.stamp = rospy.Time.now() 18 | obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map 19 | 20 | # Add point obstacle 21 | obstacle_msg.obstacles.append(ObstacleMsg()) 22 | obstacle_msg.obstacles[0].id = 0 23 | obstacle_msg.obstacles[0].polygon.points = [Point32()] 24 | obstacle_msg.obstacles[0].polygon.points[0].x = 1.5 25 | obstacle_msg.obstacles[0].polygon.points[0].y = 0 26 | obstacle_msg.obstacles[0].polygon.points[0].z = 0 27 | 28 | 29 | # Add line obstacle 30 | obstacle_msg.obstacles.append(ObstacleMsg()) 31 | obstacle_msg.obstacles[1].id = 1 32 | line_start = Point32() 33 | line_start.x = -2.5 34 | line_start.y = 0.5 35 | #line_start.y = -3 36 | line_end = Point32() 37 | line_end.x = -2.5 38 | line_end.y = 2 39 | #line_end.y = -4 40 | obstacle_msg.obstacles[1].polygon.points = [line_start, line_end] 41 | 42 | # Add polygon obstacle 43 | obstacle_msg.obstacles.append(ObstacleMsg()) 44 | obstacle_msg.obstacles[1].id = 2 45 | v1 = Point32() 46 | v1.x = -1 47 | v1.y = -1 48 | v2 = Point32() 49 | v2.x = -0.5 50 | v2.y = -1.5 51 | v3 = Point32() 52 | v3.x = 0 53 | v3.y = -1 54 | obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3] 55 | 56 | 57 | r = rospy.Rate(10) # 10hz 58 | t = 0.0 59 | while not rospy.is_shutdown(): 60 | 61 | # Vary y component of the point obstacle 62 | obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t) 63 | t = t + 0.1 64 | 65 | pub.publish(obstacle_msg) 66 | 67 | r.sleep() 68 | 69 | 70 | 71 | if __name__ == '__main__': 72 | try: 73 | publish_obstacle_msg() 74 | except rospy.ROSInterruptException: 75 | pass 76 | 77 | -------------------------------------------------------------------------------- /scripts/publish_viapoints.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Author: christoph.roesmann@tu-dortmund.de 4 | 5 | import rospy, math 6 | from geometry_msgs.msg import PoseStamped 7 | from nav_msgs.msg import Path 8 | 9 | 10 | def publish_via_points_msg(): 11 | pub = rospy.Publisher('/test_optim_node/via_points', Path, queue_size=1) 12 | rospy.init_node("test_via_points_msg") 13 | 14 | 15 | via_points_msg = Path() 16 | via_points_msg.header.stamp = rospy.Time.now() 17 | via_points_msg.header.frame_id = "odom" # CHANGE HERE: odom/map 18 | 19 | # Add via-points 20 | point1 = PoseStamped() 21 | point1.pose.position.x = 0.0; 22 | point1.pose.position.y = 1.5; 23 | 24 | point2 = PoseStamped() 25 | point2.pose.position.x = 2.0; 26 | point2.pose.position.y = -0.5; 27 | 28 | 29 | via_points_msg.poses = [point1, point2] 30 | 31 | r = rospy.Rate(5) # 10hz 32 | t = 0.0 33 | while not rospy.is_shutdown(): 34 | 35 | pub.publish(via_points_msg) 36 | 37 | r.sleep() 38 | 39 | 40 | 41 | if __name__ == '__main__': 42 | try: 43 | publish_via_points_msg() 44 | except rospy.ROSInterruptException: 45 | pass 46 | 47 | -------------------------------------------------------------------------------- /scripts/visualize_velocity_profile.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # This small script subscribes to the FeedbackMsg message of teb_local_planner 4 | # and plots the current velocity. 5 | # publish_feedback must be turned on such that the planner publishes this information. 6 | # Author: christoph.roesmann@tu-dortmund.de 7 | 8 | import rospy, math 9 | from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg 10 | from geometry_msgs.msg import PolygonStamped, Point32 11 | import numpy as np 12 | import matplotlib.pyplot as plotter 13 | 14 | def feedback_callback(data): 15 | global trajectory 16 | 17 | if not data.trajectories: # empty 18 | trajectory = [] 19 | return 20 | trajectory = data.trajectories[data.selected_trajectory_idx].trajectory 21 | 22 | 23 | def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega): 24 | ax_v.cla() 25 | ax_v.grid() 26 | ax_v.set_ylabel('Trans. velocity [m/s]') 27 | ax_v.plot(t, v, '-bx') 28 | ax_omega.cla() 29 | ax_omega.grid() 30 | ax_omega.set_ylabel('Rot. velocity [rad/s]') 31 | ax_omega.set_xlabel('Time [s]') 32 | ax_omega.plot(t, omega, '-bx') 33 | fig.canvas.draw() 34 | 35 | 36 | 37 | def velocity_plotter(): 38 | global trajectory 39 | rospy.init_node("visualize_velocity_profile", anonymous=True) 40 | 41 | topic_name = "/test_optim_node/teb_feedback" 42 | topic_name = rospy.get_param('~feedback_topic', topic_name) 43 | rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here! 44 | 45 | rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) 46 | rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.") 47 | 48 | # two subplots sharing the same t axis 49 | fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True) 50 | plotter.ion() 51 | plotter.show() 52 | 53 | 54 | r = rospy.Rate(2) # define rate here 55 | while not rospy.is_shutdown(): 56 | 57 | t = [] 58 | v = [] 59 | omega = [] 60 | 61 | for point in trajectory: 62 | t.append(point.time_from_start.to_sec()) 63 | v.append(point.velocity.linear.x) 64 | omega.append(point.velocity.angular.z) 65 | 66 | plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega)) 67 | 68 | r.sleep() 69 | 70 | if __name__ == '__main__': 71 | try: 72 | trajectory = [] 73 | velocity_plotter() 74 | except rospy.ROSInterruptException: 75 | pass 76 | 77 | -------------------------------------------------------------------------------- /src/obstacles.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2016, 6 | * TU Dortmund - Institute of Control Theory and Systems Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Author: Christoph Rösmann 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include 42 | // #include 43 | 44 | namespace teb_local_planner 45 | { 46 | 47 | 48 | void PolygonObstacle::fixPolygonClosure() 49 | { 50 | if (vertices_.size()<2) 51 | return; 52 | 53 | if (vertices_.front().isApprox(vertices_.back())) 54 | vertices_.pop_back(); 55 | } 56 | 57 | void PolygonObstacle::calcCentroid() 58 | { 59 | if (vertices_.empty()) 60 | { 61 | centroid_.setConstant(NAN); 62 | ROS_WARN("PolygonObstacle::calcCentroid(): number of vertices is empty. the resulting centroid is a vector of NANs."); 63 | return; 64 | } 65 | 66 | // if polygon is a point 67 | if (noVertices()==1) 68 | { 69 | centroid_ = vertices_.front(); 70 | return; 71 | } 72 | 73 | // if polygon is a line: 74 | if (noVertices()==2) 75 | { 76 | centroid_ = 0.5*(vertices_.front() + vertices_.back()); 77 | return; 78 | } 79 | 80 | // otherwise: 81 | 82 | centroid_.setZero(); 83 | 84 | // calculate centroid (see wikipedia http://de.wikipedia.org/wiki/Geometrischer_Schwerpunkt#Polygon) 85 | double A = 0; // A = 0.5 * sum_0_n-1 (x_i * y_{i+1} - x_{i+1} * y_i) 86 | for (int i=0; i < noVertices()-1; ++i) 87 | { 88 | A += vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1); 89 | } 90 | A += vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1); 91 | A *= 0.5; 92 | 93 | if (A!=0) 94 | { 95 | for (int i=0; i < noVertices()-1; ++i) 96 | { 97 | double aux = (vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1)); 98 | centroid_ += ( vertices_.at(i) + vertices_.at(i+1) )*aux; 99 | } 100 | double aux = (vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1)); 101 | centroid_ += ( vertices_.at(noVertices()-1) + vertices_.at(0) )*aux; 102 | centroid_ /= (6*A); 103 | } 104 | else // A == 0 -> all points are placed on a 'perfect' line 105 | { 106 | // seach for the two outer points of the line with the maximum distance inbetween 107 | int i_cand = 0; 108 | int j_cand = 0; 109 | double max_dist = 0; 110 | for (int i=0; i< noVertices(); ++i) 111 | { 112 | for (int j=i+1; j< noVertices(); ++j) // start with j=i+1 113 | { 114 | double dist = (vertices_[j] - vertices_[i]).norm(); 115 | if (dist > max_dist) 116 | { 117 | max_dist = dist; 118 | i_cand = i; 119 | j_cand = j; 120 | } 121 | } 122 | } 123 | // calc centroid of that line 124 | centroid_ = 0.5*(vertices_[i_cand] + vertices_[j_cand]); 125 | } 126 | } 127 | 128 | 129 | 130 | Eigen::Vector2d PolygonObstacle::getClosestPoint(const Eigen::Vector2d& position) const 131 | { 132 | // the polygon is a point 133 | if (noVertices() == 1) 134 | { 135 | return vertices_.front(); 136 | } 137 | 138 | if (noVertices() > 1) 139 | { 140 | 141 | Eigen::Vector2d new_pt = closest_point_on_line_segment_2d(position, vertices_.at(0), vertices_.at(1)); 142 | 143 | if (noVertices() > 2) // real polygon and not a line 144 | { 145 | double dist = (new_pt-position).norm(); 146 | Eigen::Vector2d closest_pt = new_pt; 147 | 148 | // check each polygon edge 149 | for (int i=1; i 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | namespace teb_local_planner 47 | { 48 | 49 | // ============== FailureDetector Implementation =================== 50 | 51 | void FailureDetector::update(const geometry_msgs::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps) 52 | { 53 | if (buffer_.capacity() == 0) 54 | return; 55 | 56 | VelMeasurement measurement; 57 | measurement.v = twist.linear.x; // just consider linear velocity in x-direction in the robot frame for now 58 | measurement.omega = twist.angular.z; 59 | 60 | if (measurement.v > 0 && v_max>0) 61 | measurement.v /= v_max; 62 | else if (measurement.v < 0 && v_backwards_max > 0) 63 | measurement.v /= v_backwards_max; 64 | 65 | if (omega_max > 0) 66 | measurement.omega /= omega_max; 67 | 68 | buffer_.push_back(measurement); 69 | 70 | // immediately compute new state 71 | detect(v_eps, omega_eps); 72 | } 73 | 74 | void FailureDetector::clear() 75 | { 76 | buffer_.clear(); 77 | oscillating_ = false; 78 | } 79 | 80 | bool FailureDetector::isOscillating() const 81 | { 82 | return oscillating_; 83 | } 84 | 85 | bool FailureDetector::detect(double v_eps, double omega_eps) 86 | { 87 | oscillating_ = false; 88 | 89 | if (buffer_.size() < buffer_.capacity()/2) // we start detecting only as soon as we have the buffer filled at least half 90 | return false; 91 | 92 | double n = (double)buffer_.size(); 93 | 94 | // compute mean for v and omega 95 | double v_mean=0; 96 | double omega_mean=0; 97 | int omega_zero_crossings = 0; 98 | for (int i=0; i < n; ++i) 99 | { 100 | v_mean += buffer_[i].v; 101 | omega_mean += buffer_[i].omega; 102 | if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) ) 103 | ++omega_zero_crossings; 104 | } 105 | v_mean /= n; 106 | omega_mean /= n; 107 | 108 | if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps && omega_zero_crossings>1 ) 109 | { 110 | oscillating_ = true; 111 | } 112 | // ROS_INFO_STREAM("v: " << std::abs(v_mean) << ", omega: " << std::abs(omega_mean) << ", zero crossings: " << omega_zero_crossings); 113 | return oscillating_; 114 | } 115 | 116 | 117 | 118 | } // namespace teb_local_planner 119 | -------------------------------------------------------------------------------- /teb_local_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The teb_local_planner package implements a plugin 5 | to the base_local_planner of the 2D navigation stack. 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /teb中的G2O问题的设置.md: -------------------------------------------------------------------------------- 1 | # TEB中的g2o问题的设置 2 | 3 | ## g2o问题的表述形式 4 | 5 | 参照论文[^1]给出非线性优化问题的格式： 6 | $$ 7 | V^{*}(\mathbf{b})=\min _{\mathbf{b}} \sum_{k=1}^{n-1} \Delta T_{k}^{2} 8 | $$ 9 | subject to 10 | $$ 11 | \begin{array}{l}{\mathbf{s}_{1}=\mathbf{s}_{s}, \quad \mathbf{s}_{n}=\mathbf{s}_{f}, \quad \Delta T_{k}>0} \\ {\mathbf{h}_{k}\left(\mathbf{s}_{k+1}, \mathbf{s}_{k}\right)=\mathbf{0}} \\ {r_{k}\left(\mathbf{s}_{k+1}, \mathbf{s}_{k}\right) \geq 0} \\ {\mathbf{o}_{k}\left(\mathbf{s}_{k}\right) \geq \mathbf{0}} \\ {\boldsymbol{v}_{k}\left(\mathbf{s}_{k+1}, \mathbf{s}_{k}, \Delta T_{k}\right) \geq \mathbf{0}, \quad(k=1,2, \ldots, n-1)} \\ {\boldsymbol{\alpha}_{k}\left(\mathbf{s}_{k+1}, \mathbf{s}_{k+1}, \mathbf{s}_{k}, \Delta T_{k+1}, \Delta T_{k}\right) \geq \mathbf{0}, \quad(k=2,3, \ldots, n-2)} \\ {\boldsymbol{\alpha}_{1}\left(\mathbf{s}_{2}, \mathbf{s}_{1}, \Delta T_{1}\right) \geq \mathbf{0}, \quad \boldsymbol{\alpha}_{n}\left(\mathbf{s}_{n}, \mathbf{s}_{n-1}, \Delta T_{n-1}\right) \geq \mathbf{0}}\end{array} 12 | $$ 13 | 其中，$\mathbf{b}=\left[\mathbf{s}_{1}, \Delta T_{1}, \mathbf{s}_{2}, \Delta T_{2}, \mathbf{s}_{3}, \ldots, \Delta T_{n-1}, \mathbf{s}_{n}\right]^{\top}$。 14 | 15 | ## 顶点 16 | 17 | 顶点分为两类：pose与timediff 18 | 19 | ### vetex_pose 20 | 21 | * 定义于g2o_types/vetex_pose.h 22 | * 优化变量的数据类型定义为pose_se2，定义于pose_se2.h，由三个分量组成：x,y,theta，优化维度为三维。 23 | * 在Teb初始化的过程中，将起点的Pose与终点的Pose所在的顶点设为fixed，使得g20不对这两个Pose进行优化。 24 | * 初始化某一个TEB时，其路线简单的由起点和终点的连线组成。随后在这条直线上均匀采样点作为待优化的顶点。采样的步长由cfg_->trajectory.min_samples 决定。而 timediff 顶点的初始值为步长除以 cfg_->robot.max_vel_x。每有一个pose顶点就产生一个time_diff顶点。time_diff顶点实际上是每两个Pose之间所需要的时间。 25 | 26 | ### vetex_timediff 27 | 28 | * 定义于g20_types/vetex_timediff.h 29 | * 优化变量的数据类型为double。 30 | * 由上所述，其生成在Teb初始化的时候。具体在timed_elastic_band.h/.cpp中的TimedElasticBand::initTrajectoryToGoal中。 31 | 32 | ## 边 33 | 34 | ### 障碍物约束 35 | 36 | 在AddEdgeObstacle函数中，只将离某个Pose最近的最左边与最右边的两个Obstacle加入优化中。（因为优化路径不会使得路径相对于障碍物的位置关系发生改变）。同时，还设了一个阈值，凡是离该Pose距离低于某个距离的障碍物也一并加入考虑之中。 37 | 38 | #### EdgeObstacle 39 | 40 | * 定义于g2o_types/edge_obstacle.h中，当inflated=false时使用此障碍物边 41 | 42 | * 一元边，观测值维度为1，测量值类型为Obstacle基类，连接VertexPose顶点 43 | 44 | * 存储了某个障碍物的中心点的三维位置，形状与顶点的位置 45 | 46 | * 根据机器人的轮廓模型计算当前Pose与某个障碍物的距离 47 | 48 | * $$ 49 | error = dist > min\_obstacle\_dist + \epsilon ? 0 : (min\_obstacle\_dist + \epsilon) - dist 50 | $$ 51 | 52 | * 信息矩阵为cfg_->optim.weight_obstacle * weight_multiplier 53 | 54 | #### EdgeInflatedObstacle 55 | 56 | * 定义于g2o_types/edge_obstacle.h中，当inflated=true时使用此障碍物边 57 | 58 | * 一元边，观测值维度为2，类型为Obstacle基类，连接VertexPose顶点 59 | 60 | * $$ 61 | error[0] = dist > min\_obstacle\_dist + \epsilon ? 0 : (min\_obstacle\_dist + \epsilon) - dist \\ 62 | error[1] = dist > inflation\_dist ? 0 : inflation\_dist - dist \\ 63 | $$ 64 | 65 | * 信息矩阵为对角阵，(0,0) = weight_obstacle 66 | 67 | ### via_point约束 68 | 69 | via_point是一类点，其规定了轨迹应当经过这些点，否则会产生相应的cost。via_point边会与原规划的路径中与其距离最近的Pose顶点相连。 70 | 71 | #### EdgeViaPoint 72 | 73 | * 定义于g2o_types/edge_via_point.h中 74 | * 一元边，观测值维度为1，类型为Eigen::Vector2d*，连接VertexPose顶点 75 | * 存储了某个via_point的位置。 76 | * error为其连接的Pose顶点的位置到这个Viapoint的距离的模长。 77 | * 信息矩阵为1x1的矩阵，其值为weight_viapoint 78 | 79 | ### 速度约束 80 | 81 | #### EdgeVelocity 82 | 83 | * 定义于g2o_types/edge_via_point.h中 84 | * 三元边，观测值变量维度为2，类型为double,连接两个VetexPose与一个VertexTimeDiff 85 | * 速度由两个VetexPose间的距离除以时间得到。角速度由两个VetexPose间的角度除以时间得到 86 | * error有两项，分别是线速度与线速度线速度是否在设定好的区间内。 87 | * 信息矩阵为3x3对角矩阵，(0,0) = weight_max_vel_x, (1,1) = weight_max_vel_y, (2,2) =weight_max_vel_theta（对于全向轮底盘来说） 88 | 89 | ### 加速度约束 90 | 91 | ####　EdgeAcceleration 92 | 93 | * 定义于g2o_types/edge_acceleration.h中 94 | * 五元边，观测值维度为2，类型为double，连接三个pose与两个timediff顶点 95 | * 根据三个Pose与两个timediff做两次差分得到线加速度与角加速度。 96 | * error有两项，分别是线加速度与角加速度是否在设定好的区间内。 97 | * 信息矩阵为2x2对角矩阵（对于阿克曼底盘来说），(0,0) =weight_acc_lim_x, (1,1) = weight_acc_lim_theta, 98 | 99 | ### 时间最优约束 100 | 101 | #### EdgeTimeOptimal 102 | 103 | * 定义于g2o_types/edge_time_optimal.h中 104 | * 一元边，观测值维度为１，数据类型为double，连接一个VertexTimeDiff 105 | * error直接就是连接的VertexTimeDiff的dt本身 106 | * 信息矩阵为1x1矩阵，其值为weight_optimaltime 107 | 108 | ### 动力学约束 109 | 110 | #### EdgeKinematicsCarlike 111 | 112 | * 定义于g2o_types/edge_time_optimal.h中 113 | * 二元边，观测值维度为2，数据类型为double，连接两个VertexPose 114 | * 误差由文章[^1]中的动力学约束提出。阿克曼底盘模型还增加了最小转弯半径的约束。 115 | 116 | 117 | 118 | 119 | 120 | ``` 121 | [^1]：Integrated online trajectory planning and optimization in distinctive topologies 122 | ``` --------------------------------------------------------------------------------