HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

407
navigations/teb_local_planner/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,407 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package teb_local_planner
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.9.1 (2020-05-29)
------------------
* Fixed RobotFootprintModel visualization bug (thanks to Anson Wang)
* Reserve the size of the relevant obstacles vector to avoid excessive memory allocations (thanks to João Monteiro)
* CMake: Removed system include to avoid compiling issues on some platforms
* Contributors: Anson Wang, Christoph Rösmann, João Carlos Espiúca Monteiro
0.9.0 (2020-05-26)
------------------
* Added pill resp. stadium-shaped obstacle
* Changed minimum CMake version to 3.1
* Improved efficiency of 3d h-signature computation
* Changed default value for parameter penalty_epsilon to 0.05
* Improved efficiency of findClosedTrajectoryPose()
* Removed obsolete method isHorizonReductionAppropriate()
* Contributors: Christoph Rösmann, XinyuKhan
0.8.4 (2019-12-02)
------------------
* Fixed TEB autoResize if last TimeDiff is small
* Add a rotational threshold for identifying a warm start goal
* Contributors: Rainer Kümmerle
0.8.3 (2019-10-25)
------------------
* Limiting the control look-ahead pose to the first that execeeds the expected look-ahead time (thanks to Marco Bassa)
* test_optim_node fix circular obstacles (thanks to dtaranta)
* Fix shadow variable warning (thanks to Victor Lopez)
* Use SYSTEM when including external dependencies headers (thanks to Victor Lopez)
* Robustify initTrajectoryToGoal if a plan is given (thanks to Rainer Kuemmerle)
* Adding the option to shift ahead the target pose used to extract the velocity command (thanks to Marco Bassa)
* Fixed segfault in optimal_planner.cpp when clearing graph with unallocated optimizer.
Fixes `#158 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/158>`_.
* On footprintCost, fail only if footprint is in collision, not outside the map or on unknown space (thanks to corot)
* Native MoveBaseFlex support added: Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces (thanks to corot)
* added warning if parameter optimal_time is <= 0
* Nonlinear obstacle cost from EdgeInflatedObstacle also added to EdgeObstacle.
See `#140 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/140>`_.
* Fixed proper initialization of parameter obstacle_cost_exponent in case it is not loaded from the parameter server
* Contributors: Christoph Rösmann, Marco Bassa, Rainer Kuemmerle, Victor Lopez, corot, dtaranta
0.8.2 (2019-07-02)
------------------
* Allow scripts to be executable and usable by rosrun after catkin_make install and through the catkin release process (thanks to Devon Ash)
* Add nonlinear part to obstacle cost to improve narrow gap behavior.
Parameter `obstacle_cost_exponent` defines the exponent of the nonlinear cost term.
The default linear behavior is achieved by setting this parameter to 1 (default).
A value of 4 performed well in some tests and experiments (thanks to Howard Cochran).
* Parameter `global_plan_prune_distance` added via ros parameter server.
* Fixed SIGSEGV in optimizeAllTEBs() if main thread is interrupted by boost (thanks to Howard Cochran)
* Fixed SIGSEGV crash in deleteTebDetours() (thanks to Howard Cochran)
* On footprint visualization, avoid overshadowing by obstacles (thanks to corot)
* Do not ignore robot model on the association stage.
Important mostly for polygon footprint model (thanks to corot).
* Adjustable color for footprint visualization
* Showing (detected) infeasible robot poses in a separate marker namespace and color
* Added edge for minimizing Euclidean path length (parameter: `weight_shortest_path`)
* Ackermann steering conversion (python script): fixed direction inversion in backwards mode when `cmd_angle_instead_rotvel` is true (thanks to Tobi Loew)
* Fixed wrong skipping condition in AddEdgesKinematicsCarlike() (thanks to ShiquLIU)
* Never discarding the previous best teb in renewAndAnalyzeOldTebs (thanks to Marco Bassa)
* Allowing for the fallback to a different trajectory when the costmap check fails. This prevents the switch to unfeasible trajectories (thanks to Marco Bassa).
* Skipping the generation of the homotopy exploration graph in case the maximum number of allowed classes is reached (thanks to Marco Bassa)
* Changed isTrajectoryFeasible function to allow for a more accurate linear and angular discretization (thanks to Marco Bassa)
* Function TebOptimalPlanner::computeError() considers now the actual optimizer weights.
As a result, the default value of `selection_obst_cost_scale` is reduced (thanks to Howard Cochran).
* update to use non deprecated pluginlib macro (thanks to Mikael Arguedas)
* Avoiding h signature interpolation between coincident poses (thanks to Marco Bassa)
* New strategy for the deletion of detours: Detours are now determined w.r.t. the least-cost alternative and not w.r.t. just the goal heading.
Deletion of additional alternatives applies if either an initial backward motion is detected, if the transition time is much bigger than the duration of the best teb
and if a teb cannot be optimized (thanks to Marco Bassa).
Optionally allowing the usage of the initial plan orientation when initializing new tebs.
* Contributors: Christoph Rösmann, Mikael Arguedas, Devon Ash, Howard Cochran, Marco Bassa, ShiquLIU, Tobi Loew, corot
0.8.1 (2018-08-14)
------------------
* bugfix in calculateHSignature. Fixes `#90 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/90>`_.
* fixed centroid computation in a special case of polygon-obstacles
* Contributors: Christoph Rösmann
0.8.0 (2018-08-06)
------------------
* First melodic release
* Updated to new g2o API
* Migration to tf2
* Contributors: Christoph Rösmann
0.7.3 (2018-07-05)
------------------
* Parameter `switching_blocking_period` added to homotopy class planner parameter group.
Values greater than zero enforce the homotopy class planner to only switch to new equivalence classes as soon
as the given period is expired (this might reduce oscillations in some scenarios). The value is set to zero seconds
by default in order to not change the behavior of existing configurations.
* Fixed unconsistent naming of parameter `global_plan_viapoint_sep`.
The parameter retrieved at startup was `global_plan_via_point_sep` and via dynamic_reconfigure it was `global_plan_viapoint_sep`.
`global_plan_via_point_sep` has now been replaced by `global_plan_viapoint_sep` since this is more consistent with the variable name
in the code as well as `weight_viapoint` and the ros wiki description.
In order to not break things, the old parameter name can still be used. However, a deprecated warning is printed.
* transformGlobalPlan searches now for the closest point within the complete subset of the global plan in the local costmap:
In every sampling interval, the global plan is processed in order to find the closest pose to the robot (as reference start)
and the current end pose (either local at costmap boundary or max_global_plan_lookahead_dist).
Previously, the search algorithm stopped as soon as the distance to the robot increased once.
This caused troubles with more complex global plans, hence the new strategy checks the complete subset
of the global plan in the local costmap for the closest distance to the robot.
* via-points that are very close to the current robot pose or behind the robot are now skipped (in non-ordered mode)
* Edge creation: minor performance improvement for dynamic obstacle edges
* dynamic_reconfigure: parameter visualize_with_time_as_z_axis_scale moved to group trajectory
* Contributors: Christoph Rösmann
0.7.2 (2018-06-08)
------------------
* Adds the possibility to provide via-points via a topic.
Currently, the user needs to decide whether to receive via-points from topic or to obtain them from the global reference plan
(e.g., activate the latter by setting global_plan_viapoint_sep>0 as before).
A small test script publish_viapoints.py is provided to demonstrate the feature within test_optim_node.
* Contributors: Christoph Rösmann
0.7.1 (2018-06-05)
------------------
* Fixed a crucial bug (from 0.6.6): A cost function for prefering a clockwise resp. anti-clockwise turn was enabled by default.
This cost function was only intended to be active only for recovering from an oscillating robot.
This cost led to a penalty for one of the turning directions and hence the maximum turning rate for the penalized direction could not be reached.
Furthermore, which is more crucial: since the penalty applied only to a small (initial) subset of the trajectory, the overall control performance was poor
(huge gap between planned motion and closed-loop trajectories led to frequent corrections of the robot pose and hence many motion reversals).
* Adds support for circular obstacle types. This includes support for the radius field in costmap_converter::ObstacleMsg
* rqt reconfigure: parameters are now grouped in tabs (robot, trajectory, viapoints, ...)
* Update to use non deprecated pluginlib macro
* Python scripts updated to new obstacle message definition.
* Fixed issue when start and end are at the same location (PR #43)
* Normalize marker quaternions in *test_optim_node*
* Contributors: Christoph Rösmann, Alexander Reimann, Mikael Arguedas, wollip
0.7.0 (2017-09-23)
------------------
* This update introduces support for dynamic obstacles (thanks to Franz Albers, who implemented and tested the code).
Dynamic obstacle support requires parameter *include\_dynamic\_obstacles* to be activated.
Note, this feature is still experimental and subject to testing.
Motion prediction is performed using a constant velocity model.
Dynamic obstacles might be incorporated as follows:
* via a custom message provided on topic ~/obstacles (warning: we changed the message type from teb_local_planner/ObstacleMsg to costmap_converter/ObstacleArrayMsg).
* via the CostmapToDynamicObstacles plugin as part of the costmap\_converter package (still experimental).
A tutorial is going to be provided soon.
* FeedbackMsg includes a ObstacleMsg instead of a polygon
* ObstacleMsg removed from package since it is now part of the costmap\_converter package.
* Homotopy class planer code update: graph search methods and equivalence classes (h-signatures) are now
implemented as subclasses of more general interfaces.
* TEB trajectory initialization now uses a max\_vel\_x argument instead of the desired time difference in order to give the optimizer a better warm start.
Old methods are marked as deprecated. This change does not affect users settings.
* Inplace rotations removed from trajectory initialization to improve convergence speed of the optimizer
* teb\_local\_planner::ObstacleMsg removed in favor of costmap\_converter::ObstacleArrayMsg. This also requires custom obstacle publishers to update to the new format
* the "new" trajectory resizing method is only activated, if "include_dynamic_obstacles" is set to true.
We introduced the non-fast mode with the support of dynamic obstacles
(which leads to better results in terms of x-y-t homotopy planning).
However, we have not yet tested this mode intensively, so we keep
the previous mode as default until we finish our tests.
* added parameter and code to update costmap footprint if it is dynamic (#49)
* Contributors: Franz Albers, Christoph Rösmann, procopiostein
0.6.6 (2016-12-23)
------------------
* Strategy for recovering from oscillating local plans added (see new parameters)
* Horizon reduction for resolving infeasible trajectories is not activated anymore if the global goal is already selected
(to avoid oscillations due to changing final orientations)
* Global plan orientations are now taken for TEB initialization if lobal_plan_overwrite_orientation==true
* Parameter max_samples added
* Further fixes (thanks to Matthias Füller and Daniel Neumann for providing patches)
0.6.5 (2016-11-15)
------------------
* The trajectory is now initialized backwards for goals close to and behind the robot.
Parameter 'allow_init_with_backwards_motion' added.
* Updated the TEB selection in the HomotopyClassPlanner.
* A new parameter is introduced to prefer the equivalence class of the initial plan
* Fixed some bugs related to the deletion of candidates and for keeping the equivalence class of the initial plan.
* Weight adaptation added for obstacles edges.
Added parameter 'weight_adapt_factor'.
Obstacle weights are repeatedly scaled by this factor in each outer TEB iteration.
Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions.
* Added a warning if the optim footprint + min_obstacle_dist is smaller than the costmap footprint.
Validation is performed by only comparing the inscribed radii of the footprints.
* Revision/extension of the reduced-horizon backup mode which is triggered in case infeasible trajectories are detected.
* Changed HSignature to a generic equivalence class
* Minor changes
0.6.4 (2016-10-23)
------------------
* New default obstacle association strategy:
During optimization graph creation, for each pose of the trajectory a
relevance detection is performed before considering the obstacle
during optimization. New parameters are introduced. The
old strategy is kept as 'legacy' strategy (see parameters).
* Computation of velocities, acceleration and turning radii extended:
Added an option to compute the actual arc length
instead of using the Euclidean distance approximation (see parameter `exact_arc_length`.
* Added intermediate edge layer for unary, binary and multi edges in order to reduce code redundancy.
* Script for visualizing velocity profile updated to accept the feedback topic name via rosparam server
* Removed TebConfig dependency in TebVisualization
* PolygonObstacle can now be constructed using a vertices container
* HomotopyClassPlanner public interface extended
* Changed H-Signature computation to work 'again' with few obstacles such like 1 or 2
* Removed inline flags in visualization.cpp
* Removed inline flags in timed_elastic_band.cpp.
Fixes `#15 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/15>`_.
* Increased bounds of many variables in dynamic_reconfigure.
Resolves `#14 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/14>`_.
The particular variables are maximum velocities, maximum accelerations,
minimum turning radius,...
Note: optimization weights and dt_ref as well as dt_hyst are not
tuned for velocities and accelerations beyond
the default values (e.g. >1 m/s). Just increasing the maximum velocity
bounds without adjusting the other parameters leads to an insufficient behavior.
* Default parameter value update: 'costmap_obstacles_behind_robot_dist'
* Additional minor fixes.
0.6.3 (2016-08-17)
------------------
* Changed the f0 function for calculating the H-Signature.
The new one seems to be more robust for a much larger number of obstacles
after some testing.
* HomotopyClassPlanner: vertex collision check removed since collisions will be determined in the edge collision check again
* Fixed distance calculation polygon-to-polygon-obstacle
* cmake config exports now *include directories* of external packages for dependent projects
* Enlarged upper bounds on goal position and orientation tolerances in *dynamic_reconfigure*. Fixes #13.
0.6.2 (2016-06-15)
------------------
* Fixed bug causing the goal to disappear in case the robot arrives with non-zero orientation error.
* Inflation mode for obstacles added.
* The homotopy class of the global plan is now always forced to be initialized as trajectory.
* The initial velocity of the robot is now taken into account correctly for
all candidate trajectories.
* Removed a check in which the last remaining candidate trajectory was rejected if it was close to an obstacle.
This fix addresses issue `#7 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/7>`_
0.6.1 (2016-05-23)
------------------
* Debian ARM64 library path added to SuiteSparse cmake find-script (resolves ARM compilation issue)
0.6.0 (2016-05-22)
------------------
* Extended support to holonomic robots
* Wrong parameter namespace for *costmap_converter* plugins fixed
* Added the option to scale the length of the hcp sampling area
* Compiler warnings fixed.
* Workaround for compilation issues that are caused by a bug in boost 1.58
concerning the graph library (missing move constructor/assignment operator
in boost source).
* Using *tf_listener* from *move_base* now.
* Via-point support improved.
Added the possibility to take the actual order of via-points into account.
Additionally, via-points beyond start and goal are now included.
* Obsolete include of the angles package header removed
* Update to package.xml version 2
* Some other minor fixes.
0.4.0 (2016-04-19)
------------------
* The teb_local_planner supports a path-following mode (w.r.t. the global plan) and via-points now.
This allows the user to adapt the tradeoff between time-optimality and path-following.
Check out the new tutorial: "Following the Global Plan (Via-Points)".
* All external configuration and launch files are removed, since they are part
of the new teb_local_planner_tutorials package.
0.3.1 (2016-04-14)
------------------
* Fixed wrong coordinate transformation in 'line' and 'polygon' footprint models.
* Trajectory selection strategy in case of multiple topologies updated:
* The obstacle costs for selection can now be scaling separately.
* The cost regarding time optimality can now be replaced by the actual transition time.
* Added a hysteresis to cost comparison between a new and the previously selected trajectory.
* In the default parameter setting the strategy is similar to release 0.3.0.
* Warning message removed that occured if an odom message with only zeros was received.
0.3.0 (2016-04-08)
------------------
* Different/custom robot footprints are now supported and subject to optimization (refer to the new tutorial!).
* The new robot footprint is also visualized using the common marker topic.
* The strategy of taking occupied costmap cells behind the robot into account has been improved.
These changes significantly improve navigation close to walls.
* Parameter 'max_global_plan_lookahead_dist' added.
Previously, the complete subset of the global plan contained in the local costmap
was taken into account for choosing the current intermediate goal point. With this parameter, the maximum
length of the reference global plan can be limited. The actual global plan subset
is now computed using the logical conjunction of both local costmap size and 'max_global_plan_lookahead_dist'.
* Bug fixes:
* Fixed a compilation issue on ARM architectures
* If custom obstacles are used, the container with old obstacles is now cleared properly.
* Parameter cleanup:
* "weight_X_obstacle" parameters combined to single parameter "weight_obstacle".
* "X_obstacle_poses_affected" parameters combined to single parameter "obstacle_poses_affected".
* Deprecated parameter 'costmap_emergency_stop_dist' removed.
* Code cleanup
0.2.3 (2016-02-01)
------------------
* Marker lifetime changed
* In case the local planner detects an infeasible trajectory it does now try to
reduce the horizon to 50 percent of the length. The trajectory is only reduced
if some predefined cases are detected.
This mechanism constitutes a backup behavior.
* Improved carlike robot support.
Instead of commanding the robot using translational and rotational velocities,
the robot might also be commanded using the transl. velocity and steering angle.
Appropriate parameters are added to the config.
* Changed default parameter for 'h_signature_threshold' from 0.01 to 0.1 to better match the actual precision.
* Some python scripts for data conversion added
* Minor other changes
0.2.2 (2016-01-11)
------------------
* Carlike robots (ackermann steering) are supported from now on (at least experimentally)
by specifying a minimum bound on the turning radius.
Currently, the output of the planner in carlike mode is still (v,omega).
Since I don't have any real carlike robot, I would be really happy if someone could provide me with
some feedback to further improve/extend the support.
* Obstacle cost function modified to avoid undesired jerks in the trajectory.
* Added a feedback message that contains current trajectory information (poses, velocities and temporal information).
This is useful for analyzing and debugging the velocity profile e.g. at runtime.
The message will be published only if it's activated (rosparam).
A small python script is added to plot the velocity profile (while *test_optim_node* runs).
* Cost functions are now taking the direction/sign of the translational velocity into account:
Specifying a maximum backwards velocity other than forward velocity works now.
Additionally, the change in acceleration is now computed correctly if the robot switches directions.
* The global plan is now pruned such that already passed posses are cut off
(relevant for global planners with *planning_rate=0*).
* Fixed issue#1: If a global planner with *planning_rate=0* was used,
a TF timing/extrapolation issue appeared after some time.
* The planner resets now properly if the velocity command cannot be computed due to invalid optimization results.
0.2.1 (2015-12-30)
------------------
* This is an important bugfix release.
* Fixed a major issue concerning the stability and performance of the optimization process. Each time the global planner was updating the global plan, the local planner was resetted completely even if
the updated global plan did not differ from the previous one. This led to stupid reinitializations and a slighly jerky behavior if the update rate of the global planner was high (each 0.5-2s).
From now on the local planner is able to utilize the global plan as a warm start and determine automatically whether to reinitialize or not.
* Support for polygon obstacles extended and improved (e.g. the homotopy class planner does now compute actual distances to the polygon rather than utilizing the distance to the centroid).
0.2.0 (2015-12-23)
------------------
* The teb_local_planner supports costmap_converter plugins (pluginlib) from now on. Those plugins convert occupied costmap2d cells into polygon shapes.
The costmap_converter is disabled by default, since the extension still needs to be tested (parameter choices, computation time advantages, etc.).
A tutorial will explain how to activate the converter using the ros-param server.
0.1.11 (2015-12-12)
-------------------
* This is a bugfix release (it fixes a lot of issues which occured frequently when the robot was close to the goal)
0.1.10 (2015-08-13)
-------------------
* The optimizer copies the global plan as initialization now instead of using a simple straight line approximation.
* Some bugfixes and improvements
0.1.9 (2015-06-24)
------------------
* Fixed a segmentation fault issue. This minor update is crucial for stability.
0.1.8 (2015-06-08)
------------------
* Custom obstacles can be included via publishing dedicated messages
* Goal-reached-condition also checks orientation error (desired yaw) now
* Numerical improvements of the h-signature calculation
* Minor bugfixes
0.1.7 (2015-05-22)
------------------
* Finally fixed saucy compilation issue by retaining compatiblity to newer distros
(my "new" 13.10 VM helps me to stop spamming new releases for testing).
0.1.6 (2015-05-22)
------------------
* Fixed compilation errors on ubuntu saucy caused by different FindEigen.cmake scripts.
I am not able to test releasing on saucy, forcing me to release again and again. Sorry.
0.1.5 (2015-05-21)
------------------
* Added possibility to dynamically change parameters of test_optim_node using dynamic reconfigure.
* Fixed a wrong default-min-max tuple in the dynamic reconfigure config.
* Useful config and launch files are now added to cmake install.
* Added install target for the test_optim_node executable.
0.1.4 (2015-05-20)
------------------
* Fixed compilation errors on ROS Jade
0.1.3 (2015-05-20)
------------------
* Fixed compilation errors on ubuntu saucy
0.1.2 (2015-05-19)
------------------
* Removed unused include that could break compilation.
0.1.1 (2015-05-19)
------------------
* All files added to the indigo-devel branch
* Initial commit
* Contributors: Christoph Rösmann

284
navigations/teb_local_planner/CMakeLists.txt Executable file
View File

@@ -0,0 +1,284 @@
cmake_minimum_required(VERSION 3.1)
project(teb_local_planner)
# Set to Release in order to speed up the program significantly
set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
base_local_planner
costmap_2d
costmap_converter
cmake_modules
dynamic_reconfigure
geometry_msgs
interactive_markers
message_generation
nav_core
nav_msgs
mbf_costmap_core
mbf_msgs
roscpp
std_msgs
pluginlib
tf2
tf2_eigen
tf2_geometry_msgs
tf2_ros
visualization_msgs
)
message(STATUS "System: ${CMAKE_SYSTEM}")
## System dependencies are found with CMake's conventions
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules)
message(STATUS "${CMAKE_MODULE_PATH}")
find_package(Boost REQUIRED COMPONENTS system thread graph)
find_package(SUITESPARSE REQUIRED)
find_package(G2O REQUIRED)
# Eigen3 FindScript Backward compatibility (ubuntu saucy)
# Since FindEigen.cmake is deprecated starting from jade.
if (EXISTS "FindEigen3.cmake")
find_package(Eigen3 REQUIRED)
set(Eigen_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})
elseif (EXISTS "FindEigen.cmake")
find_package(Eigen REQUIRED)
elseif (EXISTS "FindEigen.cmake")
message(WARNING "No findEigen cmake script found. You must provde one of them,
e.g. by adding it to ${PROJECT_SOURCE_DIR}/cmake_modules.")
endif (EXISTS "FindEigen3.cmake")
set(EXTERNAL_INCLUDE_DIRS ${Eigen_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ${G2O_INCLUDE_DIR})
set(EXTERNAL_LIBS ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES})
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
## C++11 support
## Unfortunately, the 3d-party dependency libg2o requires c++11 starting from ROS Jade.
## Even if the ROS Jade specifications do not want c++11-only packages,
## we cannot compile without c++11 enabled. Another option would be to downgrade
## libg2o third-party package.
## By now, if you do not want c++11, please refer to the ros indigo version.
IF(NOT MSVC)
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support which is required
by linked third party packages starting from ROS Jade. Ignore this message for ROS Indigo.")
endif()
endif()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
TrajectoryPointMsg.msg
TrajectoryMsg.msg
FeedbackMsg.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
geometry_msgs std_msgs costmap_converter
)
#add dynamic reconfigure api
#find_package(catkin REQUIRED dynamic_reconfigure)
generate_dynamic_reconfigure_options(
cfg/TebLocalPlannerReconfigure.cfg
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include ${EXTERNAL_INCLUDE_DIRS}
LIBRARIES teb_local_planner ${EXTERNAL_LIBS}
CATKIN_DEPENDS
base_local_planner
costmap_2d
costmap_converter
dynamic_reconfigure
geometry_msgs
interactive_markers
message_runtime
nav_core
nav_msgs
pluginlib
roscpp
mbf_costmap_core
std_msgs
tf2
tf2_ros
visualization_msgs
DEPENDS SUITESPARSE G2O
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include)
include_directories(
${EXTERNAL_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
## Build the teb_local_planner library
add_library(teb_local_planner
src/timed_elastic_band.cpp
src/optimal_planner.cpp
src/obstacles.cpp
src/visualization.cpp
src/recovery_behaviors.cpp
src/teb_config.cpp
src/homotopy_class_planner.cpp
src/teb_local_planner_ros.cpp
src/graph_search.cpp
)
# Dynamic reconfigure: make sure configure headers are built before any node using them
add_dependencies(teb_local_planner ${PROJECT_NAME}_gencfg)
# Generate messages before compiling the lib
add_dependencies(teb_local_planner ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(teb_local_planner
${EXTERNAL_LIBS}
${catkin_LIBRARIES}
)
## Build additional executables
add_executable(test_optim_node src/test_optim_node.cpp)
target_link_libraries(test_optim_node
teb_local_planner
${EXTERNAL_LIBS}
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
install(PROGRAMS
scripts/cmd_vel_to_ackermann_drive.py
scripts/export_to_mat.py
scripts/export_to_svg.py
scripts/publish_dynamic_obstacle.py
scripts/publish_test_obstacles.py
scripts/publish_viapoints.py
scripts/visualize_velocity_profile.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark executables and/or libraries for installation
install(TARGETS teb_local_planner
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(TARGETS test_optim_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
teb_local_planner_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY
launch cfg scripts
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
PATTERN ".svn" EXCLUDE
)
#############
## Testing ##
#############
if (CATKIN_ENABLE_TESTING)
catkin_add_gtest(test_teb_basics test/teb_basics.cpp)
if(TARGET test_teb_basics)
target_link_libraries(test_teb_basics teb_local_planner)
endif()
endif()
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_teb_local_planner.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

28
navigations/teb_local_planner/LICENSE Executable file
View File

@@ -0,0 +1,28 @@
Copyright (c) 2016, TU Dortmund - Lehrstuhl RST
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of teb_local_planner nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

56
navigations/teb_local_planner/README.md Executable file
View File

@@ -0,0 +1,56 @@
teb_local_planner ROS Package
=============================
The teb_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack.
The underlying method called Timed Elastic Band locally optimizes the robot's trajectory with respect to trajectory execution time,
separation from obstacles and compliance with kinodynamic constraints at runtime.
Refer to http://wiki.ros.org/teb_local_planner for more information and tutorials.
Build status of the *melodic-devel* branch:
- ROS Buildfarm (Melodic): [![Melodic Status](http://build.ros.org/buildStatus/icon?job=Mdev__teb_local_planner__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__teb_local_planner__ubuntu_bionic_amd64/)
## Citing the Software
*Since a lot of time and effort has gone into the development, please cite at least one of the following publications if you are using the planner for your own research:*
- 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.
- 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.
- 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.
- 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.
- 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.
<a href="https://www.buymeacoffee.com/croesmann" target="_blank"><img src="https://cdn.buymeacoffee.com/buttons/lato-black.png" alt="Buy Me A Coffee" height="31px" width="132px" ></a>
## Videos
The left of the following videos presents features of the package and shows examples from simulation and real robot situations.
Some spoken explanations are included in the audio track of the video.
The right one demonstrates features introduced in version 0.2 (supporting car-like robots and costmap conversion). Please watch the left one first.
<a href="http://www.youtube.com/watch?feature=player_embedded&v=e1Bw6JOgHME" target="_blank"><img src="http://img.youtube.com/vi/e1Bw6JOgHME/0.jpg"
alt="teb_local_planner - An Optimal Trajectory Planner for Mobile Robots" width="240" height="180" border="10" /></a>
<a href="http://www.youtube.com/watch?feature=player_embedded&v=o5wnRCzdUMo" target="_blank"><img src="http://img.youtube.com/vi/o5wnRCzdUMo/0.jpg"
alt="teb_local_planner - Car-like Robots and Costmap Conversion" width="240" height="180" border="10" /></a>
## License
The *teb_local_planner* package is licensed under the BSD license.
It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.
Some third-party dependencies are included that are licensed under different terms:
- *Eigen*, MPL2 license, http://eigen.tuxfamily.org
- *libg2o* / *g2o* itself is licensed under BSD, but the enabled *csparse_extension* is licensed under LGPL3+,
https://github.com/RainerKuemmerle/g2o. [*CSparse*](http://www.cise.ufl.edu/research/sparse/CSparse/) is included as part of the *SuiteSparse* collection, http://www.suitesparse.com.
- *Boost*, Boost Software License, http://www.boost.org
All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.
## Requirements
Install dependencies (listed in the *package.xml* and *CMakeLists.txt* file) using *rosdep*:
rosdep install teb_local_planner

447
navigations/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg Executable file
View File

@@ -0,0 +1,447 @@
#!/usr/bin/env python
from dynamic_reconfigure.parameter_generator_catkin import *
#from local_planner_limits import add_generic_localplanner_params
gen = ParameterGenerator()
# This unusual line allows to reuse existing parameter definitions
# that concern all localplanners
#add_generic_localplanner_params(gen)
# For integers and doubles:
# Name Type Reconfiguration level
# Description
# Default Min Max
grp_trajectory = gen.add_group("Trajectory", type="tab")
# Trajectory
grp_trajectory.add("teb_autosize", bool_t, 0,
"Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)",
True)
grp_trajectory.add("dt_ref", double_t, 0,
"Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)",
0.3, 0.01, 1)
grp_trajectory.add("dt_hysteresis", double_t, 0,
"Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref",
0.1, 0.002, 0.5)
grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0,
"Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically",
True)
grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0,
"If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)",
False)
grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0,
"Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]",
3.0, 0, 50.0)
grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0,
"Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)",
1.0, 0.0, 10.0)
grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0,
"Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)",
0.78, 0.0, 4.0)
grp_trajectory.add("feasibility_check_no_poses", int_t, 0,
"Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.",
5, -1, 50)
grp_trajectory.add("feasibility_check_lookahead_distance", double_t, 0,
"Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.",
-1, -1, 20)
grp_trajectory.add("exact_arc_length", bool_t, 0,
"If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.",
False)
grp_trajectory.add("publish_feedback", bool_t, 0,
"Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)",
False)
grp_trajectory.add("control_look_ahead_poses", int_t, 0,
"Index of the pose used to extract the velocity command",
1, 1, 100)
grp_trajectory.add("prevent_look_ahead_poses_near_goal", int_t, 0,
"Prevents control_look_ahead_poses to look within this many poses of the goal in order to prevent overshoot & oscillation when xy_goal_tolerance is very small",
0, 0, 20)
grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0,
"If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.",
0, 0, 1)
# ViaPoints
grp_viapoints = gen.add_group("ViaPoints", type="tab")
grp_viapoints.add("global_plan_viapoint_sep", double_t, 0,
"Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]",
-0.1, -0.1, 5.0)
grp_viapoints.add("via_points_ordered", bool_t, 0,
"If true, the planner adheres to the order of via-points in the storage container",
False)
# Robot
grp_robot = gen.add_group("Robot", type="tab")
grp_robot.add("max_vel_x", double_t, 0,
"Maximum velocity in the x direction of the robot. May be overruled by the max_vel_trans parameter",
0.4, 0.01, 100)
grp_robot.add("max_vel_x_backwards", double_t, 0,
"Maximum translational velocity of the robot for driving backwards",
0.2, 0.01, 100)
grp_robot.add("max_vel_theta", double_t, 0,
"Maximum angular velocity of the robot",
0.3, 0.01, 100)
grp_robot.add("acc_lim_x", double_t, 0,
"Maximum translational acceleration of the robot",
0.5, 0.01, 100)
grp_robot.add("acc_lim_theta", double_t, 0,
"Maximum angular acceleration of the robot",
0.5, 0.01, 100)
grp_robot.add("is_footprint_dynamic", bool_t, 0,
"If true, updated the footprint before checking trajectory feasibility",
False)
grp_robot.add("use_proportional_saturation", bool_t, 0,
"If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually",
False)
grp_robot.add("transform_tolerance", double_t, 0,
"Tolerance when querying the TF Tree for a transformation (seconds)",
0.5, 0.001, 20)
# Robot/Carlike
grp_robot_carlike = grp_robot.add_group("Carlike", type="hide")
grp_robot_carlike.add("min_turning_radius", double_t, 0,
"Minimum turning radius of a carlike robot (diff-drive robot: zero)",
0.0, 0.0, 50.0)
grp_robot_carlike.add("wheelbase", double_t, 0,
"The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!",
1.0, -10.0, 10.0)
grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0,
"Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')",
False)
# Robot/Omni
grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide")
grp_robot_omni.add("max_vel_y", double_t, 0,
"Maximum strafing velocity of the robot (should be zero for non-holonomic robots!). May be overruled by the max_vel_trans parameter",
0.0, 0.0, 100)
grp_robot_omni.add("max_vel_trans", double_t, 0,
"Maximum linear velocity of the robot. Will limit max_vel_x and max_vel_y when their linear combination exceeds its value. When set to default 0.0, it will be set equal to max_vel_x.",
0.0, 0.0, 100)
grp_robot_omni.add("acc_lim_y", double_t, 0,
"Maximum strafing acceleration of the robot",
0.5, 0.01, 100)
# GoalTolerance
grp_goal = gen.add_group("GoalTolerance", type="tab")
grp_goal.add("xy_goal_tolerance", double_t, 0,
"Allowed final euclidean distance to the goal position",
0.2, 0.001, 10)
grp_goal.add("yaw_goal_tolerance", double_t, 0,
"Allowed final orientation error to the goal orientation",
0.1, 0.001, 3.2)
grp_goal.add("free_goal_vel", bool_t, 0,
"Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)",
False)
grp_goal.add("trans_stopped_vel", double_t, 0,
"Below what maximum velocity we consider the robot to be stopped in translation",
0.1, 0.0)
grp_goal.add("theta_stopped_vel", double_t, 0,
"Below what maximum rotation velocity we consider the robot to be stopped in rotation",
0.1, 0.0)
# Obstacles
grp_obstacles = gen.add_group("Obstacles", type="tab")
grp_obstacles.add("min_obstacle_dist", double_t, 0,
"Minimum desired separation from obstacles",
0.5, 0, 10)
grp_obstacles.add("inflation_dist", double_t, 0,
"Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
0.6, 0, 15)
grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0,
"Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
0.6, 0, 15)
grp_obstacles.add("include_dynamic_obstacles", bool_t, 0,
"Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.",
False)
grp_obstacles.add("include_costmap_obstacles", bool_t, 0,
"Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)",
True)
grp_obstacles.add("legacy_obstacle_association", bool_t, 0,
"If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).",
False)
grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0,
"The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.",
1.5, 0.0, 100.0)
grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0,
"See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.",
5.0, 1.0, 100.0)
grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0,
"Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)",
1.5, 0.0, 20.0)
grp_obstacles.add("obstacle_poses_affected", int_t, 0,
"The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well",
30, 0, 200)
# Obstacle - Velocity ratio parameters
grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles")
grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0,
"Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles",
1, 0, 1)
grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0,
"Distance to a static obstacle for which the velocity should be lower",
0, 0, 10)
grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0,
"Distance to a static obstacle for which the velocity should be higher",
0.5, 0, 10)
# Optimization
grp_optimization = gen.add_group("Optimization", type="tab")
grp_optimization.add("no_inner_iterations", int_t, 0,
"Number of solver iterations called in each outerloop iteration",
5, 1, 100)
grp_optimization.add("no_outer_iterations", int_t, 0,
"Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations",
4, 1, 100)
grp_optimization.add("optimization_activate", bool_t, 0,
"Activate the optimization",
True)
grp_optimization.add("optimization_verbose", bool_t, 0,
"Print verbose information",
False)
grp_optimization.add("penalty_epsilon", double_t, 0,
"Add a small safty margin to penalty functions for hard-constraint approximations",
0.05, 0, 1.0)
grp_optimization.add("weight_max_vel_x", double_t, 0,
"Optimization weight for satisfying the maximum allowed translational velocity",
2, 0, 1000)
grp_optimization.add("weight_max_vel_y", double_t, 0,
"Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)",
2, 0, 1000)
grp_optimization.add("weight_max_vel_theta", double_t, 0,
"Optimization weight for satisfying the maximum allowed angular velocity",
1, 0, 1000)
grp_optimization.add("weight_acc_lim_x", double_t, 0,
"Optimization weight for satisfying the maximum allowed translational acceleration",
1, 0, 1000)
grp_optimization.add("weight_acc_lim_y", double_t, 0,
"Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)",
1, 0, 1000)
grp_optimization.add("weight_acc_lim_theta", double_t, 0,
"Optimization weight for satisfying the maximum allowed angular acceleration",
1, 0, 1000)
grp_optimization.add("weight_kinematics_nh", double_t, 0,
"Optimization weight for satisfying the non-holonomic kinematics",
1000 , 0, 10000)
grp_optimization.add("weight_kinematics_forward_drive", double_t, 0,
"Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)",
1, 0, 10000)
grp_optimization.add("weight_kinematics_turning_radius", double_t, 0,
"Optimization weight for enforcing a minimum turning radius (carlike robots)",
1, 0, 1000)
grp_optimization.add("weight_optimaltime", double_t, 0,
"Optimization weight for contracting the trajectory w.r.t. transition time",
1, 0, 1000)
grp_optimization.add("weight_shortest_path", double_t, 0,
"Optimization weight for contracting the trajectory w.r.t. path length",
0, 0, 100)
grp_optimization.add("weight_obstacle", double_t, 0,
"Optimization weight for satisfying a minimum seperation from obstacles",
50, 0, 1000)
grp_optimization.add("weight_inflation", double_t, 0,
"Optimization weight for the inflation penalty (should be small)",
0.1, 0, 10)
grp_optimization.add("weight_dynamic_obstacle", double_t, 0,
"Optimization weight for satisfying a minimum seperation from dynamic obstacles",
50, 0, 1000)
grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0,
"Optimization weight for the inflation penalty of dynamic obstacles (should be small)",
0.1, 0, 10)
grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0,
"Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle",
0, 0, 1000)
grp_optimization.add("weight_viapoint", double_t, 0,
"Optimization weight for minimizing the distance to via-points",
1, 0, 1000)
grp_optimization.add("weight_adapt_factor", double_t, 0,
"Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.",
2, 1, 100)
grp_optimization.add("obstacle_cost_exponent", double_t, 0,
"Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)",
1, 0.01, 100)
# Homotopy Class Planner
grp_hcp = gen.add_group("HCPlanning", type="tab")
grp_hcp.add("enable_multithreading", bool_t, 0,
"Activate multiple threading for planning multiple trajectories in parallel",
True)
grp_hcp.add("max_number_classes", int_t, 0,
"Specify the maximum number of allowed alternative homotopy classes (limits computational effort)",
5, 1, 100)
grp_hcp.add("max_number_plans_in_current_class", int_t, 0,
"Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes",
1, 1, 10)
grp_hcp.add("selection_cost_hysteresis", double_t, 0,
"Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)",
1.0, 0, 2)
grp_hcp.add("selection_prefer_initial_plan", double_t, 0,
"Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)",
0.95, 0, 1)
grp_hcp.add("selection_obst_cost_scale", double_t, 0,
"Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)",
2.0, 0, 1000)
grp_hcp.add("selection_viapoint_cost_scale", double_t, 0,
"Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)",
1.0, 0, 100)
grp_hcp.add("selection_alternative_time_cost", bool_t, 0,
"If true, time cost is replaced by the total transition time.",
False)
grp_hcp.add("selection_dropping_probability", double_t, 0,
"At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.",
0.0, 0.0, 1.0)
grp_hcp.add("switching_blocking_period", double_t, 0,
"Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed",
0.0, 0.0, 60)
grp_hcp.add("roadmap_graph_no_samples", int_t, 0,
"Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off",
15, 1, 100)
grp_hcp.add("roadmap_graph_area_width", double_t, 0,
"Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)",
5, 0.1, 20)
grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0,
"The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)",
1.0, 0.5, 2)
grp_hcp.add("h_signature_prescaler", double_t, 0,
"Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1)",
1, 0.2, 1)
grp_hcp.add("h_signature_threshold", double_t, 0,
"Two h-signuteres are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold",
0.1, 0, 1)
grp_hcp.add("obstacle_heading_threshold", double_t, 0,
"Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)",
0.45, 0, 1)
grp_hcp.add("viapoints_all_candidates", bool_t, 0,
"If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan is attached (no effect in test_optim_node).",
True)
grp_hcp.add("visualize_hc_graph", bool_t, 0,
"Visualize the graph that is created for exploring new homotopy classes",
False)
# Recovery
grp_recovery = gen.add_group("Recovery", type="tab")
grp_recovery.add("shrink_horizon_backup", bool_t, 0,
"Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.",
True)
grp_recovery.add("oscillation_recovery", bool_t, 0,
"Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).",
True)
grp_recovery_divergence = grp_recovery.add_group("Divergence Detection", type="hide")
grp_recovery_divergence.add(
"divergence_detection_enable",
bool_t,
0,
"True to enable divergence detection.",
False
)
grp_recovery_divergence.add(
"divergence_detection_max_chi_squared",
double_t,
0,
"Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.",
10,
0,
100
)
exit(gen.generate("teb_local_planner", "teb_local_planner", "TebLocalPlannerReconfigure"))

183
navigations/teb_local_planner/cfg/rviz_test_optim.rviz Executable file
View File

@@ -0,0 +1,183 @@
Panels:
- Class: rviz/Displays
Help Height: 81
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
Splitter Ratio: 0.5
Tree Height: 562
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.100000001
Cell Size: 0.100000001
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Fine Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 100
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /test_optim_node/local_plan
Unreliable: false
Value: true
- Alpha: 1
Arrow Length: 0.300000012
Axes Length: 0.300000012
Axes Radius: 0.00999999978
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: true
Head Length: 0.0700000003
Head Radius: 0.0299999993
Name: PoseArray
Shaft Length: 0.230000004
Shaft Radius: 0.00999999978
Shape: Arrow (Flat)
Topic: /test_optim_node/teb_poses
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /test_optim_node/teb_markers
Name: Marker
Namespaces:
PointObstacles: true
Queue Size: 100
Value: true
- Class: rviz/InteractiveMarkers
Enable Transparency: true
Enabled: true
Name: InteractiveMarkers
Show Axes: false
Show Descriptions: true
Show Visual Aids: false
Update Topic: /marker_obstacles/update
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 7.77247
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.56979632
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.71043873
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1354
X: 137
Y: 50

97
navigations/teb_local_planner/cmake_modules/FindG2O.cmake Executable file
View File

@@ -0,0 +1,97 @@
# Locate the g2o libraries
# A general framework for graph optimization.
#
# This module defines
# G2O_FOUND, if false, do not try to link against g2o
# G2O_LIBRARIES, path to the libg2o
# G2O_INCLUDE_DIR, where to find the g2o header files
#
# Niko Suenderhauf <niko@etit.tu-chemnitz.de>
# Adapted by Felix Endres <endres@informatik.uni-freiburg.de>
IF(UNIX)
#IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
# in cache already
# SET(G2O_FIND_QUIETLY TRUE)
#ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
MESSAGE(STATUS "Searching for g2o ...")
FIND_PATH(G2O_INCLUDE_DIR
NAMES core math_groups types
PATHS /usr/local /usr
PATH_SUFFIXES include/g2o include)
IF (G2O_INCLUDE_DIR)
MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}")
ENDIF (G2O_INCLUDE_DIR)
FIND_LIBRARY(G2O_CORE_LIB
NAMES g2o_core g2o_core_rd
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
PATH_SUFFIXES lib)
FIND_LIBRARY(G2O_STUFF_LIB
NAMES g2o_stuff g2o_stuff_rd
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
PATH_SUFFIXES lib)
FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB
NAMES g2o_types_slam2d g2o_types_slam2d_rd
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
PATH_SUFFIXES lib)
FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB
NAMES g2o_types_slam3d g2o_types_slam3d_rd
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
PATH_SUFFIXES lib)
FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB
NAMES g2o_solver_cholmod g2o_solver_cholmod_rd
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
PATH_SUFFIXES lib)
FIND_LIBRARY(G2O_SOLVER_PCG_LIB
NAMES g2o_solver_pcg g2o_solver_pcg_rd
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
PATH_SUFFIXES lib)
FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB
NAMES g2o_solver_csparse g2o_solver_csparse_rd
PATHS /usr/local /usr
PATH_SUFFIXES lib)
FIND_LIBRARY(G2O_INCREMENTAL_LIB
NAMES g2o_incremental g2o_incremental_rd
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
PATH_SUFFIXES lib)
FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB
NAMES g2o_csparse_extension g2o_csparse_extension_rd
PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
PATH_SUFFIXES lib)
SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB}
${G2O_CORE_LIB}
${G2O_STUFF_LIB}
${G2O_TYPES_SLAM2D_LIB}
${G2O_TYPES_SLAM3D_LIB}
${G2O_SOLVER_CHOLMOD_LIB}
${G2O_SOLVER_PCG_LIB}
${G2O_SOLVER_CSPARSE_LIB}
${G2O_INCREMENTAL_LIB}
)
IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
SET(G2O_FOUND "YES")
IF(NOT G2O_FIND_QUIETLY)
MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}")
ENDIF(NOT G2O_FIND_QUIETLY)
ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
IF(NOT G2O_LIBRARIES)
IF(G2O_FIND_REQUIRED)
message(FATAL_ERROR "Could not find libg2o!")
ENDIF(G2O_FIND_REQUIRED)
ENDIF(NOT G2O_LIBRARIES)
IF(NOT G2O_INCLUDE_DIR)
IF(G2O_FIND_REQUIRED)
message(FATAL_ERROR "Could not find g2o include directory!")
ENDIF(G2O_FIND_REQUIRED)
ENDIF(NOT G2O_INCLUDE_DIR)
ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
ENDIF(UNIX)

133
navigations/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake Executable file
View File

@@ -0,0 +1,133 @@
# - Try to find SUITESPARSE
# Once done this will define
#
# SUITESPARSE_FOUND - system has SUITESPARSE
# SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory
# SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE
# SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package)
# SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package)
# SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs
# SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs
# SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly
IF (SUITESPARSE_INCLUDE_DIRS)
# Already in cache, be silent
SET(SUITESPARSE_FIND_QUIETLY TRUE)
ENDIF (SUITESPARSE_INCLUDE_DIRS)
if( WIN32 )
# Find cholmod part of the suitesparse library collection
FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
PATHS "C:\\libs\\win32\\SuiteSparse\\Include" )
# Add cholmod include directory to collection include directories
IF ( CHOLMOD_INCLUDE_DIR )
list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
ENDIF( CHOLMOD_INCLUDE_DIR )
# find path suitesparse library
FIND_PATH( SUITESPARSE_LIBRARY_DIRS
amd.lib
PATHS "C:\\libs\\win32\\SuiteSparse\\libs" )
# if we found the library, add it to the defined libraries
IF ( SUITESPARSE_LIBRARY_DIRS )
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 )
ENDIF( SUITESPARSE_LIBRARY_DIRS )
else( WIN32 )
IF(APPLE)
FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
PATHS /opt/local/include/ufsparse
/usr/local/include )
FIND_PATH( SUITESPARSE_LIBRARY_DIR
NAMES libcholmod.a
PATHS /opt/local/lib
/usr/local/lib )
ELSE(APPLE)
FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
PATHS /usr/local/include
/usr/include
/usr/include/suitesparse/
${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod
PATH_SUFFIXES cholmod/ CHOLMOD/ )
FIND_PATH( SUITESPARSE_LIBRARY_DIR
NAMES libcholmod.so libcholmod.a
PATHS /usr/lib
/usr/lib64
/usr/lib/x86_64-linux-gnu
/usr/lib/i386-linux-gnu
/usr/local/lib
/usr/lib/arm-linux-gnueabihf/
/usr/lib/aarch64-linux-gnu/
/usr/lib/arm-linux-gnueabi/
/usr/lib/arm-linux-gnu)
ENDIF(APPLE)
# Add cholmod include directory to collection include directories
IF ( CHOLMOD_INCLUDE_DIR )
list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
ENDIF( CHOLMOD_INCLUDE_DIR )
# if we found the library, add it to the defined libraries
IF ( SUITESPARSE_LIBRARY_DIR )
list ( APPEND SUITESPARSE_LIBRARIES amd)
list ( APPEND SUITESPARSE_LIBRARIES btf)
list ( APPEND SUITESPARSE_LIBRARIES camd)
list ( APPEND SUITESPARSE_LIBRARIES ccolamd)
list ( APPEND SUITESPARSE_LIBRARIES cholmod)
list ( APPEND SUITESPARSE_LIBRARIES colamd)
# list ( APPEND SUITESPARSE_LIBRARIES csparse)
list ( APPEND SUITESPARSE_LIBRARIES cxsparse)
list ( APPEND SUITESPARSE_LIBRARIES klu)
# list ( APPEND SUITESPARSE_LIBRARIES spqr)
list ( APPEND SUITESPARSE_LIBRARIES umfpack)
IF (APPLE)
list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig)
ENDIF (APPLE)
# Metis and spqr are optional
FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY
NAMES metis
PATHS ${SUITESPARSE_LIBRARY_DIR} )
IF (SUITESPARSE_METIS_LIBRARY)
list ( APPEND SUITESPARSE_LIBRARIES metis)
ENDIF(SUITESPARSE_METIS_LIBRARY)
if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp")
SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid")
else()
SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid")
endif()
if(SUITESPARSE_SPQR_VALID)
FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY
NAMES spqr
PATHS ${SUITESPARSE_LIBRARY_DIR} )
IF (SUITESPARSE_SPQR_LIBRARY)
list ( APPEND SUITESPARSE_LIBRARIES spqr)
ENDIF (SUITESPARSE_SPQR_LIBRARY)
endif()
ENDIF( SUITESPARSE_LIBRARY_DIR )
endif( WIN32 )
IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
IF(WIN32)
list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig )
ENDIF(WIN32)
SET(SUITESPARSE_FOUND TRUE)
MESSAGE(STATUS "Found SuiteSparse")
ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
SET( SUITESPARSE_FOUND FALSE )
MESSAGE(FATAL_ERROR "Unable to find SuiteSparse")
ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)

464
navigations/teb_local_planner/include/teb_local_planner/distance_calculations.h Executable file
View File

@@ -0,0 +1,464 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef DISTANCE_CALCULATIONS_H
#define DISTANCE_CALCULATIONS_H
#include <Eigen/Core>
#include <teb_local_planner/misc.h>
namespace teb_local_planner
{
//! Abbrev. for a container storing 2d points
typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > Point2dContainer;
/**
* @brief Helper function to obtain the closest point on a line segment w.r.t. a reference point
* @param point 2D point
* @param line_start 2D point representing the start of the line segment
* @param line_end 2D point representing the end of the line segment
* @return Closest point on the line segment
*/
inline Eigen::Vector2d closest_point_on_line_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
{
Eigen::Vector2d diff = line_end - line_start;
double sq_norm = diff.squaredNorm();
if (sq_norm == 0)
return line_start;
double u = ((point.x() - line_start.x()) * diff.x() + (point.y() - line_start.y())*diff.y()) / sq_norm;
if (u <= 0) return line_start;
else if (u >= 1) return line_end;
return line_start + u*diff;
}
/**
* @brief Helper function to calculate the distance between a line segment and a point
* @param point 2D point
* @param line_start 2D point representing the start of the line segment
* @param line_end 2D point representing the end of the line segment
* @return minimum distance to a given line segment
*/
inline double distance_point_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
{
return (point - closest_point_on_line_segment_2d(point, line_start, line_end)).norm();
}
/**
* @brief Helper function to check whether two line segments intersects
* @param line1_start 2D point representing the start of the first line segment
* @param line1_end 2D point representing the end of the first line segment
* @param line2_start 2D point representing the start of the second line segment
* @param line2_end 2D point representing the end of the second line segment
* @param[out] intersection [optional] Write intersection point to destination (the value is only written, if both lines intersect, e.g. if the function returns \c true)
* @return \c true if both line segments intersect
*/
inline bool check_line_segments_intersection_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end,
const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end, Eigen::Vector2d* intersection = NULL)
{
// http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
double s_numer, t_numer, denom, t;
Eigen::Vector2d line1 = line1_end - line1_start;
Eigen::Vector2d line2 = line2_end - line2_start;
denom = line1.x() * line2.y() - line2.x() * line1.y();
if (denom == 0) return false; // Collinear
bool denomPositive = denom > 0;
Eigen::Vector2d aux = line1_start - line2_start;
s_numer = line1.x() * aux.y() - line1.y() * aux.x();
if ((s_numer < 0) == denomPositive) return false; // No collision
t_numer = line2.x() * aux.y() - line2.y() * aux.x();
if ((t_numer < 0) == denomPositive) return false; // No collision
if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) return false; // No collision
// Otherwise collision detected
t = t_numer / denom;
if (intersection)
{
*intersection = line1_start + t * line1;
}
return true;
}
/**
* @brief Helper function to calculate the smallest distance between two line segments
* @param line1_start 2D point representing the start of the first line segment
* @param line1_end 2D point representing the end of the first line segment
* @param line2_start 2D point representing the start of the second line segment
* @param line2_end 2D point representing the end of the second line segment
* @return smallest distance between both segments
*/
inline double distance_segment_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end,
const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end)
{
// TODO more efficient implementation
// check if segments intersect
if (check_line_segments_intersection_2d(line1_start, line1_end, line2_start, line2_end))
return 0;
// check all 4 combinations
std::array<double,4> distances;
distances[0] = distance_point_to_segment_2d(line1_start, line2_start, line2_end);
distances[1] = distance_point_to_segment_2d(line1_end, line2_start, line2_end);
distances[2] = distance_point_to_segment_2d(line2_start, line1_start, line1_end);
distances[3] = distance_point_to_segment_2d(line2_end, line1_start, line1_end);
return *std::min_element(distances.begin(), distances.end());
}
/**
* @brief Helper function to calculate the smallest distance between a point and a closed polygon
* @param point 2D point
* @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
* @return smallest distance between point and polygon
*/
inline double distance_point_to_polygon_2d(const Eigen::Vector2d& point, const Point2dContainer& vertices)
{
double dist = HUGE_VAL;
// the polygon is a point
if (vertices.size() == 1)
{
return (point - vertices.front()).norm();
}
// check each polygon edge
for (int i=0; i<(int)vertices.size()-1; ++i)
{
double new_dist = distance_point_to_segment_2d(point, vertices.at(i), vertices.at(i+1));
// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1));
if (new_dist < dist)
dist = new_dist;
}
if (vertices.size()>2) // if not a line close polygon
{
double new_dist = distance_point_to_segment_2d(point, vertices.back(), vertices.front()); // check last edge
if (new_dist < dist)
return new_dist;
}
return dist;
}
/**
* @brief Helper function to calculate the smallest distance between a line segment and a closed polygon
* @param line_start 2D point representing the start of the line segment
* @param line_end 2D point representing the end of the line segment
* @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
* @return smallest distance between point and polygon
*/
inline double distance_segment_to_polygon_2d(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, const Point2dContainer& vertices)
{
double dist = HUGE_VAL;
// the polygon is a point
if (vertices.size() == 1)
{
return distance_point_to_segment_2d(vertices.front(), line_start, line_end);
}
// check each polygon edge
for (int i=0; i<(int)vertices.size()-1; ++i)
{
double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.at(i), vertices.at(i+1));
// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1));
if (new_dist < dist)
dist = new_dist;
}
if (vertices.size()>2) // if not a line close polygon
{
double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.back(), vertices.front()); // check last edge
if (new_dist < dist)
return new_dist;
}
return dist;
}
/**
* @brief Helper function to calculate the smallest distance between two closed polygons
* @param vertices1 Vertices describing the first closed polygon (the first vertex is not repeated at the end)
* @param vertices2 Vertices describing the second closed polygon (the first vertex is not repeated at the end)
* @return smallest distance between point and polygon
*/
inline double distance_polygon_to_polygon_2d(const Point2dContainer& vertices1, const Point2dContainer& vertices2)
{
double dist = HUGE_VAL;
// the polygon1 is a point
if (vertices1.size() == 1)
{
return distance_point_to_polygon_2d(vertices1.front(), vertices2);
}
// check each edge of polygon1
for (int i=0; i<(int)vertices1.size()-1; ++i)
{
double new_dist = distance_segment_to_polygon_2d(vertices1[i], vertices1[i+1], vertices2);
if (new_dist < dist)
dist = new_dist;
}
if (vertices1.size()>2) // if not a line close polygon1
{
double new_dist = distance_segment_to_polygon_2d(vertices1.back(), vertices1.front(), vertices2); // check last edge
if (new_dist < dist)
return new_dist;
}
return dist;
}
// Further distance calculations:
// The Distance Calculations are mainly copied from http://geomalgorithms.com/a07-_distance.html
// Copyright 2001 softSurfer, 2012 Dan Sunday
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
// SoftSurfer makes no warranty for this code, and cannot be held
// liable for any real or imagined damage resulting from its use.
// Users of this code must verify correctness for their application.
inline double calc_distance_line_to_line_3d(const Eigen::Ref<const Eigen::Vector3d>& x1, Eigen::Ref<const Eigen::Vector3d>& u,
const Eigen::Ref<const Eigen::Vector3d>& x2, Eigen::Ref<const Eigen::Vector3d>& v)
{
Eigen::Vector3d w = x2 - x1;
double a = u.squaredNorm(); // dot(u,u) always >= 0
double b = u.dot(v);
double c = v.squaredNorm(); // dot(v,v) always >= 0
double d = u.dot(w);
double e = v.dot(w);
double D = a*c - b*b; // always >= 0
double sc, tc;
// compute the line parameters of the two closest points
if (D < SMALL_NUM) { // the lines are almost parallel
sc = 0.0;
tc = (b>c ? d/b : e/c); // use the largest denominator
}
else {
sc = (b*e - c*d) / D;
tc = (a*e - b*d) / D;
}
// get the difference of the two closest points
Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = L1(sc) - L2(tc)
return dP.norm(); // return the closest distance
}
inline double calc_distance_segment_to_segment3D(const Eigen::Ref<const Eigen::Vector3d>& line1_start, Eigen::Ref<const Eigen::Vector3d>& line1_end,
const Eigen::Ref<const Eigen::Vector3d>& line2_start, Eigen::Ref<const Eigen::Vector3d>& line2_end)
{
Eigen::Vector3d u = line1_end - line1_start;
Eigen::Vector3d v = line2_end - line2_start;
Eigen::Vector3d w = line2_start - line1_start;
double a = u.squaredNorm(); // dot(u,u) always >= 0
double b = u.dot(v);
double c = v.squaredNorm(); // dot(v,v) always >= 0
double d = u.dot(w);
double e = v.dot(w);
double D = a*c - b*b; // always >= 0
double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < SMALL_NUM)
{ // the lines are almost parallel
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else
{ // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0)
{ // sc < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD)
{ // sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0)
{ // tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0)
sN = 0.0;
else if (-d > a)
sN = sD;
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD)
{ // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else
{
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);
// get the difference of the two closest points
Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
return dP.norm(); // return the closest distance
}
template <typename VectorType>
double calc_closest_point_to_approach_time(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2)
{
VectorType dv = vel1 - vel2;
double dv2 = dv.squaredNorm(); // dot(v,v)
if (dv2 < SMALL_NUM) // the tracks are almost parallel
return 0.0; // any time is ok. Use time 0.
VectorType w0 = x1 - x2;
double cpatime = -w0.dot(dv) / dv2;
return cpatime; // time of CPA
}
template <typename VectorType>
double calc_closest_point_to_approach_distance(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2, double bound_cpa_time = 0)
{
double ctime = calc_closest_point_to_approach_time<VectorType>(x1, vel1, x2, vel2);
if (bound_cpa_time!=0 && ctime > bound_cpa_time) ctime = bound_cpa_time;
VectorType P1 = x1 + (ctime * vel1);
VectorType P2 = x2 + (ctime * vel2);
return (P2-P1).norm(); // distance at CPA
}
// dist_Point_to_Line(): get the distance of a point to a line
// Input: a Point P and a Line L (in any dimension)
// Return: the shortest distance from P to L
template <typename VectorType>
double calc_distance_point_to_line( const VectorType& point, const VectorType& line_base, const VectorType& line_dir)
{
VectorType w = point - line_base;
double c1 = w.dot(line_dir);
double c2 = line_dir.dot(line_dir);
double b = c1 / c2;
VectorType Pb = line_base + b * line_dir;
return (point-Pb).norm();
}
//===================================================================
// dist_Point_to_Segment(): get the distance of a point to a segment
// Input: a Point P and a Segment S (in any dimension)
// Return: the shortest distance from P to S
template <typename VectorType>
double calc_distance_point_to_segment( const VectorType& point, const VectorType& line_start, const VectorType& line_end)
{
VectorType v = line_end - line_start;
VectorType w = point - line_start;
double c1 = w.dot(v);
if ( c1 <= 0 )
return w.norm();
double c2 = v.dot(v);
if ( c2 <= c1 )
return (point-line_end).norm();
double b = c1 / c2;
VectorType Pb = line_start + b * v;
return (point-Pb).norm();
}
} // namespace teb_local_planner
#endif /* DISTANCE_CALCULATIONS_H */

103
navigations/teb_local_planner/include/teb_local_planner/equivalence_relations.h Executable file
View File

@@ -0,0 +1,103 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef EQUIVALENCE_RELATIONS_H_
#define EQUIVALENCE_RELATIONS_H_
#include <boost/shared_ptr.hpp>
namespace teb_local_planner
{
/**
* @class EquivalenceClass
* @brief Abstract class that defines an interface for computing and comparing equivalence classes
*
* Equivalence relations are utilized in order to test if two trajectories are belonging to the same
* equivalence class w.r.t. the current obstacle configurations. A common equivalence relation is
* the concept of homotopy classes. All trajectories belonging to the same homotopy class
* can CONTINUOUSLY be deformed into each other without intersecting any obstacle. Hence they likely
* share the same local minimum after invoking (local) trajectory optimization. A weaker equivalence relation
* is defined by the concept of homology classes (e.g. refer to HSignature).
*
* Each EquivalenceClass object (or subclass) stores a candidate value which might be compared to another EquivalenceClass object.
*
* @remarks Currently, the computeEquivalenceClass method is not available in the generic interface EquivalenceClass.
* Call the "compute"-methods directly on the subclass.
*/
class EquivalenceClass
{
public:
/**
* @brief Default constructor
*/
EquivalenceClass() {}
/**
* @brief virtual destructor
*/
virtual ~EquivalenceClass() {}
/**
* @brief Check if two candidate classes are equivalent
* @param other The other equivalence class to test with
*/
virtual bool isEqual(const EquivalenceClass& other) const = 0;
/**
* @brief Check if the equivalence value is detected correctly
* @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
*/
virtual bool isValid() const = 0;
/**
* @brief Check if the trajectory is non-looping around an obstacle
* @return Returns false, if the trajectory loops around an obstacle
*/
virtual bool isReasonable() const = 0;
};
using EquivalenceClassPtr = boost::shared_ptr<EquivalenceClass>;
} // namespace teb_local_planner
#endif /* EQUIVALENCE_RELATIONS_H_ */

278
navigations/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h Executable file
View File

@@ -0,0 +1,278 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef _BASE_TEB_EDGES_H_
#define _BASE_TEB_EDGES_H_
#include <teb_local_planner/teb_config.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_multi_edge.h>
namespace teb_local_planner
{
/**
* @class BaseTebUnaryEdge
* @brief Base edge connecting a single vertex in the TEB optimization problem
*
* This edge defines a base edge type for the TEB optimization problem.
* It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
* The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
* Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
* @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
*/
template <int D, typename E, typename VertexXi>
class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
{
public:
using typename g2o::BaseUnaryEdge<D, E, VertexXi>::ErrorVector;
using g2o::BaseUnaryEdge<D, E, VertexXi>::computeError;
/**
* @brief Compute and return error / cost value.
*
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
*/
ErrorVector& getError()
{
computeError();
return _error;
}
/**
* @brief Read values from input stream
*/
virtual bool read(std::istream& is)
{
// TODO generic read
return true;
}
/**
* @brief Write values to an output stream
*/
virtual bool write(std::ostream& os) const
{
// TODO generic write
return os.good();
}
/**
* @brief Assign the TebConfig class for parameters.
* @param cfg TebConfig class
*/
void setTebConfig(const TebConfig& cfg)
{
cfg_ = &cfg;
}
protected:
using g2o::BaseUnaryEdge<D, E, VertexXi>::_error;
using g2o::BaseUnaryEdge<D, E, VertexXi>::_vertices;
const TebConfig* cfg_; //!< Store TebConfig class for parameters
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class BaseTebBinaryEdge
* @brief Base edge connecting two vertices in the TEB optimization problem
*
* This edge defines a base edge type for the TEB optimization problem.
* It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
* The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
* Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
* @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
*/
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
{
public:
using typename g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::ErrorVector;
using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::computeError;
/**
* @brief Compute and return error / cost value.
*
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
*/
ErrorVector& getError()
{
computeError();
return _error;
}
/**
* @brief Read values from input stream
*/
virtual bool read(std::istream& is)
{
// TODO generic read
return true;
}
/**
* @brief Write values to an output stream
*/
virtual bool write(std::ostream& os) const
{
// TODO generic write
return os.good();
}
/**
* @brief Assign the TebConfig class for parameters.
* @param cfg TebConfig class
*/
void setTebConfig(const TebConfig& cfg)
{
cfg_ = &cfg;
}
protected:
using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_error;
using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_vertices;
const TebConfig* cfg_; //!< Store TebConfig class for parameters
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class BaseTebMultiEdge
* @brief Base edge connecting multiple vertices in the TEB optimization problem
*
* This edge defines a base edge type for the TEB optimization problem.
* It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
* The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
* Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
* @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
*/
template <int D, typename E>
class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
{
public:
using typename g2o::BaseMultiEdge<D, E>::ErrorVector;
using g2o::BaseMultiEdge<D, E>::computeError;
// Overwrites resize() from the parent class
virtual void resize(size_t size)
{
g2o::BaseMultiEdge<D, E>::resize(size);
for(std::size_t i=0; i<_vertices.size(); ++i)
_vertices[i] = NULL;
}
/**
* @brief Compute and return error / cost value.
*
* This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
* @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
*/
ErrorVector& getError()
{
computeError();
return _error;
}
/**
* @brief Read values from input stream
*/
virtual bool read(std::istream& is)
{
// TODO generic read
return true;
}
/**
* @brief Write values to an output stream
*/
virtual bool write(std::ostream& os) const
{
// TODO generic write
return os.good();
}
/**
* @brief Assign the TebConfig class for parameters.
* @param cfg TebConfig class
*/
void setTebConfig(const TebConfig& cfg)
{
cfg_ = &cfg;
}
protected:
using g2o::BaseMultiEdge<D, E>::_error;
using g2o::BaseMultiEdge<D, E>::_vertices;
const TebConfig* cfg_; //!< Store TebConfig class for parameters
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace
#endif

734
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h Executable file
View File

@@ -0,0 +1,734 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef EDGE_ACCELERATION_H_
#define EDGE_ACCELERATION_H_
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/vertex_timediff.h>
#include <teb_local_planner/g2o_types/penalties.h>
#include <teb_local_planner/teb_config.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <geometry_msgs/Twist.h>
namespace teb_local_planner
{
/**
* @class EdgeAcceleration
* @brief Edge defining the cost function for limiting the translational and rotational acceleration.
*
* The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes:
* \f$ \min \textrm{penaltyInterval}( [a, omegadot } ]^T ) \cdot weight \f$. \n
* \e a is calculated using the difference quotient (twice) and the position parts of all three poses \n
* \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
* \e weight can be set using setInformation() \n
* \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
* The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
* the second one the rotational acceleration.
* @see TebOptimalPlanner::AddEdgesAcceleration
* @see EdgeAccelerationStart
* @see EdgeAccelerationGoal
* @remarks Do not forget to call setTebConfig()
* @remarks Refer to EdgeAccelerationStart() and EdgeAccelerationGoal() for defining boundary values!
*/
class EdgeAcceleration : public BaseTebMultiEdge<2, double>
{
public:
/**
* @brief Construct edge.
*/
EdgeAcceleration()
{
this->resize(5);
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
// VELOCITY & ACCELERATION
const Eigen::Vector2d diff1 = pose2->position() - pose1->position();
const Eigen::Vector2d diff2 = pose3->position() - pose2->position();
double dist1 = diff1.norm();
double dist2 = diff2.norm();
const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta());
const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta());
if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation
{
if (angle_diff1 != 0)
{
const double radius = dist1/(2*sin(angle_diff1/2));
dist1 = fabs( angle_diff1 * radius ); // actual arg length!
}
if (angle_diff2 != 0)
{
const double radius = dist2/(2*sin(angle_diff2/2));
dist2 = fabs( angle_diff2 * radius ); // actual arg length!
}
}
double vel1 = dist1 / dt1->dt();
double vel2 = dist2 / dt2->dt();
// consider directions
// vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta()));
// vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta()));
vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) );
vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) );
const double acc_lin = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
const double omega1 = angle_diff1 / dt1->dt();
const double omega2 = angle_diff2 / dt2->dt();
const double acc_rot = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]);
}
#ifdef USE_ANALYTIC_JACOBI
#if 0
/*
* @brief Jacobi matrix of the cost function specified in computeError().
*/
void linearizeOplus()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
const VertexPointXY* conf1 = static_cast<const VertexPointXY*>(_vertices[0]);
const VertexPointXY* conf2 = static_cast<const VertexPointXY*>(_vertices[1]);
const VertexPointXY* conf3 = static_cast<const VertexPointXY*>(_vertices[2]);
const VertexTimeDiff* deltaT1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
const VertexTimeDiff* deltaT2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
const VertexOrientation* angle1 = static_cast<const VertexOrientation*>(_vertices[5]);
const VertexOrientation* angle2 = static_cast<const VertexOrientation*>(_vertices[6]);
const VertexOrientation* angle3 = static_cast<const VertexOrientation*>(_vertices[7]);
Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate();
Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate();
double dist1 = deltaS1.norm();
double dist2 = deltaS2.norm();
double sum_time = deltaT1->estimate() + deltaT2->estimate();
double sum_time_inv = 1 / sum_time;
double dt1_inv = 1/deltaT1->estimate();
double dt2_inv = 1/deltaT2->estimate();
double aux0 = 2/sum_time_inv;
double aux1 = dist1 * deltaT1->estimate();
double aux2 = dist2 * deltaT2->estimate();
double vel1 = dist1 * dt1_inv;
double vel2 = dist2 * dt2_inv;
double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv;
double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv;
double acc = (vel2 - vel1) * aux0;
double omegadot = (omega2 - omega1) * aux0;
double aux3 = -acc/2;
double aux4 = -omegadot/2;
double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
_jacobianOplus[0].resize(2,2); // conf1
_jacobianOplus[1].resize(2,2); // conf2
_jacobianOplus[2].resize(2,2); // conf3
_jacobianOplus[3].resize(2,1); // deltaT1
_jacobianOplus[4].resize(2,1); // deltaT2
_jacobianOplus[5].resize(2,1); // angle1
_jacobianOplus[6].resize(2,1); // angle2
_jacobianOplus[7].resize(2,1); // angle3
if (aux1==0) aux1=1e-20;
if (aux2==0) aux2=1e-20;
if (dev_border_acc!=0)
{
// TODO: double aux = aux0 * dev_border_acc;
// double aux123 = aux / aux1;
_jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1
_jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1
_jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2
_jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2
_jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3
_jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3
_jacobianOplus[2](0,0) = 0;
_jacobianOplus[2](0,1) = 0;
_jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1
_jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2
}
else
{
_jacobianOplus[0](0,0) = 0; // acc x1
_jacobianOplus[0](0,1) = 0; // acc y1
_jacobianOplus[1](0,0) = 0; // acc x2
_jacobianOplus[1](0,1) = 0; // acc y2
_jacobianOplus[2](0,0) = 0; // acc x3
_jacobianOplus[2](0,1) = 0; // acc y3
_jacobianOplus[3](0,0) = 0; // acc deltaT1
_jacobianOplus[4](0,0) = 0; // acc deltaT2
}
if (dev_border_omegadot!=0)
{
_jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1
_jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2
_jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1
_jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2
_jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3
}
else
{
_jacobianOplus[3](1,0) = 0; // omegadot deltaT1
_jacobianOplus[4](1,0) = 0; // omegadot deltaT2
_jacobianOplus[5](1,0) = 0; // omegadot angle1
_jacobianOplus[6](1,0) = 0; // omegadot angle2
_jacobianOplus[7](1,0) = 0; // omegadot angle3
}
_jacobianOplus[0](1,0) = 0; // omegadot x1
_jacobianOplus[0](1,1) = 0; // omegadot y1
_jacobianOplus[1](1,0) = 0; // omegadot x2
_jacobianOplus[1](1,1) = 0; // omegadot y2
_jacobianOplus[2](1,0) = 0; // omegadot x3
_jacobianOplus[2](1,1) = 0; // omegadot y3
_jacobianOplus[5](0,0) = 0; // acc angle1
_jacobianOplus[6](0,0) = 0; // acc angle2
_jacobianOplus[7](0,0) = 0; // acc angle3
}
#endif
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class EdgeAccelerationStart
* @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.
*
* The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity()
* and minimizes: \n
* \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n
* \e a is calculated using the difference quotient (twice) and the position parts of the poses. \n
* \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
* \e weight can be set using setInformation(). \n
* \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
* The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
* the second one the rotational acceleration.
* @see TebOptimalPlanner::AddEdgesAcceleration
* @see EdgeAcceleration
* @see EdgeAccelerationGoal
* @remarks Do not forget to call setTebConfig()
* @remarks Refer to EdgeAccelerationGoal() for defining boundary values at the end of the trajectory!
*/
class EdgeAccelerationStart : public BaseTebMultiEdge<2, const geometry_msgs::Twist*>
{
public:
/**
* @brief Construct edge.
*/
EdgeAccelerationStart()
{
_measurement = NULL;
this->resize(3);
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
// VELOCITY & ACCELERATION
const Eigen::Vector2d diff = pose2->position() - pose1->position();
double dist = diff.norm();
const double angle_diff = g2o::normalize_theta(pose2->theta() - pose1->theta());
if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
{
const double radius = dist/(2*sin(angle_diff/2));
dist = fabs( angle_diff * radius ); // actual arg length!
}
const double vel1 = _measurement->linear.x;
double vel2 = dist / dt->dt();
// consider directions
//vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta()));
vel2 *= fast_sigmoid( 100*(diff.x()*cos(pose1->theta()) + diff.y()*sin(pose1->theta())) );
const double acc_lin = (vel2 - vel1) / dt->dt();
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
const double omega1 = _measurement->angular.z;
const double omega2 = angle_diff / dt->dt();
const double acc_rot = (omega2 - omega1) / dt->dt();
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]);
}
/**
* @brief Set the initial velocity that is taken into account for calculating the acceleration
* @param vel_start twist message containing the translational and rotational velocity
*/
void setInitialVelocity(const geometry_msgs::Twist& vel_start)
{
_measurement = &vel_start;
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class EdgeAccelerationGoal
* @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
*
* The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity()
* and minimizes: \n
* \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n
* \e a is calculated using the difference quotient (twice) and the position parts of the poses \n
* \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
* \e weight can be set using setInformation() \n
* \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
* The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
* the second one the rotational acceleration.
* @see TebOptimalPlanner::AddEdgesAcceleration
* @see EdgeAcceleration
* @see EdgeAccelerationStart
* @remarks Do not forget to call setTebConfig()
* @remarks Refer to EdgeAccelerationStart() for defining boundary (initial) values at the end of the trajectory
*/
class EdgeAccelerationGoal : public BaseTebMultiEdge<2, const geometry_msgs::Twist*>
{
public:
/**
* @brief Construct edge.
*/
EdgeAccelerationGoal()
{
_measurement = NULL;
this->resize(3);
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
// VELOCITY & ACCELERATION
const Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();
double dist = diff.norm();
const double angle_diff = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta());
if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
{
double radius = dist/(2*sin(angle_diff/2));
dist = fabs( angle_diff * radius ); // actual arg length!
}
double vel1 = dist / dt->dt();
const double vel2 = _measurement->linear.x;
// consider directions
//vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta()));
vel1 *= fast_sigmoid( 100*(diff.x()*cos(pose_pre_goal->theta()) + diff.y()*sin(pose_pre_goal->theta())) );
const double acc_lin = (vel2 - vel1) / dt->dt();
_error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
const double omega1 = angle_diff / dt->dt();
const double omega2 = _measurement->angular.z;
const double acc_rot = (omega2 - omega1) / dt->dt();
_error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]);
}
/**
* @brief Set the goal / final velocity that is taken into account for calculating the acceleration
* @param vel_goal twist message containing the translational and rotational velocity
*/
void setGoalVelocity(const geometry_msgs::Twist& vel_goal)
{
_measurement = &vel_goal;
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class EdgeAccelerationHolonomic
* @brief Edge defining the cost function for limiting the translational and rotational acceleration.
*
* The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes:
* \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot } ]^T ) \cdot weight \f$. \n
* \e ax is calculated using the difference quotient (twice) and the x position parts of all three poses \n
* \e ay is calculated using the difference quotient (twice) and the y position parts of all three poses \n
* \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
* \e weight can be set using setInformation() \n
* \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
* The dimension of the error / cost vector is 3: the first component represents the translational acceleration (x-dir),
* the second one the strafing acceleration and the third one the rotational acceleration.
* @see TebOptimalPlanner::AddEdgesAcceleration
* @see EdgeAccelerationHolonomicStart
* @see EdgeAccelerationHolonomicGoal
* @remarks Do not forget to call setTebConfig()
* @remarks Refer to EdgeAccelerationHolonomicStart() and EdgeAccelerationHolonomicGoal() for defining boundary values!
*/
class EdgeAccelerationHolonomic : public BaseTebMultiEdge<3, double>
{
public:
/**
* @brief Construct edge.
*/
EdgeAccelerationHolonomic()
{
this->resize(5);
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
// VELOCITY & ACCELERATION
Eigen::Vector2d diff1 = pose2->position() - pose1->position();
Eigen::Vector2d diff2 = pose3->position() - pose2->position();
double cos_theta1 = std::cos(pose1->theta());
double sin_theta1 = std::sin(pose1->theta());
double cos_theta2 = std::cos(pose2->theta());
double sin_theta2 = std::sin(pose2->theta());
// transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
double p1_dx = cos_theta1*diff1.x() + sin_theta1*diff1.y();
double p1_dy = -sin_theta1*diff1.x() + cos_theta1*diff1.y();
// transform pose3 into robot frame pose2 (inverse 2d rotation matrix)
double p2_dx = cos_theta2*diff2.x() + sin_theta2*diff2.y();
double p2_dy = -sin_theta2*diff2.x() + cos_theta2*diff2.y();
double vel1_x = p1_dx / dt1->dt();
double vel1_y = p1_dy / dt1->dt();
double vel2_x = p2_dx / dt2->dt();
double vel2_y = p2_dy / dt2->dt();
double dt12 = dt1->dt() + dt2->dt();
double acc_x = (vel2_x - vel1_x)*2 / dt12;
double acc_y = (vel2_y - vel1_y)*2 / dt12;
_error[0] = penaltyBoundToInterval(acc_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(acc_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt();
double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt();
double acc_rot = (omega2 - omega1)*2 / dt12;
_error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() strafing: _error[1]=%f\n",_error[1]);
ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAcceleration::computeError() rotational: _error[2]=%f\n",_error[2]);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class EdgeAccelerationHolonomicStart
* @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.
*
* The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity()
* and minimizes: \n
* \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n
* \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses. \n
* \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses. \n
* \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
* \e weight can be set using setInformation(). \n
* \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
* The dimension of the error / cost vector is 3: the first component represents the translational acceleration,
* the second one the strafing acceleration and the third one the rotational acceleration.
* @see TebOptimalPlanner::AddEdgesAcceleration
* @see EdgeAccelerationHolonomic
* @see EdgeAccelerationHolonomicGoal
* @remarks Do not forget to call setTebConfig()
* @remarks Refer to EdgeAccelerationHolonomicGoal() for defining boundary values at the end of the trajectory!
*/
class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry_msgs::Twist*>
{
public:
/**
* @brief Construct edge.
*/
EdgeAccelerationHolonomicStart()
{
this->resize(3);
_measurement = NULL;
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
// VELOCITY & ACCELERATION
Eigen::Vector2d diff = pose2->position() - pose1->position();
double cos_theta1 = std::cos(pose1->theta());
double sin_theta1 = std::sin(pose1->theta());
// transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y();
double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
double vel1_x = _measurement->linear.x;
double vel1_y = _measurement->linear.y;
double vel2_x = p1_dx / dt->dt();
double vel2_y = p1_dy / dt->dt();
double acc_lin_x = (vel2_x - vel1_x) / dt->dt();
double acc_lin_y = (vel2_y - vel1_y) / dt->dt();
_error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
double omega1 = _measurement->angular.z;
double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt();
double acc_rot = (omega2 - omega1) / dt->dt();
_error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]);
ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n",_error[2]);
}
/**
* @brief Set the initial velocity that is taken into account for calculating the acceleration
* @param vel_start twist message containing the translational and rotational velocity
*/
void setInitialVelocity(const geometry_msgs::Twist& vel_start)
{
_measurement = &vel_start;
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class EdgeAccelerationHolonomicGoal
* @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
*
* The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity()
* and minimizes: \n
* \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n
* \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses \n
* \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses \n
* \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n
* \e weight can be set using setInformation() \n
* \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
* The dimension of the error / cost vector is 3: the first component represents the translational acceleration,
* the second one is the strafing velocity and the third one the rotational acceleration.
* @see TebOptimalPlanner::AddEdgesAcceleration
* @see EdgeAccelerationHolonomic
* @see EdgeAccelerationHolonomicStart
* @remarks Do not forget to call setTebConfig()
* @remarks Refer to EdgeAccelerationHolonomicStart() for defining boundary (initial) values at the end of the trajectory
*/
class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_msgs::Twist*>
{
public:
/**
* @brief Construct edge.
*/
EdgeAccelerationHolonomicGoal()
{
_measurement = NULL;
this->resize(3);
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
// VELOCITY & ACCELERATION
Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();
double cos_theta1 = std::cos(pose_pre_goal->theta());
double sin_theta1 = std::sin(pose_pre_goal->theta());
// transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y();
double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
double vel1_x = p1_dx / dt->dt();
double vel1_y = p1_dy / dt->dt();
double vel2_x = _measurement->linear.x;
double vel2_y = _measurement->linear.y;
double acc_lin_x = (vel2_x - vel1_x) / dt->dt();
double acc_lin_y = (vel2_y - vel1_y) / dt->dt();
_error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
// ANGULAR ACCELERATION
double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt();
double omega2 = _measurement->angular.z;
double acc_rot = (omega2 - omega1) / dt->dt();
_error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]);
ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationGoal::computeError() rotational: _error[2]=%f\n",_error[2]);
}
/**
* @brief Set the goal / final velocity that is taken into account for calculating the acceleration
* @param vel_goal twist message containing the translational and rotational velocity
*/
void setGoalVelocity(const geometry_msgs::Twist& vel_goal)
{
_measurement = &vel_goal;
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}; // end namespace
#endif /* EDGE_ACCELERATION_H_ */

153
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h Executable file
View File

@@ -0,0 +1,153 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann, Franz Albers
*********************************************************************/
#ifndef EDGE_DYNAMICOBSTACLE_H
#define EDGE_DYNAMICOBSTACLE_H
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/vertex_timediff.h>
#include <teb_local_planner/g2o_types/penalties.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/teb_config.h>
#include <teb_local_planner/robot_footprint_model.h>
namespace teb_local_planner
{
/**
* @class EdgeDynamicObstacle
* @brief Edge defining the cost function for keeping a distance from dynamic (moving) obstacles.
*
* The edge depends on two vertices \f$ \mathbf{s}_i, \Delta T_i \f$ and minimizes: \n
* \f$ \min \textrm{penaltyBelow}( dist2obstacle) \cdot weight \f$. \n
* \e dist2obstacle denotes the minimum distance to the obstacle trajectory (spatial and temporal). \n
* \e weight can be set using setInformation(). \n
* \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow(). \n
* @see TebOptimalPlanner::AddEdgesDynamicObstacles
* @remarks Do not forget to call setTebConfig(), setVertexIdx() and
* @warning Experimental
*/
class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
{
public:
/**
* @brief Construct edge.
*/
EdgeDynamicObstacle() : t_(0)
{
}
/**
* @brief Construct edge and specify the time for its associated pose (neccessary for computeError).
* @param t_ Estimated time until current pose is reached
*/
EdgeDynamicObstacle(double t) : t_(t)
{
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()");
const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_);
_error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]);
}
/**
* @brief Set Obstacle for the underlying cost function
* @param obstacle Const pointer to an Obstacle or derived Obstacle
*/
void setObstacle(const Obstacle* obstacle)
{
_measurement = obstacle;
}
/**
* @brief Set pointer to the robot model
* @param robot_model Robot model required for distance calculation
*/
void setRobotModel(const BaseRobotFootprintModel* robot_model)
{
robot_model_ = robot_model;
}
/**
* @brief Set all parameters at once
* @param cfg TebConfig class
* @param robot_model Robot model required for distance calculation
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
{
cfg_ = &cfg;
robot_model_ = robot_model;
_measurement = obstacle;
}
protected:
const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
double t_; //!< Estimated time until current pose is reached
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace
#endif

230
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h Executable file
View File

@@ -0,0 +1,230 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef _EDGE_KINEMATICS_H
#define _EDGE_KINEMATICS_H
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/penalties.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <teb_local_planner/teb_config.h>
#include <cmath>
namespace teb_local_planner
{
/**
* @class EdgeKinematicsDiffDrive
* @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot.
*
* The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation
* of the non-holonomic constraint:
* - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
*
* The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n
* A second equation is implemented to penalize backward motions (second element of the error /cost vector). \n
* The \e weight can be set using setInformation(): Matrix element 2,2: (A value ~1 allows backward driving, but penalizes it slighly). \n
* The dimension of the error / cost vector is 2: the first component represents the nonholonomic constraint cost,
* the second one backward-drive cost.
* @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsCarlike
* @remarks Do not forget to call setTebConfig()
*/
class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
{
public:
/**
* @brief Construct edge.
*/
EdgeKinematicsDiffDrive()
{
this->setMeasurement(0.);
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
Eigen::Vector2d deltaS = conf2->position() - conf1->position();
// non holonomic constraint
_error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
// positive-drive-direction constraint
Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) );
_error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);
// epsilon=0, otherwise it pushes the first bandpoints away from start
ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
}
#ifdef USE_ANALYTIC_JACOBI
#if 1
/**
* @brief Jacobi matrix of the cost function specified in computeError().
*/
void linearizeOplus()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
Eigen::Vector2d deltaS = conf2->position() - conf1->position();
double cos1 = cos(conf1->theta());
double cos2 = cos(conf2->theta());
double sin1 = sin(conf1->theta());
double sin2 = sin(conf2->theta());
double aux1 = sin1 + sin2;
double aux2 = cos1 + cos2;
double dd_error_1 = deltaS[0]*cos1;
double dd_error_2 = deltaS[1]*sin1;
double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0);
double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] -
( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
// conf1
_jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1
_jacobianOplusXi(0,1) = -aux2 * dev_nh_abs; // nh y1
_jacobianOplusXi(1,0) = -cos1 * dd_dev; // drive-dir x1
_jacobianOplusXi(1,1) = -sin1 * dd_dev; // drive-dir y1
_jacobianOplusXi(0,2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle
_jacobianOplusXi(1,2) = ( -sin1*deltaS[0] + cos1*deltaS[1] ) * dd_dev; // drive-dir angle1
// conf2
_jacobianOplusXj(0,0) = -aux1 * dev_nh_abs; // nh x2
_jacobianOplusXj(0,1) = aux2 * dev_nh_abs; // nh y2
_jacobianOplusXj(1,0) = cos1 * dd_dev; // drive-dir x2
_jacobianOplusXj(1,1) = sin1 * dd_dev; // drive-dir y2
_jacobianOplusXj(0,2) = (-sin2*deltaS[1] - cos2*deltaS[0]) * dev_nh_abs; // nh angle
_jacobianOplusXj(1,2) = 0; // drive-dir angle1
}
#endif
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class EdgeKinematicsCarlike
* @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot.
*
* The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation
* of the non-holonomic constraint:
* - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
*
* The definition is identically to the one of the differential drive robot.
* Additionally, this edge incorporates a minimum turning radius that is required by carlike robots.
* The turning radius is defined by \f$ r=v/omega \f$.
*
* The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n
* The second equation enforces a minimum turning radius.
* The \e weight can be set using setInformation(): Matrix element 2,2. \n
* The dimension of the error / cost vector is 3: the first component represents the nonholonomic constraint cost,
* the second one backward-drive cost and the third one the minimum turning radius
* @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsDiffDrive
* @remarks Bounding the turning radius from below is not affected by the penalty_epsilon parameter,
* the user might add an extra margin to the min_turning_radius param.
* @remarks Do not forget to call setTebConfig()
*/
class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
{
public:
/**
* @brief Construct edge.
*/
EdgeKinematicsCarlike()
{
this->setMeasurement(0.);
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()");
const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
Eigen::Vector2d deltaS = conf2->position() - conf1->position();
// non holonomic constraint
_error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
// limit minimum turning radius
double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() );
if (angle_diff == 0)
_error[1] = 0; // straight line motion
else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius
_error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0);
else
_error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0);
// This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter.
ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace
#endif

295
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h Executable file
View File

@@ -0,0 +1,295 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef EDGE_OBSTACLE_H_
#define EDGE_OBSTACLE_H_
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/robot_footprint_model.h>
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <teb_local_planner/g2o_types/penalties.h>
#include <teb_local_planner/teb_config.h>
namespace teb_local_planner
{
/**
* @class EdgeObstacle
* @brief Edge defining the cost function for keeping a minimum distance from obstacles.
*
* The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
* \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n
* \e dist2point denotes the minimum distance to the point obstacle. \n
* \e weight can be set using setInformation(). \n
* \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
* @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
* @remarks Do not forget to call setTebConfig() and setObstacle()
*/
class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
{
public:
/**
* @brief Construct edge.
*/
EdgeObstacle()
{
_measurement = NULL;
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()");
const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
// Original obstacle cost.
_error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
// Optional non-linear cost. Note the max cost (before weighting) is
// the same as the straight line version and that all other costs are
// below the straight line (for positive exponent), so it may be
// necessary to increase weight_obstacle and/or the inflation_weight
// when using larger exponents.
_error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
}
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]);
}
#ifdef USE_ANALYTIC_JACOBI
#if 0
/**
* @brief Jacobi matrix of the cost function specified in computeError().
*/
void linearizeOplus()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgePointObstacle()");
const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
Eigen::Vector2d deltaS = *_measurement - bandpt->position();
double angdiff = atan2(deltaS[1],deltaS[0])-bandpt->theta();
double dist_squared = deltaS.squaredNorm();
double dist = sqrt(dist_squared);
double aux0 = sin(angdiff);
double dev_left_border = penaltyBoundFromBelowDerivative(dist*fabs(aux0),cfg_->obstacles.min_obstacle_dist,cfg_->optim.penalty_epsilon);
if (dev_left_border==0)
{
_jacobianOplusXi( 0 , 0 ) = 0;
_jacobianOplusXi( 0 , 1 ) = 0;
_jacobianOplusXi( 0 , 2 ) = 0;
return;
}
double aux1 = -fabs(aux0) / dist;
double dev_norm_x = deltaS[0]*aux1;
double dev_norm_y = deltaS[1]*aux1;
double aux2 = cos(angdiff) * g2o::sign(aux0);
double aux3 = aux2 / dist_squared;
double dev_proj_x = aux3 * deltaS[1] * dist;
double dev_proj_y = -aux3 * deltaS[0] * dist;
double dev_proj_angle = -aux2;
_jacobianOplusXi( 0 , 0 ) = dev_left_border * ( dev_norm_x + dev_proj_x );
_jacobianOplusXi( 0 , 1 ) = dev_left_border * ( dev_norm_y + dev_proj_y );
_jacobianOplusXi( 0 , 2 ) = dev_left_border * dev_proj_angle;
}
#endif
#endif
/**
* @brief Set pointer to associated obstacle for the underlying cost function
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setObstacle(const Obstacle* obstacle)
{
_measurement = obstacle;
}
/**
* @brief Set pointer to the robot model
* @param robot_model Robot model required for distance calculation
*/
void setRobotModel(const BaseRobotFootprintModel* robot_model)
{
robot_model_ = robot_model;
}
/**
* @brief Set all parameters at once
* @param cfg TebConfig class
* @param robot_model Robot model required for distance calculation
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
{
cfg_ = &cfg;
robot_model_ = robot_model;
_measurement = obstacle;
}
protected:
const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class EdgeInflatedObstacle
* @brief Edge defining the cost function for keeping a minimum distance from inflated obstacles.
*
* The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
* \f$ \min \textrm{penaltyBelow}( dist2point, min_obstacle_dist ) \cdot weight_inflation \f$. \n
* Additional, a second penalty is provided with \n
* \f$ \min \textrm{penaltyBelow}( dist2point, inflation_dist ) \cdot weight_inflation \f$.
* It is assumed that inflation_dist > min_obstacle_dist and weight_inflation << weight_inflation.
* \e dist2point denotes the minimum distance to the point obstacle. \n
* \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
* @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeObstacle
* @remarks Do not forget to call setTebConfig() and setObstacle()
*/
class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
{
public:
/**
* @brief Construct edge.
*/
EdgeInflatedObstacle()
{
_measurement = NULL;
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeInflatedObstacle()");
const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
// Original "straight line" obstacle cost. The max possible value
// before weighting is min_obstacle_dist
_error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
// Optional non-linear cost. Note the max cost (before weighting) is
// the same as the straight line version and that all other costs are
// below the straight line (for positive exponent), so it may be
// necessary to increase weight_obstacle and/or the inflation_weight
// when using larger exponents.
_error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
}
// Additional linear inflation cost
_error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0);
ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeInflatedObstacle::computeError() _error[0]=%f, _error[1]=%f\n",_error[0], _error[1]);
}
/**
* @brief Set pointer to associated obstacle for the underlying cost function
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setObstacle(const Obstacle* obstacle)
{
_measurement = obstacle;
}
/**
* @brief Set pointer to the robot model
* @param robot_model Robot model required for distance calculation
*/
void setRobotModel(const BaseRobotFootprintModel* robot_model)
{
robot_model_ = robot_model;
}
/**
* @brief Set all parameters at once
* @param cfg TebConfig class
* @param robot_model Robot model required for distance calculation
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
{
cfg_ = &cfg;
robot_model_ = robot_model;
_measurement = obstacle;
}
protected:
const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace
#endif

115
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h Executable file
View File

@@ -0,0 +1,115 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef EDGE_PREFER_ROTDIR_H_
#define EDGE_PREFER_ROTDIR_H_
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <teb_local_planner/g2o_types/penalties.h>
#include "g2o/core/base_unary_edge.h"
namespace teb_local_planner
{
/**
* @class EdgePreferRotDir
* @brief Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns
*
* The edge depends on two consecutive vertices \f$ \mathbf{s}_i, \mathbf{s}_{i+1} \f$ and penalizes a given rotation direction
* based on the \e weight and \e dir (\f$ dir \in \{-1,1\} \f$)
* \e dir should be +1 to prefer left rotations and -1 to prefer right rotations \n
* \e weight can be set using setInformation(). \n
* @see TebOptimalPlanner::AddEdgePreferRotDir
*/
class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose>
{
public:
/**
* @brief Construct edge.
*/
EdgePreferRotDir()
{
_measurement = 1;
}
/**
* @brief Actual cost function
*/
void computeError()
{
const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
_error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]);
}
/**
* @brief Specify the prefered direction of rotation
* @param dir +1 to prefer the left side, -1 to prefer the right side
*/
void setRotDir(double dir)
{
_measurement = dir;
}
/** Prefer rotations to the right */
void preferRight() {_measurement = -1;}
/** Prefer rotations to the right */
void preferLeft() {_measurement = 1;}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace
#endif

89
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h Executable file
View File

@@ -0,0 +1,89 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef EDGE_SHORTEST_PATH_H_
#define EDGE_SHORTEST_PATH_H_
#include <float.h>
#include <base_local_planner/BaseLocalPlannerConfig.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <Eigen/Core>
namespace teb_local_planner {
/**
* @class EdgeShortestPath
* @brief Edge defining the cost function for minimizing the Euclidean distance between two consectuive poses.
*
* @see TebOptimalPlanner::AddEdgesShortestPath
*/
class EdgeShortestPath : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> {
public:
/**
* @brief Construct edge.
*/
EdgeShortestPath() { this->setMeasurement(0.); }
/**
* @brief Actual cost function
*/
void computeError() {
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeShortestPath()");
const VertexPose *pose1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose *pose2 = static_cast<const VertexPose*>(_vertices[1]);
_error[0] = (pose2->position() - pose1->position()).norm();
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeShortestPath::computeError() _error[0]=%f\n", _error[0]);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace
#endif /* EDGE_SHORTEST_PATH_H_ */

116
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h Executable file
View File

@@ -0,0 +1,116 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef EDGE_TIMEOPTIMAL_H_
#define EDGE_TIMEOPTIMAL_H_
#include <float.h>
#include <base_local_planner/BaseLocalPlannerConfig.h>
#include <teb_local_planner/g2o_types/vertex_timediff.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <teb_local_planner/g2o_types/penalties.h>
#include <teb_local_planner/teb_config.h>
#include <Eigen/Core>
namespace teb_local_planner
{
/**
* @class EdgeTimeOptimal
* @brief Edge defining the cost function for minimizing transition time of the trajectory.
*
* The edge depends on a single vertex \f$ \Delta T_i \f$ and minimizes: \n
* \f$ \min \Delta T_i^2 \cdot scale \cdot weight \f$. \n
* \e scale is determined using the penaltyEquality() function, since we experiences good convergence speeds with it. \n
* \e weight can be set using setInformation() (something around 1.0 seems to be fine). \n
* @see TebOptimalPlanner::AddEdgesTimeOptimal
* @remarks Do not forget to call setTebConfig()
*/
class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff>
{
public:
/**
* @brief Construct edge.
*/
EdgeTimeOptimal()
{
this->setMeasurement(0.);
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
const VertexTimeDiff* timediff = static_cast<const VertexTimeDiff*>(_vertices[0]);
_error[0] = timediff->dt();
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]);
}
#ifdef USE_ANALYTIC_JACOBI
/**
* @brief Jacobi matrix of the cost function specified in computeError().
*/
void linearizeOplus()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
_jacobianOplusXi( 0 , 0 ) = 1;
}
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}; // end namespace
#endif /* EDGE_TIMEOPTIMAL_H_ */

283
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h Executable file
View File

@@ -0,0 +1,283 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef EDGE_VELOCITY_H
#define EDGE_VELOCITY_H
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/vertex_timediff.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <teb_local_planner/g2o_types/penalties.h>
#include <teb_local_planner/teb_config.h>
#include <iostream>
namespace teb_local_planner
{
/**
* @class EdgeVelocity
* @brief Edge defining the cost function for limiting the translational and rotational velocity.
*
* The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n
* \f$ \min \textrm{penaltyInterval}( [v,omega]^T ) \cdot weight \f$. \n
* \e v is calculated using the difference quotient and the position parts of both poses. \n
* \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n
* \e weight can be set using setInformation(). \n
* \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
* The dimension of the error / cost vector is 2: the first component represents the translational velocity and
* the second one the rotational velocity.
* @see TebOptimalPlanner::AddEdgesVelocity
* @remarks Do not forget to call setTebConfig()
*/
class EdgeVelocity : public BaseTebMultiEdge<2, double>
{
public:
/**
* @brief Construct edge.
*/
EdgeVelocity()
{
this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
double dist = deltaS.norm();
const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
{
double radius = dist/(2*sin(angle_diff/2));
dist = fabs( angle_diff * radius ); // actual arg length!
}
double vel = dist / deltaT->estimate();
// vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
const double omega = angle_diff / deltaT->estimate();
_error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
}
#ifdef USE_ANALYTIC_JACOBI
#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value)
// Change accordingly...
/**
* @brief Jacobi matrix of the cost function specified in computeError().
*/
void linearizeOplus()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
Eigen::Vector2d deltaS = conf2->position() - conf1->position();
double dist = deltaS.norm();
double aux1 = dist*deltaT->estimate();
double aux2 = 1/deltaT->estimate();
double vel = dist * aux2;
double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2;
double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
_jacobianOplus[0].resize(2,3); // conf1
_jacobianOplus[1].resize(2,3); // conf2
_jacobianOplus[2].resize(2,1); // deltaT
// if (aux1==0) aux1=1e-6;
// if (aux2==0) aux2=1e-6;
if (dev_border_vel!=0)
{
double aux3 = dev_border_vel / aux1;
_jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1
_jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1
_jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2
_jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2
_jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT
}
else
{
_jacobianOplus[0](0,0) = 0; // vel x1
_jacobianOplus[0](0,1) = 0; // vel y1
_jacobianOplus[1](0,0) = 0; // vel x2
_jacobianOplus[1](0,1) = 0; // vel y2
_jacobianOplus[2](0,0) = 0; // vel deltaT
}
if (dev_border_omega!=0)
{
double aux4 = aux2 * dev_border_omega;
_jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT
_jacobianOplus[0](1,2) = -aux4; // omega angle1
_jacobianOplus[1](1,2) = aux4; // omega angle2
}
else
{
_jacobianOplus[2](1,0) = 0; // omega deltaT
_jacobianOplus[0](1,2) = 0; // omega angle1
_jacobianOplus[1](1,2) = 0; // omega angle2
}
_jacobianOplus[0](1,0) = 0; // omega x1
_jacobianOplus[0](1,1) = 0; // omega y1
_jacobianOplus[1](1,0) = 0; // omega x2
_jacobianOplus[1](1,1) = 0; // omega y2
_jacobianOplus[0](0,2) = 0; // vel angle1
_jacobianOplus[1](0,2) = 0; // vel angle2
}
#endif
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class EdgeVelocityHolonomic
* @brief Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta.
*
* The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n
* \f$ \min \textrm{penaltyInterval}( [vx,vy,omega]^T ) \cdot weight \f$. \n
* \e vx denotes the translational velocity w.r.t. x-axis (computed using finite differneces). \n
* \e vy denotes the translational velocity w.r.t. y-axis (computed using finite differneces). \n
* \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n
* \e weight can be set using setInformation(). \n
* \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
* The dimension of the error / cost vector is 3: the first component represents the translational velocity w.r.t. x-axis,
* the second one w.r.t. the y-axis and the third one the rotational velocity.
* @see TebOptimalPlanner::AddEdgesVelocity
* @remarks Do not forget to call setTebConfig()
*/
class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
{
public:
/**
* @brief Construct edge.
*/
EdgeVelocityHolonomic()
{
this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()");
const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
Eigen::Vector2d deltaS = conf2->position() - conf1->position();
double cos_theta1 = std::cos(conf1->theta());
double sin_theta1 = std::sin(conf1->theta());
// transform conf2 into current robot frame conf1 (inverse 2d rotation matrix)
double r_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
double r_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
double vx = r_dx / deltaT->estimate();
double vy = r_dy / deltaT->estimate();
double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
double max_vel_trans_remaining_y;
double max_vel_trans_remaining_x;
max_vel_trans_remaining_y = std::sqrt(std::max(0.0, cfg_->robot.max_vel_trans * cfg_->robot.max_vel_trans - vx * vx));
max_vel_trans_remaining_x = std::sqrt(std::max(0.0, cfg_->robot.max_vel_trans * cfg_->robot.max_vel_trans - vy * vy));
double max_vel_y = std::min(max_vel_trans_remaining_y, cfg_->robot.max_vel_y);
double max_vel_x = std::min(max_vel_trans_remaining_x, cfg_->robot.max_vel_x);
double max_vel_x_backwards = std::min(max_vel_trans_remaining_x, cfg_->robot.max_vel_x_backwards);
_error[0] = penaltyBoundToInterval(vx, -max_vel_x_backwards, max_vel_x, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(vy, max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
_error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]),
"EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace
#endif

166
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h Executable file
View File

@@ -0,0 +1,166 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
*********************************************************************/
#pragma once
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include <teb_local_planner/g2o_types/vertex_timediff.h>
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/robot_footprint_model.h>
namespace teb_local_planner
{
/**
* @class EdgeVelocityObstacleRatio
* @brief Edge defining the cost function for keeping a minimum distance from obstacles.
*
* The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
* \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n
* \e dist2point denotes the minimum distance to the point obstacle. \n
* \e weight can be set using setInformation(). \n
* \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
* @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
* @remarks Do not forget to call setTebConfig() and setObstacle()
*/
class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
{
public:
/**
* @brief Construct edge.
*/
EdgeVelocityObstacleRatio() :
robot_model_(nullptr)
{
// The three vertices are two poses and one time difference
this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeVelocityObstacleRatio()");
const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
double dist = deltaS.norm();
const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
{
double radius = dist/(2*sin(angle_diff/2));
dist = fabs( angle_diff * radius ); // actual arg length!
}
double vel = dist / deltaT->estimate();
vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
const double omega = angle_diff / deltaT->estimate();
double dist_to_obstacle = robot_model_->calculateDistance(conf1->pose(), _measurement);
double ratio;
if (dist_to_obstacle < cfg_->obstacles.obstacle_proximity_lower_bound)
ratio = 0;
else if (dist_to_obstacle > cfg_->obstacles.obstacle_proximity_upper_bound)
ratio = 1;
else
ratio = (dist_to_obstacle - cfg_->obstacles.obstacle_proximity_lower_bound) /
(cfg_->obstacles.obstacle_proximity_upper_bound - cfg_->obstacles.obstacle_proximity_lower_bound);
ratio *= cfg_->obstacles.obstacle_proximity_ratio_max_vel;
const double max_vel_fwd = ratio * cfg_->robot.max_vel_x;
const double max_omega = ratio * cfg_->robot.max_vel_theta;
_error[0] = penaltyBoundToInterval(vel, max_vel_fwd, 0);
_error[1] = penaltyBoundToInterval(omega, max_omega, 0);
ROS_ASSERT_MSG(std::isfinite(_error[0]) || std::isfinite(_error[1]), "EdgeVelocityObstacleRatio::computeError() _error[0]=%f , _error[1]=%f\n",_error[0],_error[1]);
}
/**
* @brief Set pointer to associated obstacle for the underlying cost function
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setObstacle(const Obstacle* obstacle)
{
_measurement = obstacle;
}
/**
* @brief Set pointer to the robot model
* @param robot_model Robot model required for distance calculation
*/
void setRobotModel(const BaseRobotFootprintModel* robot_model)
{
robot_model_ = robot_model;
}
/**
* @brief Set all parameters at once
* @param cfg TebConfig class
* @param robot_model Robot model required for distance calculation
* @param obstacle 2D position vector containing the position of the obstacle
*/
void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
{
cfg_ = &cfg;
robot_model_ = robot_model;
_measurement = obstacle;
}
protected:
const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace

120
navigations/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h Executable file
View File

@@ -0,0 +1,120 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef EDGE_VIA_POINT_H_
#define EDGE_VIA_POINT_H_
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/base_teb_edges.h>
#include "g2o/core/base_unary_edge.h"
namespace teb_local_planner
{
/**
* @class EdgeViaPoint
* @brief Edge defining the cost function for pushing a configuration towards a via point
*
* The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
* \f$ \min dist2point \cdot weight \f$. \n
* \e dist2point denotes the distance to the via point. \n
* \e weight can be set using setInformation(). \n
* @see TebOptimalPlanner::AddEdgesViaPoints
* @remarks Do not forget to call setTebConfig() and setViaPoint()
*/
class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose>
{
public:
/**
* @brief Construct edge.
*/
EdgeViaPoint()
{
_measurement = NULL;
}
/**
* @brief Actual cost function
*/
void computeError()
{
ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()");
const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
_error[0] = (bandpt->position() - *_measurement).norm();
ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]);
}
/**
* @brief Set pointer to associated via point for the underlying cost function
* @param via_point 2D position vector containing the position of the via point
*/
void setViaPoint(const Eigen::Vector2d* via_point)
{
_measurement = via_point;
}
/**
* @brief Set all parameters at once
* @param cfg TebConfig class
* @param via_point 2D position vector containing the position of the via point
*/
void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point)
{
cfg_ = &cfg;
_measurement = via_point;
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace
#endif

193
navigations/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h Executable file
View File

@@ -0,0 +1,193 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef PENALTIES_H
#define PENALTIES_H
#include <cmath>
#include <Eigen/Core>
#include <g2o/stuff/misc.h>
namespace teb_local_planner
{
/**
* @brief Linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$
* @param var The scalar that should be bounded
* @param a lower and upper absolute bound
* @param epsilon safty margin (move bound to the interior of the interval)
* @see penaltyBoundToIntervalDerivative
* @return Penalty / cost value that is nonzero if the constraint is not satisfied
*/
inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon)
{
if (var < -a+epsilon)
{
return (-var - (a - epsilon));
}
if (var <= a-epsilon)
{
return 0.;
}
else
{
return (var - (a - epsilon));
}
}
/**
* @brief Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
* @param var The scalar that should be bounded
* @param a lower bound
* @param b upper bound
* @param epsilon safty margin (move bound to the interior of the interval)
* @see penaltyBoundToIntervalDerivative
* @return Penalty / cost value that is nonzero if the constraint is not satisfied
*/
inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
{
if (var < a+epsilon)
{
return (-var + (a + epsilon));
}
if (var <= b-epsilon)
{
return 0.;
}
else
{
return (var - (b - epsilon));
}
}
/**
* @brief Linear penalty function for bounding \c var from below: \f$ a < var \f$
* @param var The scalar that should be bounded
* @param a lower bound
* @param epsilon safty margin (move bound to the interior of the interval)
* @see penaltyBoundFromBelowDerivative
* @return Penalty / cost value that is nonzero if the constraint is not satisfied
*/
inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon)
{
if (var >= a+epsilon)
{
return 0.;
}
else
{
return (-var + (a+epsilon));
}
}
/**
* @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$
* @param var The scalar that should be bounded
* @param a lower and upper absolute bound
* @param epsilon safty margin (move bound to the interior of the interval)
* @see penaltyBoundToInterval
* @return Derivative of the penalty function w.r.t. \c var
*/
inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon)
{
if (var < -a+epsilon)
{
return -1;
}
if (var <= a-epsilon)
{
return 0.;
}
else
{
return 1;
}
}
/**
* @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
* @param var The scalar that should be bounded
* @param a lower bound
* @param b upper bound
* @param epsilon safty margin (move bound to the interior of the interval)
* @see penaltyBoundToInterval
* @return Derivative of the penalty function w.r.t. \c var
*/
inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& b, const double& epsilon)
{
if (var < a+epsilon)
{
return -1;
}
if (var <= b-epsilon)
{
return 0.;
}
else
{
return 1;
}
}
/**
* @brief Derivative of the linear penalty function for bounding \c var from below: \f$ a < var \f$
* @param var The scalar that should be bounded
* @param a lower bound
* @param epsilon safty margin (move bound to the interior of the interval)
* @see penaltyBoundFromBelow
* @return Derivative of the penalty function w.r.t. \c var
*/
inline double penaltyBoundFromBelowDerivative(const double& var, const double& a,const double& epsilon)
{
if (var >= a+epsilon)
{
return 0.;
}
else
{
return -1;
}
}
} // namespace teb_local_planner
#endif // PENALTIES_H

229
navigations/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h Executable file
View File

@@ -0,0 +1,229 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef VERTEX_POSE_H_
#define VERTEX_POSE_H_
#include <g2o/config.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/hyper_graph_action.h>
#include <g2o/stuff/misc.h>
#include <teb_local_planner/pose_se2.h>
namespace teb_local_planner
{
/**
* @class VertexPose
* @brief This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o
* @see PoseSE2
* @see VertexTimeDiff
*/
class VertexPose : public g2o::BaseVertex<3, PoseSE2 >
{
public:
/**
* @brief Default constructor
* @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
*/
VertexPose(bool fixed = false)
{
setToOriginImpl();
setFixed(fixed);
}
/**
* @brief Construct pose using a given PoseSE2
* @param pose PoseSE2 defining the pose [x, y, angle_rad]
* @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
*/
VertexPose(const PoseSE2& pose, bool fixed = false)
{
_estimate = pose;
setFixed(fixed);
}
/**
* @brief Construct pose using a given 2D position vector and orientation
* @param position Eigen::Vector2d containing x and y coordinates
* @param theta yaw-angle
* @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
*/
VertexPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed = false)
{
_estimate.position() = position;
_estimate.theta() = theta;
setFixed(fixed);
}
/**
* @brief Construct pose using single components x, y, and the yaw angle
* @param x x-coordinate
* @param y y-coordinate
* @param theta yaw angle in rad
* @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
*/
VertexPose(double x, double y, double theta, bool fixed = false)
{
_estimate.x() = x;
_estimate.y() = y;
_estimate.theta() = theta;
setFixed(fixed);
}
/**
* @brief Access the pose
* @see estimate
* @return reference to the PoseSE2 estimate
*/
inline PoseSE2& pose() {return _estimate;}
/**
* @brief Access the pose (read-only)
* @see estimate
* @return const reference to the PoseSE2 estimate
*/
inline const PoseSE2& pose() const {return _estimate;}
/**
* @brief Access the 2D position part
* @see estimate
* @return reference to the 2D position part
*/
inline Eigen::Vector2d& position() {return _estimate.position();}
/**
* @brief Access the 2D position part (read-only)
* @see estimate
* @return const reference to the 2D position part
*/
inline const Eigen::Vector2d& position() const {return _estimate.position();}
/**
* @brief Access the x-coordinate the pose
* @return reference to the x-coordinate
*/
inline double& x() {return _estimate.x();}
/**
* @brief Access the x-coordinate the pose (read-only)
* @return const reference to the x-coordinate
*/
inline const double& x() const {return _estimate.x();}
/**
* @brief Access the y-coordinate the pose
* @return reference to the y-coordinate
*/
inline double& y() {return _estimate.y();}
/**
* @brief Access the y-coordinate the pose (read-only)
* @return const reference to the y-coordinate
*/
inline const double& y() const {return _estimate.y();}
/**
* @brief Access the orientation part (yaw angle) of the pose
* @return reference to the yaw angle
*/
inline double& theta() {return _estimate.theta();}
/**
* @brief Access the orientation part (yaw angle) of the pose (read-only)
* @return const reference to the yaw angle
*/
inline const double& theta() const {return _estimate.theta();}
/**
* @brief Set the underlying estimate (2D vector) to zero.
*/
virtual void setToOriginImpl() override
{
_estimate.setZero();
}
/**
* @brief Define the update increment \f$ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} \f$.
* A simple addition for the position.
* The angle is first added to the previous estimated angle and afterwards normalized to the interval \f$ [-\pi \pi] \f$
* @param update increment that should be added to the previous esimate
*/
virtual void oplusImpl(const double* update) override
{
_estimate.plus(update);
}
/**
* @brief Read an estimate from an input stream.
* First the x-coordinate followed by y and the yaw angle.
* @param is input stream
* @return always \c true
*/
virtual bool read(std::istream& is) override
{
is >> _estimate.x() >> _estimate.y() >> _estimate.theta();
return true;
}
/**
* @brief Write the estimate to an output stream
* First the x-coordinate followed by y and the yaw angle.
* @param os output stream
* @return \c true if the export was successful, otherwise \c false
*/
virtual bool write(std::ostream& os) const override
{
os << _estimate.x() << " " << _estimate.y() << " " << _estimate.theta();
return os.good();
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif // VERTEX_POSE_H_

145
navigations/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h Executable file
View File

@@ -0,0 +1,145 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Notes:
* The following class is derived from a class defined by the
* g2o-framework. g2o is licensed under the terms of the BSD License.
* Refer to the base class source for detailed licensing information.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef VERTEX_TIMEDIFF_H
#define VERTEX_TIMEDIFF_H
#include "g2o/config.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/hyper_graph_action.h"
#include <Eigen/Core>
namespace teb_local_planner
{
/**
* @class VertexTimeDiff
* @brief This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o
*/
class VertexTimeDiff : public g2o::BaseVertex<1, double>
{
public:
/**
* @brief Default constructor
* @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
*/
VertexTimeDiff(bool fixed = false)
{
setToOriginImpl();
setFixed(fixed);
}
/**
* @brief Construct the TimeDiff vertex with a value
* @param dt time difference value of the vertex
* @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
*/
VertexTimeDiff(double dt, bool fixed = false)
{
_estimate = dt;
setFixed(fixed);
}
/**
* @brief Access the timediff value of the vertex
* @see estimate
* @return reference to dt
*/
inline double& dt() {return _estimate;}
/**
* @brief Access the timediff value of the vertex (read-only)
* @see estimate
* @return const reference to dt
*/
inline const double& dt() const {return _estimate;}
/**
* @brief Set the underlying TimeDiff estimate \f$ \Delta T \f$ to default.
*/
virtual void setToOriginImpl() override
{
_estimate = 0.1;
}
/**
* @brief Define the update increment \f$ \Delta T_{k+1} = \Delta T_k + update \f$.
* A simple addition implements what we want.
* @param update increment that should be added to the previous esimate
*/
virtual void oplusImpl(const double* update) override
{
_estimate += *update;
}
/**
* @brief Read an estimate of \f$ \Delta T \f$ from an input stream
* @param is input stream
* @return always \c true
*/
virtual bool read(std::istream& is) override
{
is >> _estimate;
return true;
}
/**
* @brief Write the estimate \f$ \Delta T \f$ to an output stream
* @param os output stream
* @return \c true if the export was successful, otherwise \c false
*/
virtual bool write(std::ostream& os) const override
{
os << estimate();
return os.good();
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
#endif

215
navigations/teb_local_planner/include/teb_local_planner/graph_search.h Executable file
View File

@@ -0,0 +1,215 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Authors: Christoph Rösmann, Franz Albers
*********************************************************************/
#ifndef GRAPH_SEARCH_INTERFACE_H
#define GRAPH_SEARCH_INTERFACE_H
#ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
#include <boost/graph/adjacency_list.hpp>
#else
// Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48:
// boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated,
// but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now.
#define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
#include <boost/graph/adjacency_list.hpp>
#undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
#endif
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/utility.hpp>
#include <boost/random.hpp>
#include <Eigen/Core>
#include <geometry_msgs/Twist.h>
#include <teb_local_planner/equivalence_relations.h>
#include <teb_local_planner/pose_se2.h>
#include <teb_local_planner/teb_config.h>
namespace teb_local_planner
{
class HomotopyClassPlanner; // Forward declaration
//! Vertex in the graph that is used to find homotopy classes (only stores 2D positions)
struct HcGraphVertex
{
public:
Eigen::Vector2d pos; // position of vertices in the map
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//! Abbrev. for the homotopy class search-graph type @see HcGraphVertex
typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph;
//! Abbrev. for vertex type descriptors in the homotopy class search-graph
typedef boost::graph_traits<HcGraph>::vertex_descriptor HcGraphVertexType;
//! Abbrev. for edge type descriptors in the homotopy class search-graph
typedef boost::graph_traits<HcGraph>::edge_descriptor HcGraphEdgeType;
//! Abbrev. for the vertices iterator of the homotopy class search-graph
typedef boost::graph_traits<HcGraph>::vertex_iterator HcGraphVertexIterator;
//! Abbrev. for the edges iterator of the homotopy class search-graph
typedef boost::graph_traits<HcGraph>::edge_iterator HcGraphEdgeIterator;
//! Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one
typedef boost::graph_traits<HcGraph>::adjacency_iterator HcGraphAdjecencyIterator;
//!< Inline function used for calculateHSignature() in combination with HCP graph vertex descriptors
inline std::complex<long double> getCplxFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
{
return std::complex<long double>(graph[vert_descriptor].pos.x(), graph[vert_descriptor].pos.y());
}
//!< Inline function used for initializing the TEB in combination with HCP graph vertex descriptors
inline const Eigen::Vector2d& getVector2dFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
{
return graph[vert_descriptor].pos;
}
/**
* @brief Base class for graph based path planning / homotopy class sampling
*/
class GraphSearchInterface
{
public:
virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false) = 0;
/**
* @brief Clear any existing graph of the homotopy class search
*/
void clearGraph() {graph_.clear();}
// HcGraph graph() const {return graph_;}
// Workaround. graph_ is public for now, beacuse otherwise the compilation fails with the same boost bug mentioned above.
HcGraph graph_; //!< Store the graph that is utilized to find alternative homotopy classes.
protected:
/**
* @brief Protected constructor that should be called by subclasses
*/
GraphSearchInterface(const TebConfig& cfg, HomotopyClassPlanner* hcp) : cfg_(&cfg), hcp_(hcp){}
/**
* @brief Depth First Search implementation to find all paths between the start and the specified goal vertex.
*
* Complete paths are stored to the internal path container.
* @sa http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/
* @param g Graph on which the depth first should be performed
* @param visited A container that stores visited vertices (pass an empty container, it will be filled inside during recursion).
* @param goal Desired goal vertex
* @param start_orientation Orientation of the first trajectory pose, required to initialize the trajectory/TEB
* @param goal_orientation Orientation of the goal trajectory pose, required to initialize the trajectory/TEB
* @param start_velocity start velocity (optional)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
*/
void DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false);
protected:
const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
HomotopyClassPlanner* const hcp_; //!< Raw pointer to the HomotopyClassPlanner. The HomotopyClassPlanner itself is guaranteed to outlive the graph search class it is holding.
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class lrKeyPointGraph : public GraphSearchInterface
{
public:
lrKeyPointGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){}
virtual ~lrKeyPointGraph(){}
/**
* @brief Create a graph containing points in the global frame that can be used to explore new possible paths between start and goal.
*
* 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
* All feasible paths between start and goal point are extracted using a Depth First Search afterwards. \n
* This version works very well for small point obstacles. For more complex obstacles call the createProbRoadmapGraph()
* method that samples keypoints in a predefined area and hopefully finds all relevant alternative paths.
*
* @see createProbRoadmapGraph
* @param start Start pose from wich to start on (e.g. the current robot pose).
* @param goal Goal pose to find paths to (e.g. the robot's goal).
* @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).
* @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]
* @param start_velocity start velocity (optional)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
*/
virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false);
};
class ProbRoadmapGraph : public GraphSearchInterface
{
public:
ProbRoadmapGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){}
virtual ~ProbRoadmapGraph(){}
/**
* @brief Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal.
*
* This version of the graph samples keypoints in a predefined area (config) in the current frame between start and goal. \n
* Afterwards all feasible paths between start and goal point are extracted using a Depth First Search. \n
* Use the sampling method for complex, non-point or huge obstacles. \n
* You may call createGraph() instead.
*
* @see createGraph
* @param start Start pose from wich to start on (e.g. the current robot pose).
* @param goal Goal pose to find paths to (e.g. the robot's goal).
* @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).
* @param no_samples number of random samples
* @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]
* @param start_velocity start velocity (optional)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
*/
virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false);
private:
boost::random::mt19937 rnd_generator_; //!< Random number generator used by createProbRoadmapGraph to sample graph keypoints.
};
} // end namespace
#endif // GRAPH_SEARCH_INTERFACE_H

428
navigations/teb_local_planner/include/teb_local_planner/h_signature.h Executable file
View File

@@ -0,0 +1,428 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Authors: Christoph Rösmann, Franz Albers
*********************************************************************/
#ifndef H_SIGNATURE_H_
#define H_SIGNATURE_H_
#include <teb_local_planner/equivalence_relations.h>
#include <teb_local_planner/misc.h>
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/teb_config.h>
#include <teb_local_planner/timed_elastic_band.h>
#include <ros/ros.h>
#include <math.h>
#include <algorithm>
#include <functional>
#include <vector>
#include <iterator>
namespace teb_local_planner
{
/**
* @brief The H-signature defines an equivalence relation based on homology in terms of complex calculus.
*
* The H-Signature depends on the obstacle configuration and can be utilized
* to check whether two trajectores belong to the same homology class.
* Refer to: \n
* - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
*/
class HSignature : public EquivalenceClass
{
public:
/**
* @brief Constructor accepting a TebConfig
* @param cfg TebConfig storing some user configuration options
*/
HSignature(const TebConfig& cfg) : cfg_(&cfg) {}
/**
* @brief Calculate the H-Signature of a path
*
* The implemented function accepts generic path descriptions that are restricted to the following structure: \n
* The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n
* Provide a unary function with the following signature <c> std::complex< long double > (const T& point_type) </c>
* that returns a complex value for the position (Re(*)=x, Im(*)=y).
*
* T could also be a pointer type, if the passed function also accepts a const T* point_Type.
*
* @param path_start Iterator to the first element in the path
* @param path_end Iterator to the last element in the path
* @param obstacles obstacle container
* @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
* @tparam BidirIter Bidirectional iterator type
* @tparam Fun function of the form std::complex< long double > (const T& point_type)
*/
template<typename BidirIter, typename Fun>
void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles)
{
if (obstacles->empty())
{
hsignature_ = std::complex<double>(0,0);
return;
}
ROS_ASSERT_MSG(cfg_->hcp.h_signature_prescaler>0.1 && cfg_->hcp.h_signature_prescaler<=1, "Only a prescaler on the interval (0.1,1] ist allowed.");
// guess values for f0
// paper proposes a+b=N-1 && |a-b|<=1, 1...N obstacles
int m = std::max( (int)obstacles->size()-1, 5 ); // for only a few obstacles we need a min threshold in order to get significantly high H-Signatures
int a = (int) std::ceil(double(m)/2.0);
int b = m-a;
std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points
typedef std::complex<long double> cplx;
// guess map size (only a really really coarse guess is required
// use distance from start to goal as distance to each direction
// TODO: one could move the map determination outside this function, since it remains constant for the whole planning interval
cplx start = fun_cplx_point(*path_start);
cplx end = fun_cplx_point(*path_end); // path_end points to the last point now after calling std::advance before
cplx delta = end-start;
cplx normal(-delta.imag(), delta.real());
cplx map_bottom_left;
cplx map_top_right;
if (std::abs(delta) < 3.0)
{ // set minimum bound on distance (we do not want to have numerical instabilities) and 3.0 performs fine...
map_bottom_left = start + cplx(0, -3);
map_top_right = start + cplx(3, 3);
}
else
{
map_bottom_left = start - normal;
map_top_right = start + delta + normal;
}
hsignature_ = 0; // reset local signature
std::vector<double> imag_proposals(5);
// iterate path
while(path_start != path_end)
{
cplx z1 = fun_cplx_point(*path_start);
cplx z2 = fun_cplx_point(*std::next(path_start));
for (std::size_t l=0; l<obstacles->size(); ++l) // iterate all obstacles
{
cplx obst_l = obstacles->at(l)->getCentroidCplx();
//cplx f0 = (long double) prescaler * std::pow(obst_l-map_bottom_left,a) * std::pow(obst_l-map_top_right,b);
cplx f0 = (long double) cfg_->hcp.h_signature_prescaler * (long double)a*(obst_l-map_bottom_left) * (long double)b*(obst_l-map_top_right);
// denum contains product with all obstacles exepct j==l
cplx Al = f0;
for (std::size_t j=0; j<obstacles->size(); ++j)
{
if (j==l)
continue;
cplx obst_j = obstacles->at(j)->getCentroidCplx();
cplx diff = obst_l - obst_j;
//if (diff.real()!=0 || diff.imag()!=0)
if (std::abs(diff)<0.05) // skip really close obstacles
continue;
else
Al /= diff;
}
// compute log value
double diff2 = std::abs(z2-obst_l);
double diff1 = std::abs(z1-obst_l);
if (diff2 == 0 || diff1 == 0)
continue;
double log_real = std::log(diff2)-std::log(diff1);
// complex ln has more than one solution -> choose minimum abs angle -> paper
double arg_diff = std::arg(z2-obst_l)-std::arg(z1-obst_l);
imag_proposals.at(0) = arg_diff;
imag_proposals.at(1) = arg_diff+2*M_PI;
imag_proposals.at(2) = arg_diff-2*M_PI;
imag_proposals.at(3) = arg_diff+4*M_PI;
imag_proposals.at(4) = arg_diff-4*M_PI;
double log_imag = *std::min_element(imag_proposals.begin(),imag_proposals.end(),smaller_than_abs);
cplx log_value(log_real,log_imag);
//cplx log_value = std::log(z2-obst_l)-std::log(z1-obst_l); // the principal solution doesn't seem to work
hsignature_ += Al*log_value;
}
++path_start;
}
}
/**
* @brief Check if two candidate classes are equivalent
* @param other The other equivalence class to test with
*/
virtual bool isEqual(const EquivalenceClass& other) const
{
const HSignature* hother = dynamic_cast<const HSignature*>(&other); // TODO: better architecture without dynamic_cast
if (hother)
{
double diff_real = std::abs(hother->hsignature_.real() - hsignature_.real());
double diff_imag = std::abs(hother->hsignature_.imag() - hsignature_.imag());
if (diff_real<=cfg_->hcp.h_signature_threshold && diff_imag<=cfg_->hcp.h_signature_threshold)
return true; // Found! Homotopy class already exists, therefore nothing added
}
else
ROS_ERROR("Cannot compare HSignature equivalence classes with types other than HSignature.");
return false;
}
/**
* @brief Check if the equivalence value is detected correctly
* @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
*/
virtual bool isValid() const
{
return std::isfinite(hsignature_.real()) && std::isfinite(hsignature_.imag());
}
/**
* @brief Check if the trajectory is non-looping around an obstacle.
* @return Returns always true, as this cannot be detected by this kind of H-Signature.
*/
virtual bool isReasonable() const
{
return true;
}
/**
* @brief Get the current value of the h-signature (read-only)
* @return h-signature in complex-number format
*/
const std::complex<long double>& value() const {return hsignature_;}
private:
const TebConfig* cfg_;
std::complex<long double> hsignature_;
};
/**
* @brief The H-signature in three dimensions (here: x-y-t) defines an equivalence relation based on homology using theorems from electro magnetism.
*
* The H-Signature depends on the obstacle configuration and can be utilized
* to check whether two trajectores belong to the same homology class.
* Refer to: \n
* - S. Bhattacharya et al.: Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3D, 2011
*/
class HSignature3d : public EquivalenceClass
{
public:
/**
* @brief Constructor accepting a TebConfig
* @param cfg TebConfig storing some user configuration options
*/
HSignature3d(const TebConfig& cfg) : cfg_(&cfg) {}
/**
* @brief Calculate the H-Signature of a path
*
* The implemented function accepts generic path descriptions that are restricted to the following structure: \n
* The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n
* Provide a unary function with the following signature <c> std::complex< long double > (const T& point_type) </c>
* that returns a complex value for the position (Re(*)=x, Im(*)=y).
*
* T could also be a pointer type, if the passed function also accepts a const T* point_Type.
*
* @param path_start Iterator to the first element in the path
* @param path_end Iterator to the last element in the path
* @param obstacles obstacle container
* @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
* @tparam BidirIter Bidirectional iterator type
* @tparam Fun function of the form std::complex< long double > (const T& point_type)
*/
template<typename BidirIter, typename Fun>
void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
{
hsignature3d_.resize(obstacles->size());
std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points)
constexpr int num_int_steps_per_segment = 10;
for (std::size_t l = 0; l < obstacles->size(); ++l) // iterate all obstacles
{
double H = 0;
double transition_time = 0;
double next_transition_time = 0;
BidirIter path_iter;
TimeDiffSequence::iterator timediff_iter;
Eigen::Vector3d s1 (obstacles->at(l)->getCentroid()(0), obstacles->at(l)->getCentroid()(1), 0);
double t = 120; // some large value for defining the end point of the obstacle/"conductor" model
Eigen::Vector3d s2;
obstacles->at(l)->predictCentroidConstantVelocity(t, s2.head(2));
s2[2] = t;
Eigen::Vector3d ds = s2 - s1;
double ds_sq_norm = ds.squaredNorm(); // by definition not zero as t > 0 (3rd component)
// iterate path
for (path_iter = path_start, timediff_iter = timediff_start.get(); path_iter != path_end; ++path_iter, ++timediff_iter)
{
std::complex<long double> z1 = fun_cplx_point(*path_iter);
std::complex<long double> z2 = fun_cplx_point(*std::next(path_iter));
transition_time = next_transition_time;
if (timediff_start == boost::none || timediff_end == boost::none) // if no time information is provided yet, approximate transition time
next_transition_time += std::abs(z2 - z1) / cfg_->robot.max_vel_x; // Approximate the time, if no time is known
else // otherwise use the time information from the teb trajectory
{
if (std::distance(path_iter, path_end) != std::distance(timediff_iter, timediff_end.get()))
ROS_ERROR("Size of poses and timediff vectors does not match. This is a bug.");
next_transition_time += (*timediff_iter)->dt();
}
Eigen::Vector3d direction_vec;
direction_vec[0] = z2.real() - z1.real();
direction_vec[1] = z2.imag() - z1.imag();
direction_vec[2] = next_transition_time - transition_time;
if(direction_vec.norm() < 1e-15) // Coincident poses
continue;
Eigen::Vector3d r(z1.real(), z1.imag(), transition_time);
Eigen::Vector3d dl = 1.0/static_cast<double>(num_int_steps_per_segment) * direction_vec; // Integrate with multiple steps between each pose
Eigen::Vector3d p1, p2, d, phi;
for (int i = 0; i < num_int_steps_per_segment; ++i, r += dl)
{
p1 = s1 - r;
p2 = s2 - r;
d = (ds.cross(p1.cross(p2))) / ds_sq_norm;
phi = 1.0 / d.squaredNorm() * ((d.cross(p2) / p2.norm()) - (d.cross(p1) / p1.norm()));
H += phi.dot(dl);
}
}
// normalize to 1
hsignature3d_.at(l) = H/(4.0*M_PI);
}
}
/**
* @brief Check if two candidate classes are equivalent
*
* If the absolute value of the H-Signature is equal or greater than 1, a loop (in x-y) around the obstacle is indicated.
* Positive H-Signature: Obstacle lies on the left hand side of the planned trajectory
* Negative H-Signature: Obstacle lies on the right hand side of the planned trajectory
* H-Signature equals zero: Obstacle lies in infinity, has no influence on trajectory
*
* @param other The other equivalence class to test with
*/
virtual bool isEqual(const EquivalenceClass& other) const
{
const HSignature3d* hother = dynamic_cast<const HSignature3d*>(&other); // TODO: better architecture without dynamic_cast
if (hother)
{
if (hsignature3d_.size() == hother->hsignature3d_.size())
{
for(size_t i = 0; i < hsignature3d_.size(); ++i)
{
// If the H-Signature for one obstacle is below this threshold, that obstacle is far away from the planned trajectory,
// and therefore ignored in the homotopy class planning
if (std::abs(hother->hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold || std::abs(hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold)
continue;
if (boost::math::sign(hother->hsignature3d_.at(i)) != boost::math::sign(hsignature3d_.at(i)))
return false; // Signatures are not equal, new homotopy class
}
return true; // Found! Homotopy class already exists, therefore nothing added
}
}
else
ROS_ERROR("Cannot compare HSignature3d equivalence classes with types other than HSignature3d.");
return false;
}
/**
* @brief Check if the equivalence value is detected correctly
* @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
*/
virtual bool isValid() const
{
for(const double& value : hsignature3d_)
{
if (!std::isfinite(value))
return false;
}
return true;
}
/**
* @brief Check if the trajectory is non-looping around any obstacle. Values greater than 1 indicate a looping trajectory.
* @return Returns false, if the trajectory loops around an obstacle
*/
virtual bool isReasonable() const
{
for(const double& value : hsignature3d_)
{
if (value > 1.0)
return false;
}
return true;
}
/**
* @brief Get the current h-signature (read-only)
* @return h-signature in complex-number format
*/
const std::vector<double>& values() const {return hsignature3d_;}
private:
const TebConfig* cfg_;
std::vector<double> hsignature3d_;
};
} // namespace teb_local_planner
#endif /* H_SIGNATURE_H_ */

593
navigations/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h Executable file
View File

@@ -0,0 +1,593 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef HOMOTOPY_CLASS_PLANNER_H_
#define HOMOTOPY_CLASS_PLANNER_H_
#include <math.h>
#include <algorithm>
#include <functional>
#include <vector>
#include <iterator>
#include <random>
#include <boost/shared_ptr.hpp>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/ColorRGBA.h>
#include <ros/console.h>
#include <ros/ros.h>
#include <teb_local_planner/planner_interface.h>
#include <teb_local_planner/teb_config.h>
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/optimal_planner.h>
#include <teb_local_planner/visualization.h>
#include <teb_local_planner/robot_footprint_model.h>
#include <teb_local_planner/equivalence_relations.h>
#include <teb_local_planner/graph_search.h>
namespace teb_local_planner
{
//!< Inline function used for calculateHSignature() in combination with VertexPose pointers
inline std::complex<long double> getCplxFromVertexPosePtr(const VertexPose* pose)
{
return std::complex<long double>(pose->x(), pose->y());
};
//!< Inline function used for calculateHSignature() in combination with geometry_msgs::PoseStamped
inline std::complex<long double> getCplxFromMsgPoseStamped(const geometry_msgs::PoseStamped& pose)
{
return std::complex<long double>(pose.pose.position.x, pose.pose.position.y);
};
/**
* @class HomotopyClassPlanner
* @brief Local planner that explores alternative homotopy classes, create a plan for each alternative
* and finally return the robot controls for the current best path (repeated in each sampling interval)
*
* Equivalence classes (e.g. homotopy) are explored using the help of a search-graph. \n
* A couple of possible candidates are sampled / generated and filtered afterwards such that only a single candidate
* per homotopy class remain. Filtering is applied using the H-Signature, a homotopy (resp. homology) invariant: \n
* - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
* - C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.
*
* Followed by the homotopy class search, each candidate is used as an initialization for the underlying trajectory
* optimization (in this case utilizing the TebOptimalPlanner class with the TimedElasticBand). \n
* Depending on the config parameters, the optimization is performed in parallel. \n
* After the optimization is completed, the best optimized candidate is selected w.r.t. to trajectory cost, since the
* cost already contains important features like clearance from obstacles and transition time. \n
*
* Everyhting is performed by calling one of the overloaded plan() methods. \n
* Afterwards the velocity command to control the robot is obtained from the "best" candidate
* via getVelocityCommand(). \n
*
* All steps are repeated in the subsequent sampling interval with the exception, that already planned (optimized) trajectories
* are preferred against new path initilizations in order to improve the hot-starting capability.
*/
class HomotopyClassPlanner : public PlannerInterface
{
public:
using EquivalenceClassContainer = std::vector< std::pair<EquivalenceClassPtr, bool> >;
/**
* @brief Default constructor
*/
HomotopyClassPlanner();
/**
* @brief Construct and initialize the HomotopyClassPlanner
* @param cfg Const reference to the TebConfig class for internal parameters
* @param obstacles Container storing all relevant obstacles (see Obstacle)
* @param robot_model Shared pointer to the robot shape model used for optimization (optional)
* @param visualization Shared pointer to the TebVisualization class (optional)
* @param via_points Container storing via-points (optional)
*/
HomotopyClassPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
/**
* @brief Destruct the HomotopyClassPlanner.
*/
virtual ~HomotopyClassPlanner();
/**
* @brief Initialize the HomotopyClassPlanner
* @param cfg Const reference to the TebConfig class for internal parameters
* @param obstacles Container storing all relevant obstacles (see Obstacle)
* @param robot_model Shared pointer to the robot shape model used for optimization (optional)
* @param visualization Shared pointer to the TebVisualization class (optional)
* @param via_points Container storing via-points (optional)
*/
void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
void updateRobotModel(RobotFootprintModelPtr robot_model );
/** @name Plan a trajectory */
//@{
/**
* @brief Plan a trajectory based on an initial reference plan.
*
* Provide this method to create and optimize a trajectory that is initialized
* according to an initial reference plan (given as a container of poses).
* @warning The current implementation extracts only the start and goal pose and calls the overloaded plan()
* @param initial_plan vector of geometry_msgs::PoseStamped (must be valid until clearPlanner() is called!)
* @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
/**
* @brief Plan a trajectory between a given start and goal pose (tf::Pose version).
*
* Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
* @param start tf::Pose containing the start pose of the trajectory
* @param goal tf::Pose containing the goal pose of the trajectory
* @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
/**
* @brief Plan a trajectory between a given start and goal pose.
*
* Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
* @param start PoseSE2 containing the start pose of the trajectory
* @param goal PoseSE2 containing the goal pose of the trajectory
* @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity).
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
/**
* @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
* @warning Call plan() first and check if the generated plan is feasible.
* @param[out] vx translational velocity [m/s]
* @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s]
* @param[out] omega rotational velocity [rad/s]
* @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
* @return \c true if command is valid, \c false otherwise
*/
virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const;
/**
* @brief Access current best trajectory candidate (that relates to the "best" homotopy class).
*
* If no trajectory is available, the pointer will be empty.
* If only a single trajectory is available, return it.
* Otherwise return the best one, but call selectBestTeb() before to perform the actual selection (part of the plan() methods).
* @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
*/
TebOptimalPlannerPtr bestTeb() const {return tebs_.empty() ? TebOptimalPlannerPtr() : tebs_.size()==1 ? tebs_.front() : best_teb_;}
/**
* @brief Check whether the planned trajectory is feasible or not.
*
* This method currently checks only that the trajectory, or a part of the trajectory is collision free.
* Obstacles are here represented as costmap instead of the internal ObstacleContainer.
* @param costmap_model Pointer to the costmap model
* @param footprint_spec The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
* @return \c true, if the robot footprint along the first part of the trajectory intersects with
* any obstacle in the costmap, \c false otherwise.
*/
virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector<geometry_msgs::Point>& footprint_spec,
double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0);
/**
* @brief In case of empty best teb, scores again the available plans to find the best one.
* The best_teb_ variable is updated consequently.
* @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
* An empty pointer is returned if no plan is available.
*/
TebOptimalPlannerPtr findBestTeb();
/**
* @brief Removes the specified teb and the corresponding homotopy class from the list of available ones.
* @param pointer to the teb Band to be removed
* @return Iterator to the next valid teb if available, else to the end of the tebs container.
*/
TebOptPlannerContainer::iterator removeTeb(TebOptimalPlannerPtr& teb);
//@}
/** @name Visualization */
//@{
/**
* @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
* @param visualization shared pointer to a TebVisualization instance
* @see visualizeTeb
*/
void setVisualization(TebVisualizationPtr visualization);
/**
* @brief Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz).
*
* Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.
* @see setVisualization
*/
virtual void visualize();
//@}
/** @name Important steps that are called during planning */
//@{
/**
* @brief Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them.
*
* This "all-in-one" method creates a graph with position keypoints from which
* feasible paths (with clearance from obstacles) are extracted. \n
* All obtained paths are filted to only keep a single path for each equivalence class. \n
* Each time a new equivalence class is explored (by means of \b no previous trajectory/TEB remain in that equivalence class),
* a new trajectory/TEB will be initialized. \n
*
* Everything is prepared now for the optimization step: see optimizeAllTEBs().
* @param start Current start pose (e.g. pose of the robot)
* @param goal Goal pose (e.g. robot's goal)
* @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).
* @param @param start_velocity start velocity (optional)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
*/
void exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::Twist* start_vel, bool free_goal_vel = false);
/**
* @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it using a generic 2D reference path
*
* Refer to TimedElasticBand::initTEBtoGoal() for more details about the template parameters.
* @param path_start start iterator of a generic 2d path
* @param path_end end iterator of a generic 2d path
* @param fun_position unary function that returns the Eigen::Vector2d object
* @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
* @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
* @param start_velocity start velocity (optional)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
* @tparam BidirIter Bidirectional iterator type
* @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d
* @return Shared pointer to the newly created teb optimal planner
*/
template<typename BidirIter, typename Fun>
TebOptimalPlannerPtr addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false);
/**
* @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it with a simple straight line between a given start and goal
* @param start start pose
* @param goal goal pose
* @param start_velocity start velocity (optional)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
* @return Shared pointer to the newly created teb optimal planner
*/
TebOptimalPlannerPtr addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false);
/**
* @brief Add a new Teb to the internal trajectory container , if this teb constitutes a new equivalence class. Initialize it using a PoseStamped container
* @param initial_plan container of poses (start and goal orientation should be valid!)
* @param start_velocity start velocity (optional)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
* @return Shared pointer to the newly created teb optimal planner
*/
TebOptimalPlannerPtr addAndInitNewTeb(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false);
/**
* @brief Update TEBs with new pose, goal and current velocity.
* @param start New start pose (optional)
* @param goal New goal pose (optional)
* @param start_velocity start velocity (optional)
*/
void updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::Twist* start_velocity);
/**
* @brief Optimize all available trajectories by invoking the optimizer on each one.
*
* Depending on the configuration parameters, the optimization is performed either single or multi threaded.
* @param iter_innerloop Number of inner iterations (see TebOptimalPlanner::optimizeTEB())
* @param iter_outerloop Number of outer iterations (see TebOptimalPlanner::optimizeTEB())
*/
void optimizeAllTEBs(int iter_innerloop, int iter_outerloop);
/**
* @brief Returns a shared pointer to the TEB related to the initial plan
* @return A non-empty shared ptr is returned if a match was found; Otherwise the shared ptr is empty.
*/
TebOptimalPlannerPtr getInitialPlanTEB();
/**
* @brief In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values.
*
* The trajectory cost includes features such as transition time and clearance from obstacles. \n
* The best trajectory can be accessed later by bestTeb() within the current sampling interval in order to avoid unessary recalculations.
* @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
*/
TebOptimalPlannerPtr selectBestTeb();
//@}
/**
* @brief Reset the planner.
*
* Clear all previously found H-signatures, paths, tebs and the hcgraph.
*/
virtual void clearPlanner() {clearGraph(); equivalence_classes_.clear(); tebs_.clear(); initial_plan_ = NULL;}
/**
* @brief Prefer a desired initial turning direction (by penalizing the opposing one)
*
* A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
* solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
* Initial means that the penalty is applied only to the first few poses of the trajectory.
* @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
*/
virtual void setPreferredTurningDir(RotType dir);
/**
* @brief Calculate the equivalence class of a path
*
* Currently, only the H-signature (refer to HSignature) is implemented.
*
* @param path_start Iterator to the first element in the path
* @param path_end Iterator to the last element in the path
* @param obstacles obstacle container
* @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
* @tparam BidirIter Bidirectional iterator type
* @tparam Fun function of the form std::complex< long double > (const T& point_type)
* @return pointer to the equivalence class base type
*/
template<typename BidirIter, typename Fun>
EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles = NULL,
boost::optional<TimeDiffSequence::iterator> timediff_start = boost::none, boost::optional<TimeDiffSequence::iterator> timediff_end = boost::none);
/**
* @brief Read-only access to the internal trajectory container.
* @return read-only reference to the teb container.
*/
const TebOptPlannerContainer& getTrajectoryContainer() const {return tebs_;}
bool hasDiverged() const override;
/**
* Compute and return the cost of the current optimization graph (supports multiple trajectories)
* @param[out] cost current cost value for each trajectory
* [for a planner with just a single trajectory: size=1, vector will not be cleared]
* @param obst_cost_scale Specify extra scaling for obstacle costs
* @param viapoint_cost_scale Specify extra scaling for via points.
* @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
*/
virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
/**
* @brief Check if two h-signatures are similar (w.r.t. a certain threshold)
* @param h1 first h-signature
* @param h2 second h-signature
* @return \c true if both h-signatures are similar, false otherwise.
*/
inline static bool isHSignatureSimilar(const std::complex<long double>& h1, const std::complex<long double>& h2, double threshold)
{
double diff_real = std::abs(h2.real() - h1.real());
double diff_imag = std::abs(h2.imag() - h1.imag());
if (diff_real<=threshold && diff_imag<=threshold)
return true; // Found! Homotopy class already exists, therefore nothing added
return false;
}
/**
* @brief Checks if the orientation of the computed trajectories differs from that of the best plan of more than the
* specified threshold and eventually deletes them.
* Also deletes detours with a duration much bigger than the duration of the best_teb (duration / best duration > max_ratio_detours_duration_best_duration).
* @param orient_threshold: Threshold paramter for allowed orientation changes in radians
* @param len_orientation_vector: length of the vector used to compute the start orientation
*/
void deletePlansDetouringBackwards(const double orient_threshold, const double len_orientation_vector);
/**
* @brief Given a plan, computes its start orientation using a vector of length >= len_orientation_vector
* starting from the initial pose.
* @param plan: Teb to be analyzed
* @param len_orientation_vector: min length of the vector used to compute the start orientation
* @param orientation: computed start orientation
* @return: Could the vector for the orientation check be computed? (False if the plan has no pose with a distance
* > len_orientation_vector from the start poseq)
*/
bool computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, double& orientation);
/**
* @brief Access config (read-only)
* @return const pointer to the config instance
*/
const TebConfig* config() const {return cfg_;}
/**
* @brief Access current obstacle container (read-only)
* @return const pointer to the obstacle container instance
*/
const ObstContainer* obstacles() const {return obstacles_;}
/**
* @brief Returns true if the planner is initialized
*/
bool isInitialized() const {return initialized_;}
/**
* @brief Clear any existing graph of the homotopy class search
*/
void clearGraph() {if(graph_search_) graph_search_->clearGraph();}
/**
* @brief find the index of the currently best TEB in the container
* @remarks bestTeb() should be preferred whenever possible
* @return index of the best TEB obtained with bestTEB(), if no TEB is avaiable, it returns -1.
*/
int bestTebIdx() const;
/**
* @brief Internal helper function that adds a new equivalence class to the list of known classes only if it is unique.
* @param eq_class equivalence class that should be tested
* @param lock if \c true, exclude the H-signature from deletion.
* @return \c true if the h-signature was added and no duplicate was found, \c false otherwise
*/
bool addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock=false);
/**
* @brief Return the current set of equivalence erelations (read-only)
* @return reference to the internal set of currently tracked equivalence relations
*/
const EquivalenceClassContainer& getEquivalenceClassRef() const {return equivalence_classes_;}
bool isInBestTebClass(const EquivalenceClassPtr& eq_class) const;
int numTebsInClass(const EquivalenceClassPtr& eq_class) const;
int numTebsInBestTebClass() const;
/**
* @brief Randomly drop non-optimal TEBs to so we can explore other alternatives
*
* The HCP has a tendency to become "fixated" once its tebs_ list becomes
* fully populated, repeatedly refining and evaluating paths from the same
* few homotopy classes until the robot moves far enough for a teb to become
* invalid. As a result, it can fail to discover a more optimal path. This
* function alleviates this problem by randomly dropping TEBs other than the
* current "best" one with a probability controlled by
* selection_dropping_probability parameter.
*/
void randomlyDropTebs();
protected:
/** @name Explore new paths and keep only a single one for each homotopy class */
//@{
/**
* @brief Check if a h-signature exists already.
* @param eq_class equivalence class that should be tested
* @return \c true if the h-signature is found, \c false otherwise
*/
bool hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const;
/**
* @brief Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can be discarded.
*
* Calling this method in each new planning interval is really important.
* First all old h-signatures are deleted, since they could be invalid for this planning step (obstacle position may changed).
* Afterwards the h-signatures are calculated for each existing TEB/trajectory and is inserted to the list of known h-signatures.
* Doing this is important to prefer already optimized trajectories in contrast to initialize newly explored coarse paths.
* @param delete_detours if this param is \c true, all existing TEBs are cleared from detour-candidates calling deletePlansGoingBackwards().
*/
void renewAndAnalyzeOldTebs(bool delete_detours);
/**
* @brief Associate trajectories with via-points
*
* If \c all_trajectories is true, all trajectory candidates are connected with the set of via_points,
* otherwise only the trajectory sharing the homotopy class of the initial/global plan (and all via-points from alternative trajectories are removed)
* @remarks Requires that the plan method is called with an initial plan provided and that via-points are enabled (config)
* @param all_trajectories see method description
*/
void updateReferenceTrajectoryViaPoints(bool all_trajectories);
//@}
// external objects (store weak pointers)
const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning
const ViaPointContainer* via_points_; //!< Store the current list of via-points
// internal objects (memory management owned)
TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...)
TebOptimalPlannerPtr best_teb_; //!< Store the current best teb.
EquivalenceClassPtr best_teb_eq_class_; //!< Store the equivalence class of the current best teb
RobotFootprintModelPtr robot_model_; //!< Robot model shared instance
const std::vector<geometry_msgs::PoseStamped>* initial_plan_; //!< Store the initial plan if available for a better trajectory initialization
EquivalenceClassPtr initial_plan_eq_class_; //!< Store the equivalence class of the initial plan
TebOptimalPlannerPtr initial_plan_teb_; //!< Store pointer to the TEB related to the initial plan (use method getInitialPlanTEB() since it checks if initial_plan_teb_ is still included in tebs_.)
TebOptPlannerContainer tebs_; //!< Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs
EquivalenceClassContainer equivalence_classes_; //!< Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.
// The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).
boost::shared_ptr<GraphSearchInterface> graph_search_;
ros::Time last_eq_class_switching_time_; //!< Store the time at which the equivalence class changed recently
std::default_random_engine random_;
bool initialized_; //!< Keeps track about the correct initialization of this class
TebOptimalPlannerPtr last_best_teb_; //!< Points to the plan used in the previous control cycle
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//! Abbrev. for a shared pointer of a HomotopyClassPlanner instance.
typedef boost::shared_ptr<HomotopyClassPlanner> HomotopyClassPlannerPtr;
} // namespace teb_local_planner
// include template implementations / definitions
#include <teb_local_planner/homotopy_class_planner.hpp>
#endif /* HOMOTOPY_CLASS_PLANNER_H_ */

95
navigations/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp Executable file
View File

@@ -0,0 +1,95 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/homotopy_class_planner.h>
#include <teb_local_planner/h_signature.h>
namespace teb_local_planner
{
template<typename BidirIter, typename Fun>
EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
{
if(cfg_->obstacles.include_dynamic_obstacles)
{
HSignature3d* H = new HSignature3d(*cfg_);
H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles, timediff_start, timediff_end);
return EquivalenceClassPtr(H);
}
else
{
HSignature* H = new HSignature(*cfg_);
H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles);
return EquivalenceClassPtr(H);
}
}
template<typename BidirIter, typename Fun>
TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity, bool free_goal_vel)
{
TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(*cfg_, obstacles_, robot_model_));
candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples,
cfg_->trajectory.allow_init_with_backwards_motion);
if (start_velocity)
candidate->setVelocityStart(*start_velocity);
EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
if (free_goal_vel)
candidate->setVelocityGoalFree();
if(addEquivalenceClassIfNew(H))
{
tebs_.push_back(candidate);
return tebs_.back();
}
// If the candidate constitutes no new equivalence class, return a null pointer
return TebOptimalPlannerPtr();
}
} // namespace teb_local_planner

152
navigations/teb_local_planner/include/teb_local_planner/misc.h Executable file
View File

@@ -0,0 +1,152 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef MISC_H
#define MISC_H
#include <Eigen/Core>
#include <boost/utility.hpp>
#include <boost/type_traits.hpp>
namespace teb_local_planner
{
#define SMALL_NUM 0.00000001
//! Symbols for left/none/right rotations
enum class RotType { left, none, right };
/**
* @brief Check whether two variables (double) are close to each other
* @param a the first value to compare
* @param b the second value to compare
* @param epsilon precision threshold
* @return \c true if |a-b| < epsilon, false otherwise
*/
inline bool is_close(double a, double b, double epsilon = 1e-4)
{
return std::fabs(a - b) < epsilon;
}
/**
* @brief Return the average angle of an arbitrary number of given angles [rad]
* @param angles vector containing all angles
* @return average / mean angle, that is normalized to [-pi, pi]
*/
inline double average_angles(const std::vector<double>& angles)
{
double x=0, y=0;
for (std::vector<double>::const_iterator it = angles.begin(); it!=angles.end(); ++it)
{
x += cos(*it);
y += sin(*it);
}
if(x == 0 && y == 0)
return 0;
else
return std::atan2(y, x);
}
/** @brief Small helper function: check if |a|<|b| */
inline bool smaller_than_abs(double i, double j) {return std::fabs(i)<std::fabs(j);}
/**
* @brief Calculate a fast approximation of a sigmoid function
* @details The following function is implemented: \f$ x / (1 + |x|) \f$
* @param x the argument of the function
*/
inline double fast_sigmoid(double x)
{
return x / (1 + fabs(x));
}
/**
* @brief Calculate Euclidean distance between two 2D point datatypes
* @param point1 object containing fields x and y
* @param point2 object containing fields x and y
* @return Euclidean distance: ||point2-point1||
*/
template <typename P1, typename P2>
inline double distance_points2d(const P1& point1, const P2& point2)
{
return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) );
}
/**
* @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d)
* @param v1 object containing public methods x() and y()
* @param v2 object containing fields x() and y()
* @return magnitude that would result in the 3D case (along the z-axis)
*/
template <typename V1, typename V2>
inline double cross2d(const V1& v1, const V2& v2)
{
return v1.x()*v2.y() - v2.x()*v1.y();
}
/**
* @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
*
* Return a constant reference for boths input variants (pointer or reference).
* @remarks Makes only sense in combination with the overload getConstReference(const T& val).
* @param ptr pointer of type T
* @tparam T arbitrary type
* @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
*/
template<typename T>
inline const T& get_const_reference(const T* ptr) {return *ptr;}
/**
* @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
*
* Return a constant reference for boths input variants (pointer or reference).
* @remarks Makes only sense in combination with the overload getConstReference(const T* val).
* @param val
* @param dummy SFINAE helper variable
* @tparam T arbitrary type
* @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
*/
template<typename T>
inline const T& get_const_reference(const T& val, typename boost::disable_if<boost::is_pointer<T> >::type* dummy = 0) {return val;}
} // namespace teb_local_planner
#endif /* MISC_H */

1116
navigations/teb_local_planner/include/teb_local_planner/obstacles.h Executable file
View File

File diff suppressed because it is too large Load Diff

714
navigations/teb_local_planner/include/teb_local_planner/optimal_planner.h Executable file
View File

@@ -0,0 +1,714 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef OPTIMAL_PLANNER_H_
#define OPTIMAL_PLANNER_H_
#include <math.h>
// teb stuff
#include <teb_local_planner/teb_config.h>
#include <teb_local_planner/misc.h>
#include <teb_local_planner/timed_elastic_band.h>
#include <teb_local_planner/planner_interface.h>
#include <teb_local_planner/visualization.h>
#include <teb_local_planner/robot_footprint_model.h>
// g2o lib stuff
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
// messages
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_datatypes.h>
#include <teb_local_planner/TrajectoryMsg.h>
#include <nav_msgs/Odometry.h>
#include <limits.h>
namespace teb_local_planner
{
//! Typedef for the block solver utilized for optimization
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > TEBBlockSolver;
//! Typedef for the linear solver utilized for optimization
typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
//typedef g2o::LinearSolverCholmod<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
//! Typedef for a container storing via-points
typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ViaPointContainer;
/**
* @class TebOptimalPlanner
* @brief This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.
*
* For an introduction and further details about the TEB optimization problem refer to:
* - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
* - C. Rösmann et al.: Efficient trajectory optimization using a sparse model, ECMR, 2013.
* - R. Kümmerle et al.: G2o: A general framework for graph optimization, ICRA, 2011.
*
* @todo: Call buildGraph() only if the teb structure has been modified to speed up hot-starting from previous solutions.
* @todo: We introduced the non-fast mode with the support of dynamic obstacles
* (which leads to better results in terms of x-y-t homotopy planning).
* However, we have not tested this mode intensively yet, so we keep
* the legacy fast mode as default until we finish our tests.
*/
class TebOptimalPlanner : public PlannerInterface
{
public:
/**
* @brief Default constructor
*/
TebOptimalPlanner();
/**
* @brief Construct and initialize the TEB optimal planner.
* @param cfg Const reference to the TebConfig class for internal parameters
* @param obstacles Container storing all relevant obstacles (see Obstacle)
* @param robot_model Shared pointer to the robot shape model used for optimization (optional)
* @param visual Shared pointer to the TebVisualization class (optional)
* @param via_points Container storing via-points (optional)
*/
TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
/**
* @brief Destruct the optimal planner.
*/
virtual ~TebOptimalPlanner();
/**
* @brief Initializes the optimal planner
* @param cfg Const reference to the TebConfig class for internal parameters
* @param obstacles Container storing all relevant obstacles (see Obstacle)
* @param robot_model Shared pointer to the robot shape model used for optimization (optional)
* @param visual Shared pointer to the TebVisualization class (optional)
* @param via_points Container storing via-points (optional)
*/
void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
/**
* @param robot_model Shared pointer to the robot shape model used for optimization (optional)
*/
void updateRobotModel(RobotFootprintModelPtr robot_model );
/** @name Plan a trajectory */
//@{
/**
* @brief Plan a trajectory based on an initial reference plan.
*
* Call this method to create and optimize a trajectory that is initialized
* according to an initial reference plan (given as a container of poses). \n
* The method supports hot-starting from previous solutions, if avaiable: \n
* - If no trajectory exist yet, a new trajectory is initialized based on the initial plan,
* see TimedElasticBand::initTEBtoGoal
* - If a previous solution is avaiable, update the trajectory based on the initial plan,
* see bool TimedElasticBand::updateAndPruneTEB
* - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
* @param initial_plan vector of geometry_msgs::PoseStamped
* @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
/**
* @brief Plan a trajectory between a given start and goal pose (tf::Pose version)
*
* Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n
* The method supports hot-starting from previous solutions, if avaiable: \n
* - If no trajectory exist yet, a new trajectory is initialized between start and goal poses,
* see TimedElasticBand::initTEBtoGoal
* - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB
* - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
* @param start tf::Pose containing the start pose of the trajectory
* @param goal tf::Pose containing the goal pose of the trajectory
* @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
/**
* @brief Plan a trajectory between a given start and goal pose
*
* Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n
* The method supports hot-starting from previous solutions, if avaiable: \n
* - If no trajectory exist yet, a new trajectory is initialized between start and goal poses
* @see TimedElasticBand::initTEBtoGoal
* - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB
* - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o
* @param start PoseSE2 containing the start pose of the trajectory
* @param goal PoseSE2 containing the goal pose of the trajectory
* @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity).
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
/**
* @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
* @warning Call plan() first and check if the generated plan is feasible.
* @param[out] vx translational velocity [m/s]
* @param[out] vy strafing velocity which can be nonzero for holonomic robots[m/s]
* @param[out] omega rotational velocity [rad/s]
* @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
* @return \c true if command is valid, \c false otherwise
*/
virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const;
/**
* @brief Optimize a previously initialized trajectory (actual TEB optimization loop).
*
* optimizeTEB implements the main optimization loop. \n
* It consist of two nested loops:
* - The outer loop resizes the trajectory according to the temporal resolution by invoking TimedElasticBand::autoResize().
* Afterwards the internal method optimizeGraph() is called that constitutes the innerloop.
* - The inner loop calls the solver (g2o framework, resp. sparse Levenberg-Marquardt) and iterates a specified
* number of optimization calls (\c iterations_innerloop).
*
* The outer loop is repeated \c iterations_outerloop times. \n
* The ratio of inner and outer loop iterations significantly defines the contraction behavior
* and convergence rate of the trajectory optimization. Based on our experiences, 2-6 innerloop iterations are sufficient. \n
* The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate. \n
* Optionally, the cost vector can be calculated by specifying \c compute_cost_afterwards, see computeCurrentCost().
* @remarks This method is usually called from a plan() method
* @param iterations_innerloop Number of iterations for the actual solver loop
* @param iterations_outerloop Specifies how often the trajectory should be resized followed by the inner solver loop.
* @param compute_cost_afterwards if \c true Calculate the cost vector according to computeCurrentCost(),
* the vector can be accessed afterwards using getCurrentCost().
* @param obst_cost_scale Specify extra scaling for obstacle costs (only used if \c compute_cost_afterwards is true)
* @param viapoint_cost_scale Specify extra scaling for via-point costs (only used if \c compute_cost_afterwards is true)
* @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
* (only used if \c compute_cost_afterwards is true).
* @return \c true if the optimization terminates successfully, \c false otherwise
*/
bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false,
double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
//@}
/** @name Desired initial and final velocity */
//@{
/**
* @brief Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload].
* @remarks Calling this function is not neccessary if the initial velocity is passed via the plan() method
* @param vel_start Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used,
* for holonomic robots also linear.y)
*/
void setVelocityStart(const geometry_msgs::Twist& vel_start);
/**
* @brief Set the desired final velocity at the trajectory's goal pose.
* @remarks Call this function only if a non-zero velocity is desired and if \c free_goal_vel is set to \c false in plan()
* @param vel_goal twist message containing the translational and angular final velocity
*/
void setVelocityGoal(const geometry_msgs::Twist& vel_goal);
/**
* @brief Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit
* @remarks Calling this function is not neccessary if \c free_goal_vel is set to \c false in plan()
*/
void setVelocityGoalFree() {vel_goal_.first = false;}
//@}
/** @name Take obstacles into account */
//@{
/**
* @brief Assign a new set of obstacles
* @param obst_vector pointer to an obstacle container (can also be a nullptr)
* @remarks This method overrids the obstacle container optinally assigned in the constructor.
*/
void setObstVector(ObstContainer* obst_vector) {obstacles_ = obst_vector;}
/**
* @brief Access the internal obstacle container.
* @return Const reference to the obstacle container
*/
const ObstContainer& getObstVector() const {return *obstacles_;}
//@}
/** @name Take via-points into account */
//@{
/**
* @brief Assign a new set of via-points
* @param via_points pointer to a via_point container (can also be a nullptr)
* @details Any previously set container will be overwritten.
*/
void setViaPoints(const ViaPointContainer* via_points) {via_points_ = via_points;}
/**
* @brief Access the internal via-point container.
* @return Const reference to the via-point container
*/
const ViaPointContainer& getViaPoints() const {return *via_points_;}
//@}
/** @name Visualization */
//@{
/**
* @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
* @param visualization shared pointer to a TebVisualization instance
* @see visualize
*/
void setVisualization(TebVisualizationPtr visualization);
/**
* @brief Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz).
*
* Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.
* @see setVisualization
*/
virtual void visualize();
//@}
/** @name Utility methods and more */
//@{
/**
* @brief Reset the planner by clearing the internal graph and trajectory.
*/
virtual void clearPlanner()
{
clearGraph();
teb_.clearTimedElasticBand();
}
/**
* @brief Prefer a desired initial turning direction (by penalizing the opposing one)
*
* A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
* solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
* Initial means that the penalty is applied only to the first few poses of the trajectory.
* @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
*/
virtual void setPreferredTurningDir(RotType dir) {prefer_rotdir_=dir;}
/**
* @brief Register the vertices and edges defined for the TEB to the g2o::Factory.
*
* This allows the user to export the internal graph to a text file for instance.
* Access the optimizer() for more details.
*/
static void registerG2OTypes();
/**
* @brief Access the internal TimedElasticBand trajectory.
* @warning In general, the underlying teb must not be modified directly. Use with care...
* @return reference to the teb
*/
TimedElasticBand& teb() {return teb_;};
/**
* @brief Access the internal TimedElasticBand trajectory (read-only).
* @return const reference to the teb
*/
const TimedElasticBand& teb() const {return teb_;};
/**
* @brief Access the internal g2o optimizer.
* @warning In general, the underlying optimizer must not be modified directly. Use with care...
* @return const shared pointer to the g2o sparse optimizer
*/
boost::shared_ptr<g2o::SparseOptimizer> optimizer() {return optimizer_;};
/**
* @brief Access the internal g2o optimizer (read-only).
* @return const shared pointer to the g2o sparse optimizer
*/
boost::shared_ptr<const g2o::SparseOptimizer> optimizer() const {return optimizer_;};
/**
* @brief Check if last optimization was successful
* @return \c true if the last optimization returned without errors,
* otherwise \c false (also if no optimization has been called before).
*/
bool isOptimized() const {return optimized_;};
/**
* @brief Returns true if the planner has diverged.
*/
bool hasDiverged() const override;
/**
* @brief Compute the cost vector of a given optimization problen (hyper-graph must exist).
*
* Use this method to obtain information about the current edge errors / costs (local cost functions). \n
* The vector of cost values is composed according to the different edge types (time_optimal, obstacles, ...). \n
* Refer to the method declaration for the detailed composition. \n
* The cost for the edges that minimize time differences (EdgeTimeOptimal) corresponds to the sum of all single
* squared time differneces: \f$ \sum_i \Delta T_i^2 \f$. Sometimes, the user may want to get a value that is proportional
* or identical to the actual trajectory transition time \f$ \sum_i \Delta T_i \f$. \n
* Set \c alternative_time_cost to true in order to get the cost calculated using the latter equation, but check the
* implemented definition, if the value is scaled to match the magnitude of other cost values.
*
* @todo Remove the scaling term for the alternative time cost.
* @todo Can we use the last error (chi2) calculated from g2o instead of calculating it by ourself?
* @see getCurrentCost
* @see optimizeTEB
* @param obst_cost_scale Specify extra scaling for obstacle costs.
* @param viapoint_cost_scale Specify extra scaling for via points.
* @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time.
* @return TebCostVec containing the cost values
*/
void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
/**
* Compute and return the cost of the current optimization graph (supports multiple trajectories)
* @param[out] cost current cost value for each trajectory
* [for a planner with just a single trajectory: size=1, vector will not be cleared]
* @param obst_cost_scale Specify extra scaling for obstacle costs
* @param viapoint_cost_scale Specify extra scaling for via points.
* @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
*/
virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
{
computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
cost.push_back( getCurrentCost() );
}
/**
* @brief Access the cost vector.
*
* The accumulated cost value previously calculated using computeCurrentCost
* or by calling optimizeTEB with enabled cost flag.
* @return const reference to the TebCostVec.
*/
double getCurrentCost() const {return cost_;}
/**
* @brief Extract the velocity from consecutive poses and a time difference (including strafing velocity for holonomic robots)
*
* The velocity is extracted using finite differences.
* The direction of the translational velocity is also determined.
* @param pose1 pose at time k
* @param pose2 consecutive pose at time k+1
* @param dt actual time difference between k and k+1 (must be >0 !!!)
* @param[out] vx translational velocity
* @param[out] vy strafing velocity which can be nonzero for holonomic robots
* @param[out] omega rotational velocity
*/
inline void extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const;
/**
* @brief Compute the velocity profile of the trajectory
*
* This method computes the translational and rotational velocity for the complete
* planned trajectory.
* The first velocity is the one that is provided as initial velocity (fixed).
* Velocities at index k=2...end-1 are related to the transition from pose_{k-1} to pose_k.
* The last velocity is the final velocity (fixed).
* The number of Twist objects is therefore sizePoses()+1;
* In summary:
* v[0] = v_start,
* v[1,...end-1] = +-(pose_{k+1}-pose{k})/dt,
* v(end) = v_goal
* It can be used for evaluation and debugging purposes or
* for open-loop control. For computing the velocity required for controlling the robot
* to the next step refer to getVelocityCommand().
* @param[out] velocity_profile velocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1
*/
void getVelocityProfile(std::vector<geometry_msgs::Twist>& velocity_profile) const;
/**
* @brief Return the complete trajectory including poses, velocity profiles and temporal information
*
* It is useful for evaluation and debugging purposes or for open-loop control.
* Since the velocity obtained using difference quotients is the mean velocity between consecutive poses,
* the velocity at each pose at time stamp k is obtained by taking the average between both velocities.
* The velocity of the first pose is v_start (provided initial value) and the last one is v_goal (usually zero, if free_goal_vel is off).
* See getVelocityProfile() for the list of velocities between consecutive points.
* @todo The acceleration profile is not added at the moment.
* @param[out] trajectory the resulting trajectory
*/
void getFullTrajectory(std::vector<TrajectoryPointMsg>& trajectory) const;
/**
* @brief Check whether the planned trajectory is feasible or not.
*
* This method currently checks only that the trajectory, or a part of the trajectory is collision free.
* Obstacles are here represented as costmap instead of the internal ObstacleContainer.
* @param costmap_model Pointer to the costmap model
* @param footprint_spec The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
* @return \c true, if the robot footprint along the first part of the trajectory intersects with
* any obstacle in the costmap, \c false otherwise.
*/
virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius = 0.0,
double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0);
//@}
protected:
/** @name Hyper-Graph creation and optimization */
//@{
/**
* @brief Build the hyper-graph representing the TEB optimization problem.
*
* This method creates the optimization problem according to the hyper-graph formulation. \n
* For more details refer to the literature cited in the TebOptimalPlanner class description.
* @see optimizeGraph
* @see clearGraph
* @param weight_multiplier Specify a weight multipler for selected weights in optimizeGraph
* This might be used for weight adapation strategies.
* Currently, only obstacle collision weights are considered.
* @return \c true, if the graph was created successfully, \c false otherwise.
*/
bool buildGraph(double weight_multiplier=1.0);
/**
* @brief Optimize the previously constructed hyper-graph to deform / optimize the TEB.
*
* This method invokes the g2o framework to solve the optimization problem considering dedicated sparsity patterns. \n
* The current implementation calls a non-constrained sparse Levenberg-Marquardt algorithm. Constraints are considered
* by utilizing penalty approximations. Refer to the literature cited in the TebOptimalPlanner class description.
* @see buildGraph
* @see clearGraph
* @param no_iterations Number of solver iterations
* @param clear_after Clear the graph after optimization.
* @return \c true, if optimization terminates successfully, \c false otherwise.
*/
bool optimizeGraph(int no_iterations, bool clear_after=true);
/**
* @brief Clear an existing internal hyper-graph.
* @see buildGraph
* @see optimizeGraph
*/
void clearGraph();
/**
* @brief Add all relevant vertices to the hyper-graph as optimizable variables.
*
* Vertices (if unfixed) represent the variables that will be optimized. \n
* In case of the Timed-Elastic-Band poses and time differences form the vertices of the hyper-graph. \n
* The order of insertion of vertices (to the graph) is important for efficiency,
* since it affect the sparsity pattern of the underlying hessian computed for optimization.
* @see VertexPose
* @see VertexTimeDiff
* @see buildGraph
* @see optimizeGraph
*/
void AddTEBVertices();
/**
* @brief Add all edges (local cost functions) for limiting the translational and angular velocity.
* @see EdgeVelocity
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesVelocity();
/**
* @brief Add all edges (local cost functions) for limiting the translational and angular acceleration.
* @see EdgeAcceleration
* @see EdgeAccelerationStart
* @see EdgeAccelerationGoal
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesAcceleration();
/**
* @brief Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences)
* @see EdgeTimeOptimal
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesTimeOptimal();
/**
* @brief Add all edges (local cost functions) for minimizing the path length
* @see EdgeShortestPath
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesShortestPath();
/**
* @brief Add all edges (local cost functions) related to keeping a distance from static obstacles
* @warning do not combine with AddEdgesInflatedObstacles
* @see EdgeObstacle
* @see buildGraph
* @see optimizeGraph
* @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
*/
void AddEdgesObstacles(double weight_multiplier=1.0);
/**
* @brief Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy)
* @see EdgeObstacle
* @see buildGraph
* @see optimizeGraph
* @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
*/
void AddEdgesObstaclesLegacy(double weight_multiplier=1.0);
/**
* @brief Add all edges (local cost functions) related to minimizing the distance to via-points
* @see EdgeViaPoint
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesViaPoints();
/**
* @brief Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles.
* @warning experimental
* @todo Should we also add neighbors to decrease jiggling/oscillations
* @see EdgeDynamicObstacle
* @see buildGraph
* @see optimizeGraph
* @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight)
*/
void AddEdgesDynamicObstacles(double weight_multiplier=1.0);
/**
* @brief Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot
* @warning do not combine with AddEdgesKinematicsCarlike()
* @see AddEdgesKinematicsCarlike
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesKinematicsDiffDrive();
/**
* @brief Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot
* @warning do not combine with AddEdgesKinematicsDiffDrive()
* @see AddEdgesKinematicsDiffDrive
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesKinematicsCarlike();
/**
* @brief Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one)
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesPreferRotDir();
/**
* @brief Add all edges (local cost function) for reducing the velocity of a vertex due to its associated obstacles
* @see buildGraph
* @see optimizeGraph
*/
void AddEdgesVelocityObstacleRatio();
//@}
/**
* @brief Initialize and configure the g2o sparse optimizer.
* @return shared pointer to the g2o::SparseOptimizer instance
*/
boost::shared_ptr<g2o::SparseOptimizer> initOptimizer();
// external objects (store weak pointers)
const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning
const ViaPointContainer* via_points_; //!< Store via points for planning
std::vector<ObstContainer> obstacles_per_vertex_; //!< Store the obstacles associated with the n-1 initial vertices
double cost_; //!< Store cost value of the current hyper-graph
RotType prefer_rotdir_; //!< Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates)
// internal objects (memory management owned)
TebVisualizationPtr visualization_; //!< Instance of the visualization class
TimedElasticBand teb_; //!< Actual trajectory object
RobotFootprintModelPtr robot_model_; //!< Robot model
boost::shared_ptr<g2o::SparseOptimizer> optimizer_; //!< g2o optimizer for trajectory optimization
std::pair<bool, geometry_msgs::Twist> vel_start_; //!< Store the initial velocity at the start pose
std::pair<bool, geometry_msgs::Twist> vel_goal_; //!< Store the final velocity at the goal pose
bool initialized_; //!< Keeps track about the correct initialization of this class
bool optimized_; //!< This variable is \c true as long as the last optimization has been completed successful
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//! Abbrev. for shared instances of the TebOptimalPlanner
typedef boost::shared_ptr<TebOptimalPlanner> TebOptimalPlannerPtr;
//! Abbrev. for shared const TebOptimalPlanner pointers
typedef boost::shared_ptr<const TebOptimalPlanner> TebOptimalPlannerConstPtr;
//! Abbrev. for containers storing multiple teb optimal planners
typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer;
} // namespace teb_local_planner
#endif /* OPTIMAL_PLANNER_H_ */

208
navigations/teb_local_planner/include/teb_local_planner/planner_interface.h Executable file
View File

@@ -0,0 +1,208 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef PLANNER_INTERFACE_H_
#define PLANNER_INTERFACE_H_
// boost
#include <boost/shared_ptr.hpp>
// ros
#include <tf/transform_datatypes.h>
#include <base_local_planner/costmap_model.h>
// this package
#include <teb_local_planner/pose_se2.h>
#include <teb_local_planner/robot_footprint_model.h>
// messages
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
namespace teb_local_planner
{
/**
* @class PlannerInterface
* @brief This abstract class defines an interface for local planners
*/
class PlannerInterface
{
public:
/**
* @brief Default constructor
*/
PlannerInterface()
{
}
/**
* @brief Virtual destructor.
*/
virtual ~PlannerInterface()
{
}
/** @name Plan a trajectory */
//@{
/**
* @brief Plan a trajectory based on an initial reference plan.
*
* Provide this method to create and optimize a trajectory that is initialized
* according to an initial reference plan (given as a container of poses).
* @param initial_plan vector of geometry_msgs::PoseStamped
* @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
/**
* @brief Plan a trajectory between a given start and goal pose (tf::Pose version).
*
* Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
* @param start tf::Pose containing the start pose of the trajectory
* @param goal tf::Pose containing the goal pose of the trajectory
* @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used)
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
/**
* @brief Plan a trajectory between a given start and goal pose.
*
* Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
* @param start PoseSE2 containing the start pose of the trajectory
* @param goal PoseSE2 containing the goal pose of the trajectory
* @param start_vel Initial velocity at the start pose (twist msg containing the translational and angular velocity).
* @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
* otherwise the final velocity will be zero (default: false)
* @return \c true if planning was successful, \c false otherwise
*/
virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false) = 0;
/**
* @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
* @warning Call plan() first and check if the generated plan is feasible.
* @param[out] vx translational velocity [m/s]
* @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s]
* @param[out] omega rotational velocity [rad/s]
* @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
* @return \c true if command is valid, \c false otherwise
*/
virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const = 0;
//@}
/**
* @brief Reset the planner.
*/
virtual void clearPlanner() = 0;
/**
* @brief Prefer a desired initial turning direction (by penalizing the opposing one)
*
* A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
* solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
* Initial means that the penalty is applied only to the first few poses of the trajectory.
* @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
*/
virtual void setPreferredTurningDir(RotType dir) {ROS_WARN("setPreferredTurningDir() not implemented for this planner.");}
/**
* @brief Visualize planner specific stuff.
* Overwrite this method to provide an interface to perform all planner related visualizations at once.
*/
virtual void visualize()
{
}
virtual void updateRobotModel(RobotFootprintModelPtr robot_model)
{
}
/**
* @brief Check whether the planned trajectory is feasible or not.
*
* This method currently checks only that the trajectory, or a part of the trajectory is collision free.
* Obstacles are here represented as costmap instead of the internal ObstacleContainer.
* @param costmap_model Pointer to the costmap model
* @param footprint_spec The specification of the footprint of the robot in world coordinates
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
* @return \c true, if the robot footprint along the first part of the trajectory intersects with
* any obstacle in the costmap, \c false otherwise.
*/
virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector<geometry_msgs::Point>& footprint_spec,
double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0) = 0;
/**
* Compute and return the cost of the current optimization graph (supports multiple trajectories)
* @param[out] cost current cost value for each trajectory
* [for a planner with just a single trajectory: size=1, vector will not be cleared]
* @param obst_cost_scale Specify extra scaling for obstacle costs
* @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
*/
virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, bool alternative_time_cost=false)
{
}
/**
* @brief Returns true if the planner has diverged.
*/
virtual bool hasDiverged() const = 0;
};
//! Abbrev. for shared instances of PlannerInterface or it's subclasses
typedef boost::shared_ptr<PlannerInterface> PlannerInterfacePtr;
} // namespace teb_local_planner
#endif /* PLANNER_INTERFACE_H__ */

406
navigations/teb_local_planner/include/teb_local_planner/pose_se2.h Executable file
View File

@@ -0,0 +1,406 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef POSE_SE2_H_
#define POSE_SE2_H_
#include <g2o/stuff/misc.h>
#include <Eigen/Core>
#include <teb_local_planner/misc.h>
#include <geometry_msgs/Pose.h>
#include <tf/transform_datatypes.h>
namespace teb_local_planner
{
/**
* @class PoseSE2
* @brief This class implements a pose in the domain SE2: \f$ \mathbb{R}^2 \times S^1 \f$
* The pose consist of the position x and y and an orientation given as angle theta [-pi, pi].
*/
class PoseSE2
{
public:
/** @name Construct PoseSE2 instances */
///@{
/**
* @brief Default constructor
*/
PoseSE2()
{
setZero();
}
/**
* @brief Construct pose given a position vector and an angle theta
* @param position 2D position vector
* @param theta angle given in rad
*/
PoseSE2(const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
{
_position = position;
_theta = theta;
}
/**
* @brief Construct pose using single components x, y, and the yaw angle
* @param x x-coordinate
* @param y y-coordinate
* @param theta yaw angle in rad
*/
PoseSE2(double x, double y, double theta)
{
_position.coeffRef(0) = x;
_position.coeffRef(1) = y;
_theta = theta;
}
/**
* @brief Construct pose using a geometry_msgs::Pose
* @param pose geometry_msgs::Pose object
*/
PoseSE2(const geometry_msgs::Pose& pose)
{
_position.coeffRef(0) = pose.position.x;
_position.coeffRef(1) = pose.position.y;
_theta = tf::getYaw( pose.orientation );
}
/**
* @brief Construct pose using a tf::Pose
* @param pose tf::Pose object
*/
PoseSE2(const tf::Pose& pose)
{
_position.coeffRef(0) = pose.getOrigin().getX();
_position.coeffRef(1) = pose.getOrigin().getY();
_theta = tf::getYaw( pose.getRotation() );
}
/**
* @brief Copy constructor
* @param pose PoseSE2 instance
*/
PoseSE2(const PoseSE2& pose)
{
_position = pose._position;
_theta = pose._theta;
}
///@}
/**
* @brief Destructs the PoseSE2
*/
~PoseSE2() {}
/** @name Access and modify values */
///@{
/**
* @brief Access the 2D position part
* @see estimate
* @return reference to the 2D position part
*/
Eigen::Vector2d& position() {return _position;}
/**
* @brief Access the 2D position part (read-only)
* @see estimate
* @return const reference to the 2D position part
*/
const Eigen::Vector2d& position() const {return _position;}
/**
* @brief Access the x-coordinate the pose
* @return reference to the x-coordinate
*/
double& x() {return _position.coeffRef(0);}
/**
* @brief Access the x-coordinate the pose (read-only)
* @return const reference to the x-coordinate
*/
const double& x() const {return _position.coeffRef(0);}
/**
* @brief Access the y-coordinate the pose
* @return reference to the y-coordinate
*/
double& y() {return _position.coeffRef(1);}
/**
* @brief Access the y-coordinate the pose (read-only)
* @return const reference to the y-coordinate
*/
const double& y() const {return _position.coeffRef(1);}
/**
* @brief Access the orientation part (yaw angle) of the pose
* @return reference to the yaw angle
*/
double& theta() {return _theta;}
/**
* @brief Access the orientation part (yaw angle) of the pose (read-only)
* @return const reference to the yaw angle
*/
const double& theta() const {return _theta;}
/**
* @brief Set pose to [0,0,0]
*/
void setZero()
{
_position.setZero();
_theta = 0;
}
/**
* @brief Convert PoseSE2 to a geometry_msgs::Pose
* @param[out] pose Pose message
*/
void toPoseMsg(geometry_msgs::Pose& pose) const
{
pose.position.x = _position.x();
pose.position.y = _position.y();
pose.position.z = 0;
pose.orientation = tf::createQuaternionMsgFromYaw(_theta);
}
/**
* @brief Return the unit vector of the current orientation
* @returns [cos(theta), sin(theta))]^T
*/
Eigen::Vector2d orientationUnitVec() const {return Eigen::Vector2d(std::cos(_theta), std::sin(_theta));}
///@}
/** @name Arithmetic operations for which operators are not always reasonable */
///@{
/**
* @brief Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi]
* @param factor scale factor
*/
void scale(double factor)
{
_position *= factor;
_theta = g2o::normalize_theta( _theta*factor );
}
/**
* @brief Increment the pose by adding a double[3] array
* The angle is normalized afterwards
* @param pose_as_array 3D double array [x, y, theta]
*/
void plus(const double* pose_as_array)
{
_position.coeffRef(0) += pose_as_array[0];
_position.coeffRef(1) += pose_as_array[1];
_theta = g2o::normalize_theta( _theta + pose_as_array[2] );
}
/**
* @brief Get the mean / average of two poses and store it in the caller class
* For the position part: 0.5*(x1+x2)
* For the angle: take the angle of the mean direction vector
* @param pose1 first pose to consider
* @param pose2 second pose to consider
*/
void averageInPlace(const PoseSE2& pose1, const PoseSE2& pose2)
{
_position = (pose1._position + pose2._position)/2;
_theta = g2o::average_angle(pose1._theta, pose2._theta);
}
/**
* @brief Get the mean / average of two poses and return the result (static)
* For the position part: 0.5*(x1+x2)
* For the angle: take the angle of the mean direction vector
* @param pose1 first pose to consider
* @param pose2 second pose to consider
* @return mean / average of \c pose1 and \c pose2
*/
static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2)
{
return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );
}
/**
* @brief Rotate pose globally
*
* Compute [pose_x, pose_y] = Rot(\c angle) * [pose_x, pose_y].
* if \c adjust_theta, pose_theta is also rotated by \c angle
* @param angle the angle defining the 2d rotation
* @param adjust_theta if \c true, the orientation theta is also rotated
*/
void rotateGlobal(double angle, bool adjust_theta=true)
{
double new_x = std::cos(angle)*_position.x() - std::sin(angle)*_position.y();
double new_y = std::sin(angle)*_position.x() + std::cos(angle)*_position.y();
_position.x() = new_x;
_position.y() = new_y;
if (adjust_theta)
_theta = g2o::normalize_theta(_theta+angle);
}
///@}
/** @name Operator overloads / Allow some arithmetic operations */
///@{
/**
* @brief Asignment operator
* @param rhs PoseSE2 instance
* @todo exception safe version of the assignment operator
*/
PoseSE2& operator=( const PoseSE2& rhs )
{
if (&rhs != this)
{
_position = rhs._position;
_theta = rhs._theta;
}
return *this;
}
/**
* @brief Compound assignment operator (addition)
* @param rhs addend
*/
PoseSE2& operator+=(const PoseSE2& rhs)
{
_position += rhs._position;
_theta = g2o::normalize_theta(_theta + rhs._theta);
return *this;
}
/**
* @brief Arithmetic operator overload for additions
* @param lhs First addend
* @param rhs Second addend
*/
friend PoseSE2 operator+(PoseSE2 lhs, const PoseSE2& rhs)
{
return lhs += rhs;
}
/**
* @brief Compound assignment operator (subtraction)
* @param rhs value to subtract
*/
PoseSE2& operator-=(const PoseSE2& rhs)
{
_position -= rhs._position;
_theta = g2o::normalize_theta(_theta - rhs._theta);
return *this;
}
/**
* @brief Arithmetic operator overload for subtractions
* @param lhs First term
* @param rhs Second term
*/
friend PoseSE2 operator-(PoseSE2 lhs, const PoseSE2& rhs)
{
return lhs -= rhs;
}
/**
* @brief Multiply pose with scalar and return copy without normalizing theta
* This operator is useful for calculating velocities ...
* @param pose pose to scale
* @param scalar factor to multiply with
* @warning theta is not normalized after multiplying
*/
friend PoseSE2 operator*(PoseSE2 pose, double scalar)
{
pose._position *= scalar;
pose._theta *= scalar;
return pose;
}
/**
* @brief Multiply pose with scalar and return copy without normalizing theta
* This operator is useful for calculating velocities ...
* @param scalar factor to multiply with
* @param pose pose to scale
* @warning theta is not normalized after multiplying
*/
friend PoseSE2 operator*(double scalar, PoseSE2 pose)
{
pose._position *= scalar;
pose._theta *= scalar;
return pose;
}
/**
* @brief Output stream operator
* @param stream output stream
* @param pose to be used
*/
friend std::ostream& operator<< (std::ostream& stream, const PoseSE2& pose)
{
stream << "x: " << pose._position[0] << " y: " << pose._position[1] << " theta: " << pose._theta;
return stream;
}
///@}
private:
Eigen::Vector2d _position;
double _theta;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace teb_local_planner
#endif // POSE_SE2_H_

134
navigations/teb_local_planner/include/teb_local_planner/recovery_behaviors.h Executable file
View File

@@ -0,0 +1,134 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef RECOVERY_BEHAVIORS_H__
#define RECOVERY_BEHAVIORS_H__
#include <boost/circular_buffer.hpp>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
namespace teb_local_planner
{
/**
* @class FailureDetector
* @brief This class implements methods in order to detect if the robot got stucked or is oscillating
*
* The StuckDetector analyzes the last N commanded velocities in order to detect whether the robot
* might got stucked or oscillates between left/right/forward/backwards motions.
*/
class FailureDetector
{
public:
/**
* @brief Default constructor
*/
FailureDetector() {}
/**
* @brief destructor.
*/
~FailureDetector() {}
/**
* @brief Set buffer length (measurement history)
* @param length number of measurements to be kept
*/
void setBufferLength(int length) {buffer_.set_capacity(length);}
/**
* @brief Add a new twist measurement to the internal buffer and compute a new decision
* @param twist geometry_msgs::Twist velocity information
* @param v_max maximum forward translational velocity
* @param v_backwards_max maximum backward translational velocity
* @param omega_max maximum angular velocity
* @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1)
* @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1)
*/
void update(const geometry_msgs::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps);
/**
* @brief Check if the robot got stucked
*
* This call does not compute the actual decision,
* since it is computed within each update() invocation.
* @return true if the robot got stucked, false otherwise.
*/
bool isOscillating() const;
/**
* @brief Clear the current internal state
*
* This call also resets the internal buffer
*/
void clear();
protected:
/** Variables to be monitored */
struct VelMeasurement
{
double v = 0;
double omega = 0;
};
/**
* @brief Detect if the robot got stucked based on the current buffer content
*
* Afterwards the status might be checked using gotStucked();
* @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1)
* @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1)
* @return true if the robot got stucked, false otherwise
*/
bool detect(double v_eps, double omega_eps);
private:
boost::circular_buffer<VelMeasurement> buffer_; //!< Circular buffer to store the last measurements @see setBufferLength
bool oscillating_ = false; //!< Current state: true if robot is oscillating
};
} // namespace teb_local_planner
#endif /* RECOVERY_BEHAVIORS_H__ */

778
navigations/teb_local_planner/include/teb_local_planner/robot_footprint_model.h Executable file
View File

@@ -0,0 +1,778 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef ROBOT_FOOTPRINT_MODEL_H
#define ROBOT_FOOTPRINT_MODEL_H
#include <teb_local_planner/pose_se2.h>
#include <teb_local_planner/obstacles.h>
#include <visualization_msgs/Marker.h>
namespace teb_local_planner
{
/**
* @class BaseRobotFootprintModel
* @brief Abstract class that defines the interface for robot footprint/contour models
*
* The robot model class is currently used in optimization only, since
* taking the navigation stack footprint into account might be
* inefficient. The footprint is only used for checking feasibility.
*/
class BaseRobotFootprintModel
{
public:
/**
* @brief Default constructor of the abstract obstacle class
*/
BaseRobotFootprintModel()
{
}
/**
* @brief Virtual destructor.
*/
virtual ~BaseRobotFootprintModel()
{
}
/**
* @brief Calculate the distance between the robot and an obstacle
* @param current_pose Current robot pose
* @param obstacle Pointer to the obstacle
* @return Euclidean distance to the robot
*/
virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const = 0;
/**
* @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
* @param current_pose robot pose, from which the distance to the obstacle is estimated
* @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
* @param t time, for which the predicted distance to the obstacle is calculated
* @return Euclidean distance to the robot
*/
virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const = 0;
/**
* @brief Visualize the robot using a markers
*
* Fill a marker message with all necessary information (type, pose, scale and color).
* The header, namespace, id and marker lifetime will be overwritten.
* @param current_pose Current robot pose
* @param[out] markers container of marker messages describing the robot shape
* @param color Color of the footprint
*/
virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::Marker>& markers, const std_msgs::ColorRGBA& color) const {}
/**
* @brief Compute the inscribed radius of the footprint model
* @return inscribed radius
*/
virtual double getInscribedRadius() = 0;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//! Abbrev. for shared obstacle pointers
typedef boost::shared_ptr<BaseRobotFootprintModel> RobotFootprintModelPtr;
//! Abbrev. for shared obstacle const pointers
typedef boost::shared_ptr<const BaseRobotFootprintModel> RobotFootprintModelConstPtr;
/**
* @class PointRobotShape
* @brief Class that defines a point-robot
*
* Instead of using a CircularRobotFootprint this class might
* be utitilzed and the robot radius can be added to the mininum distance
* parameter. This avoids a subtraction of zero each time a distance is calculated.
*/
class PointRobotFootprint : public BaseRobotFootprintModel
{
public:
/**
* @brief Default constructor of the abstract obstacle class
*/
PointRobotFootprint() {}
/**
* @brief Default constructor of the abstract obstacle class
* @param min_obstacle_dist Minimum obstacle distance
*/
PointRobotFootprint(const double min_obstacle_dist) : min_obstacle_dist_(min_obstacle_dist) {}
/**
* @brief Virtual destructor.
*/
virtual ~PointRobotFootprint() {}
/**
* @brief Calculate the distance between the robot and an obstacle
* @param current_pose Current robot pose
* @param obstacle Pointer to the obstacle
* @return Euclidean distance to the robot
*/
virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
{
return obstacle->getMinimumDistance(current_pose.position());
}
/**
* @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
* @param current_pose robot pose, from which the distance to the obstacle is estimated
* @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
* @param t time, for which the predicted distance to the obstacle is calculated
* @return Euclidean distance to the robot
*/
virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
{
return obstacle->getMinimumSpatioTemporalDistance(current_pose.position(), t);
}
/**
* @brief Compute the inscribed radius of the footprint model
* @return inscribed radius
*/
virtual double getInscribedRadius() {return 0.0;}
/**
* @brief Visualize the robot using a markers
*
* Fill a marker message with all necessary information (type, pose, scale and color).
* The header, namespace, id and marker lifetime will be overwritten.
* @param current_pose Current robot pose
* @param[out] markers container of marker messages describing the robot shape
* @param color Color of the footprint
*/
virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::Marker>& markers, const std_msgs::ColorRGBA& color) const
{
// point footprint
markers.push_back(visualization_msgs::Marker());
visualization_msgs::Marker& marker = markers.back();
marker.type = visualization_msgs::Marker::POINTS;
current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame!
marker.points.push_back(geometry_msgs::Point());
marker.scale.x = 0.025;
marker.color = color;
if (min_obstacle_dist_ <= 0)
{
return;
}
// footprint with min_obstacle_dist
markers.push_back(visualization_msgs::Marker());
visualization_msgs::Marker& marker2 = markers.back();
marker2.type = visualization_msgs::Marker::LINE_STRIP;
marker2.scale.x = 0.025;
marker2.color = color;
current_pose.toPoseMsg(marker2.pose); // all points are transformed into the robot frame!
const double n = 9;
const double r = min_obstacle_dist_;
for (double theta = 0; theta <= 2 * M_PI; theta += M_PI / n)
{
geometry_msgs::Point pt;
pt.x = r * cos(theta);
pt.y = r * sin(theta);
marker2.points.push_back(pt);
}
}
private:
const double min_obstacle_dist_ = 0.0;
};
/**
* @class CircularRobotFootprint
* @brief Class that defines the a robot of circular shape
*/
class CircularRobotFootprint : public BaseRobotFootprintModel
{
public:
/**
* @brief Default constructor of the abstract obstacle class
* @param radius radius of the robot
*/
CircularRobotFootprint(double radius) : radius_(radius) { }
/**
* @brief Virtual destructor.
*/
virtual ~CircularRobotFootprint() { }
/**
* @brief Set radius of the circular robot
* @param radius radius of the robot
*/
void setRadius(double radius) {radius_ = radius;}
/**
* @brief Calculate the distance between the robot and an obstacle
* @param current_pose Current robot pose
* @param obstacle Pointer to the obstacle
* @return Euclidean distance to the robot
*/
virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
{
return obstacle->getMinimumDistance(current_pose.position()) - radius_;
}
/**
* @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
* @param current_pose robot pose, from which the distance to the obstacle is estimated
* @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
* @param t time, for which the predicted distance to the obstacle is calculated
* @return Euclidean distance to the robot
*/
virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
{
return obstacle->getMinimumSpatioTemporalDistance(current_pose.position(), t) - radius_;
}
/**
* @brief Visualize the robot using a markers
*
* Fill a marker message with all necessary information (type, pose, scale and color).
* The header, namespace, id and marker lifetime will be overwritten.
* @param current_pose Current robot pose
* @param[out] markers container of marker messages describing the robot shape
* @param color Color of the footprint
*/
virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::Marker>& markers, const std_msgs::ColorRGBA& color) const
{
markers.resize(1);
visualization_msgs::Marker& marker = markers.back();
marker.type = visualization_msgs::Marker::CYLINDER;
current_pose.toPoseMsg(marker.pose);
marker.scale.x = marker.scale.y = 2*radius_; // scale = diameter
marker.scale.z = 0.05;
marker.color = color;
}
/**
* @brief Compute the inscribed radius of the footprint model
* @return inscribed radius
*/
virtual double getInscribedRadius() {return radius_;}
private:
double radius_;
};
/**
* @class TwoCirclesRobotFootprint
* @brief Class that approximates the robot with two shifted circles
*/
class TwoCirclesRobotFootprint : public BaseRobotFootprintModel
{
public:
/**
* @brief Default constructor of the abstract obstacle class
* @param front_offset shift the center of the front circle along the robot orientation starting from the center at the rear axis (in meters)
* @param front_radius radius of the front circle
* @param rear_offset shift the center of the rear circle along the opposite robot orientation starting from the center at the rear axis (in meters)
* @param rear_radius radius of the front circle
*/
TwoCirclesRobotFootprint(double front_offset, double front_radius, double rear_offset, double rear_radius)
: front_offset_(front_offset), front_radius_(front_radius), rear_offset_(rear_offset), rear_radius_(rear_radius) { }
/**
* @brief Virtual destructor.
*/
virtual ~TwoCirclesRobotFootprint() { }
/**
* @brief Set parameters of the contour/footprint
* @param front_offset shift the center of the front circle along the robot orientation starting from the center at the rear axis (in meters)
* @param front_radius radius of the front circle
* @param rear_offset shift the center of the rear circle along the opposite robot orientation starting from the center at the rear axis (in meters)
* @param rear_radius radius of the front circle
*/
void setParameters(double front_offset, double front_radius, double rear_offset, double rear_radius)
{front_offset_=front_offset; front_radius_=front_radius; rear_offset_=rear_offset; rear_radius_=rear_radius;}
/**
* @brief Calculate the distance between the robot and an obstacle
* @param current_pose Current robot pose
* @param obstacle Pointer to the obstacle
* @return Euclidean distance to the robot
*/
virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
{
Eigen::Vector2d dir = current_pose.orientationUnitVec();
double dist_front = obstacle->getMinimumDistance(current_pose.position() + front_offset_*dir) - front_radius_;
double dist_rear = obstacle->getMinimumDistance(current_pose.position() - rear_offset_*dir) - rear_radius_;
return std::min(dist_front, dist_rear);
}
/**
* @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
* @param current_pose robot pose, from which the distance to the obstacle is estimated
* @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
* @param t time, for which the predicted distance to the obstacle is calculated
* @return Euclidean distance to the robot
*/
virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
{
Eigen::Vector2d dir = current_pose.orientationUnitVec();
double dist_front = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() + front_offset_*dir, t) - front_radius_;
double dist_rear = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() - rear_offset_*dir, t) - rear_radius_;
return std::min(dist_front, dist_rear);
}
/**
* @brief Visualize the robot using a markers
*
* Fill a marker message with all necessary information (type, pose, scale and color).
* The header, namespace, id and marker lifetime will be overwritten.
* @param current_pose Current robot pose
* @param[out] markers container of marker messages describing the robot shape
* @param color Color of the footprint
*/
virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::Marker>& markers, const std_msgs::ColorRGBA& color) const
{
Eigen::Vector2d dir = current_pose.orientationUnitVec();
if (front_radius_>0)
{
markers.push_back(visualization_msgs::Marker());
visualization_msgs::Marker& marker1 = markers.back();
marker1.type = visualization_msgs::Marker::CYLINDER;
current_pose.toPoseMsg(marker1.pose);
marker1.pose.position.x += front_offset_*dir.x();
marker1.pose.position.y += front_offset_*dir.y();
marker1.scale.x = marker1.scale.y = 2*front_radius_; // scale = diameter
// marker1.scale.z = 0.05;
marker1.color = color;
}
if (rear_radius_>0)
{
markers.push_back(visualization_msgs::Marker());
visualization_msgs::Marker& marker2 = markers.back();
marker2.type = visualization_msgs::Marker::CYLINDER;
current_pose.toPoseMsg(marker2.pose);
marker2.pose.position.x -= rear_offset_*dir.x();
marker2.pose.position.y -= rear_offset_*dir.y();
marker2.scale.x = marker2.scale.y = 2*rear_radius_; // scale = diameter
// marker2.scale.z = 0.05;
marker2.color = color;
}
}
/**
* @brief Compute the inscribed radius of the footprint model
* @return inscribed radius
*/
virtual double getInscribedRadius()
{
double min_longitudinal = std::min(rear_offset_ + rear_radius_, front_offset_ + front_radius_);
double min_lateral = std::min(rear_radius_, front_radius_);
return std::min(min_longitudinal, min_lateral);
}
private:
double front_offset_;
double front_radius_;
double rear_offset_;
double rear_radius_;
};
/**
* @class LineRobotFootprint
* @brief Class that approximates the robot with line segment (zero-width)
*/
class LineRobotFootprint : public BaseRobotFootprintModel
{
public:
/**
* @brief Default constructor of the abstract obstacle class
* @param line_start start coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
* @param line_end end coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
*/
LineRobotFootprint(const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end)
{
setLine(line_start, line_end);
}
/**
* @brief Default constructor of the abstract obstacle class (Eigen Version)
* @param line_start start coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
* @param line_end end coordinates (only x and y) of the line (w.r.t. robot center at (0,0))
*/
LineRobotFootprint(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, const double min_obstacle_dist) : min_obstacle_dist_(min_obstacle_dist)
{
setLine(line_start, line_end);
}
/**
* @brief Virtual destructor.
*/
virtual ~LineRobotFootprint() { }
/**
* @brief Set vertices of the contour/footprint
* @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
*/
void setLine(const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end)
{
line_start_.x() = line_start.x;
line_start_.y() = line_start.y;
line_end_.x() = line_end.x;
line_end_.y() = line_end.y;
}
/**
* @brief Set vertices of the contour/footprint (Eigen version)
* @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
*/
void setLine(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end)
{
line_start_ = line_start;
line_end_ = line_end;
}
/**
* @brief Calculate the distance between the robot and an obstacle
* @param current_pose Current robot pose
* @param obstacle Pointer to the obstacle
* @return Euclidean distance to the robot
*/
virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
{
Eigen::Vector2d line_start_world;
Eigen::Vector2d line_end_world;
transformToWorld(current_pose, line_start_world, line_end_world);
return obstacle->getMinimumDistance(line_start_world, line_end_world);
}
/**
* @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
* @param current_pose robot pose, from which the distance to the obstacle is estimated
* @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
* @param t time, for which the predicted distance to the obstacle is calculated
* @return Euclidean distance to the robot
*/
virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
{
Eigen::Vector2d line_start_world;
Eigen::Vector2d line_end_world;
transformToWorld(current_pose, line_start_world, line_end_world);
return obstacle->getMinimumSpatioTemporalDistance(line_start_world, line_end_world, t);
}
/**
* @brief Visualize the robot using a markers
*
* Fill a marker message with all necessary information (type, pose, scale and color).
* The header, namespace, id and marker lifetime will be overwritten.
* @param current_pose Current robot pose
* @param[out] markers container of marker messages describing the robot shape
* @param color Color of the footprint
*/
virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::Marker>& markers, const std_msgs::ColorRGBA& color) const
{
markers.push_back(visualization_msgs::Marker());
visualization_msgs::Marker& marker = markers.back();
marker.type = visualization_msgs::Marker::LINE_STRIP;
current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame!
// line
geometry_msgs::Point line_start_world;
line_start_world.x = line_start_.x();
line_start_world.y = line_start_.y();
line_start_world.z = 0;
marker.points.push_back(line_start_world);
geometry_msgs::Point line_end_world;
line_end_world.x = line_end_.x();
line_end_world.y = line_end_.y();
line_end_world.z = 0;
marker.points.push_back(line_end_world);
marker.scale.x = 0.025;
marker.color = color;
if (min_obstacle_dist_ <= 0)
{
return;
}
// footprint with min_obstacle_dist
markers.push_back(visualization_msgs::Marker());
visualization_msgs::Marker& marker2 = markers.back();
marker2.type = visualization_msgs::Marker::LINE_STRIP;
marker2.scale.x = 0.025;
marker2.color = color;
current_pose.toPoseMsg(marker2.pose); // all points are transformed into the robot frame!
const double n = 9;
const double r = min_obstacle_dist_;
const double ori = atan2(line_end_.y() - line_start_.y(), line_end_.x() - line_start_.x());
// first half-circle
for (double theta = M_PI_2 + ori; theta <= 3 * M_PI_2 + ori; theta += M_PI / n)
{
geometry_msgs::Point pt;
pt.x = line_start_.x() + r * cos(theta);
pt.y = line_start_.y() + r * sin(theta);
marker2.points.push_back(pt);
}
// second half-circle
for (double theta = -M_PI_2 + ori; theta <= M_PI_2 + ori; theta += M_PI / n)
{
geometry_msgs::Point pt;
pt.x = line_end_.x() + r * cos(theta);
pt.y = line_end_.y() + r * sin(theta);
marker2.points.push_back(pt);
}
// duplicate 1st point to close shape
geometry_msgs::Point pt;
pt.x = line_start_.x() + r * cos(M_PI_2 + ori);
pt.y = line_start_.y() + r * sin(M_PI_2 + ori);
marker2.points.push_back(pt);
}
/**
* @brief Compute the inscribed radius of the footprint model
* @return inscribed radius
*/
virtual double getInscribedRadius()
{
return 0.0; // lateral distance = 0.0
}
private:
/**
* @brief Transforms a line to the world frame manually
* @param current_pose Current robot pose
* @param[out] line_start line_start_ in the world frame
* @param[out] line_end line_end_ in the world frame
*/
void transformToWorld(const PoseSE2& current_pose, Eigen::Vector2d& line_start_world, Eigen::Vector2d& line_end_world) const
{
double cos_th = std::cos(current_pose.theta());
double sin_th = std::sin(current_pose.theta());
line_start_world.x() = current_pose.x() + cos_th * line_start_.x() - sin_th * line_start_.y();
line_start_world.y() = current_pose.y() + sin_th * line_start_.x() + cos_th * line_start_.y();
line_end_world.x() = current_pose.x() + cos_th * line_end_.x() - sin_th * line_end_.y();
line_end_world.y() = current_pose.y() + sin_th * line_end_.x() + cos_th * line_end_.y();
}
Eigen::Vector2d line_start_;
Eigen::Vector2d line_end_;
const double min_obstacle_dist_ = 0.0;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @class PolygonRobotFootprint
* @brief Class that approximates the robot with a closed polygon
*/
class PolygonRobotFootprint : public BaseRobotFootprintModel
{
public:
/**
* @brief Default constructor of the abstract obstacle class
* @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
*/
PolygonRobotFootprint(const Point2dContainer& vertices) : vertices_(vertices) { }
/**
* @brief Virtual destructor.
*/
virtual ~PolygonRobotFootprint() { }
/**
* @brief Set vertices of the contour/footprint
* @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end)
*/
void setVertices(const Point2dContainer& vertices) {vertices_ = vertices;}
/**
* @brief Calculate the distance between the robot and an obstacle
* @param current_pose Current robot pose
* @param obstacle Pointer to the obstacle
* @return Euclidean distance to the robot
*/
virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const
{
Point2dContainer polygon_world(vertices_.size());
transformToWorld(current_pose, polygon_world);
return obstacle->getMinimumDistance(polygon_world);
}
/**
* @brief Estimate the distance between the robot and the predicted location of an obstacle at time t
* @param current_pose robot pose, from which the distance to the obstacle is estimated
* @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed)
* @param t time, for which the predicted distance to the obstacle is calculated
* @return Euclidean distance to the robot
*/
virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const
{
Point2dContainer polygon_world(vertices_.size());
transformToWorld(current_pose, polygon_world);
return obstacle->getMinimumSpatioTemporalDistance(polygon_world, t);
}
/**
* @brief Visualize the robot using a markers
*
* Fill a marker message with all necessary information (type, pose, scale and color).
* The header, namespace, id and marker lifetime will be overwritten.
* @param current_pose Current robot pose
* @param[out] markers container of marker messages describing the robot shape
* @param color Color of the footprint
*/
virtual void visualizeRobot(const PoseSE2& current_pose, std::vector<visualization_msgs::Marker>& markers, const std_msgs::ColorRGBA& color) const
{
if (vertices_.empty())
return;
markers.push_back(visualization_msgs::Marker());
visualization_msgs::Marker& marker = markers.back();
marker.type = visualization_msgs::Marker::LINE_STRIP;
current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame!
for (std::size_t i = 0; i < vertices_.size(); ++i)
{
geometry_msgs::Point point;
point.x = vertices_[i].x();
point.y = vertices_[i].y();
point.z = 0;
marker.points.push_back(point);
}
// add first point again in order to close the polygon
geometry_msgs::Point point;
point.x = vertices_.front().x();
point.y = vertices_.front().y();
point.z = 0;
marker.points.push_back(point);
marker.scale.x = 0.025;
marker.color = color;
}
/**
* @brief Compute the inscribed radius of the footprint model
* @return inscribed radius
*/
virtual double getInscribedRadius()
{
double min_dist = std::numeric_limits<double>::max();
Eigen::Vector2d center(0.0, 0.0);
if (vertices_.size() <= 2)
return 0.0;
for (int i = 0; i < (int)vertices_.size() - 1; ++i)
{
// compute distance from the robot center point to the first vertex
double vertex_dist = vertices_[i].norm();
double edge_dist = distance_point_to_segment_2d(center, vertices_[i], vertices_[i+1]);
min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
}
// we also need to check the last vertex and the first vertex
double vertex_dist = vertices_.back().norm();
double edge_dist = distance_point_to_segment_2d(center, vertices_.back(), vertices_.front());
return std::min(min_dist, std::min(vertex_dist, edge_dist));
}
private:
/**
* @brief Transforms a polygon to the world frame manually
* @param current_pose Current robot pose
* @param[out] polygon_world polygon in the world frame
*/
void transformToWorld(const PoseSE2& current_pose, Point2dContainer& polygon_world) const
{
double cos_th = std::cos(current_pose.theta());
double sin_th = std::sin(current_pose.theta());
for (std::size_t i=0; i<vertices_.size(); ++i)
{
polygon_world[i].x() = current_pose.x() + cos_th * vertices_[i].x() - sin_th * vertices_[i].y();
polygon_world[i].y() = current_pose.y() + sin_th * vertices_[i].x() + cos_th * vertices_[i].y();
}
}
Point2dContainer vertices_;
};
} // namespace teb_local_planner
#endif /* ROBOT_FOOTPRINT_MODEL_H */

431
navigations/teb_local_planner/include/teb_local_planner/teb_config.h Executable file
View File

@@ -0,0 +1,431 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef TEB_CONFIG_H_
#define TEB_CONFIG_H_
#include <ros/console.h>
#include <ros/ros.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <teb_local_planner/TebLocalPlannerReconfigureConfig.h>
// Definitions
#define USE_ANALYTIC_JACOBI // if available for a specific edge, use analytic jacobi
namespace teb_local_planner
{
/**
* @class TebConfig
* @brief Config class for the teb_local_planner and its components.
*/
class TebConfig
{
public:
std::string odom_topic; //!< Topic name of the odometry message, provided by the robot driver or simulator
std::string map_frame; //!< Global planning frame
//! Trajectory related parameters
struct Trajectory
{
double teb_autosize; //!< Enable automatic resizing of the trajectory w.r.t to the temporal resolution (recommended)
double dt_ref; //!< Desired temporal resolution of the trajectory (should be in the magniture of the underlying control rate)
double dt_hysteresis; //!< Hysteresis for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref
int min_samples; //!< Minimum number of samples (should be always greater than 2)
int max_samples; //!< Maximum number of samples; Warning: if too small the discretization/resolution might not be sufficient for the given robot model or obstacle avoidance does not work anymore.
bool global_plan_overwrite_orientation; //!< Overwrite orientation of local subgoals provided by the global planner
bool allow_init_with_backwards_motion; //!< If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)
double global_plan_viapoint_sep; //!< Min. separation between each two consecutive via-points extracted from the global plan (if negative: disabled)
bool via_points_ordered; //!< If true, the planner adheres to the order of via-points in the storage container
double max_global_plan_lookahead_dist; //!< Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if <=0: disabled; the length is also bounded by the local costmap size!]
double global_plan_prune_distance; //!< Distance between robot and via_points of global plan which is used for pruning
bool exact_arc_length; //!< If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.
double force_reinit_new_goal_dist; //!< Reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)
double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (skip hot-starting)
int feasibility_check_no_poses; //!< Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.
double feasibility_check_lookahead_distance; //!< Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.
bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)
double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad]
int control_look_ahead_poses; //! Index of the pose used to extract the velocity command
int prevent_look_ahead_poses_near_goal; //! Prevents control_look_ahead_poses to look within this many poses of the goal in order to prevent overshoot & oscillation when xy_goal_tolerance is very small
} trajectory; //!< Trajectory related parameters
//! Robot related parameters
struct Robot
{
double max_vel_x; //!< Maximum translational velocity of the robot
double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards
double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
double max_vel_trans; //!< Maximum translational velocity of the robot for omni robots, which is different from max_vel_x
double max_vel_theta; //!< Maximum angular velocity of the robot
double acc_lim_x; //!< Maximum translational acceleration of the robot
double acc_lim_y; //!< Maximum strafing acceleration of the robot
double acc_lim_theta; //!< Maximum angular acceleration of the robot
double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero);
double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')
bool is_footprint_dynamic; //<! If true, updated the footprint before checking trajectory feasibility
bool use_proportional_saturation; //<! If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually
double transform_tolerance = 0.5; //<! Tolerance when querying the TF Tree for a transformation (seconds)
} robot; //!< Robot related parameters
//! Goal tolerance related parameters
struct GoalTolerance
{
double yaw_goal_tolerance; //!< Allowed final orientation error
double xy_goal_tolerance; //!< Allowed final euclidean distance to the goal position
bool free_goal_vel; //!< Allow the robot's velocity to be nonzero (usally max_vel) for planning purposes
double trans_stopped_vel; //!< Below what maximum velocity we consider the robot to be stopped in translation
double theta_stopped_vel; //!< Below what maximum rotation velocity we consider the robot to be stopped in rotation
bool complete_global_plan; // true prevents the robot from ending the path early when it cross the end goal
} goal_tolerance; //!< Goal tolerance related parameters
//! Obstacle related parameters
struct Obstacles
{
double min_obstacle_dist; //!< Minimum desired separation from obstacles
double inflation_dist; //!< buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
double dynamic_obstacle_inflation_dist; //!< Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)
bool include_dynamic_obstacles; //!< Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also effects homotopy class planning); If false, all obstacles are considered to be static.
bool include_costmap_obstacles; //!< Specify whether the obstacles in the costmap should be taken into account directly
double costmap_obstacles_behind_robot_dist; //!< Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)
int obstacle_poses_affected; //!< The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well
bool legacy_obstacle_association; //!< If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only "relevant" obstacles).
double obstacle_association_force_inclusion_factor; //!< The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.
double obstacle_association_cutoff_factor; //!< See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.
std::string costmap_converter_plugin; //!< Define a plugin name of the costmap_converter package (costmap cells are converted to points/lines/polygons)
bool costmap_converter_spin_thread; //!< If \c true, the costmap converter invokes its callback queue in a different thread
int costmap_converter_rate; //!< The rate that defines how often the costmap_converter plugin processes the current costmap (the value should not be much higher than the costmap update rate)
double obstacle_proximity_ratio_max_vel; //!< Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to a static obstacles
double obstacle_proximity_lower_bound; //!< Distance to a static obstacle for which the velocity should be lower
double obstacle_proximity_upper_bound; //!< Distance to a static obstacle for which the velocity should be higher
} obstacles; //!< Obstacle related parameters
//! Optimization related parameters
struct Optimization
{
int no_inner_iterations; //!< Number of solver iterations called in each outerloop iteration
int no_outer_iterations; //!< Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations
bool optimization_activate; //!< Activate the optimization
bool optimization_verbose; //!< Print verbose information
double penalty_epsilon; //!< Add a small safety margin to penalty functions for hard-constraint approximations
double weight_max_vel_x; //!< Optimization weight for satisfying the maximum allowed translational velocity
double weight_max_vel_y; //!< Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)
double weight_max_vel_theta; //!< Optimization weight for satisfying the maximum allowed angular velocity
double weight_acc_lim_x; //!< Optimization weight for satisfying the maximum allowed translational acceleration
double weight_acc_lim_y; //!< Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)
double weight_acc_lim_theta; //!< Optimization weight for satisfying the maximum allowed angular acceleration
double weight_kinematics_nh; //!< Optimization weight for satisfying the non-holonomic kinematics
double weight_kinematics_forward_drive; //!< Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)
double weight_kinematics_turning_radius; //!< Optimization weight for enforcing a minimum turning radius (carlike robots)
double weight_optimaltime; //!< Optimization weight for contracting the trajectory w.r.t. transition time
double weight_shortest_path; //!< Optimization weight for contracting the trajectory w.r.t. path length
double weight_obstacle; //!< Optimization weight for satisfying a minimum separation from obstacles
double weight_inflation; //!< Optimization weight for the inflation penalty (should be small)
double weight_dynamic_obstacle; //!< Optimization weight for satisfying a minimum separation from dynamic obstacles
double weight_dynamic_obstacle_inflation; //!< Optimization weight for the inflation penalty of dynamic obstacles (should be small)
double weight_velocity_obstacle_ratio; //!< Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle
double weight_viapoint; //!< Optimization weight for minimizing the distance to via-points
double weight_prefer_rotdir; //!< Optimization weight for preferring a specific turning direction (-> currently only activated if an oscillation is detected, see 'oscillation_recovery'
double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.
double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)
} optim; //!< Optimization related parameters
struct HomotopyClasses
{
bool enable_homotopy_class_planning; //!< Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectories are optimized at once).
bool enable_multithreading; //!< Activate multiple threading for planning multiple trajectories in parallel.
bool simple_exploration; //!< If true, distinctive trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal.
int max_number_classes; //!< Specify the maximum number of allowed alternative homotopy classes (limits computational effort)
int max_number_plans_in_current_class; //!< Specify the maximum number of trajectories to try that are in the same homotopy class as the current trajectory (helps avoid local minima)
double selection_cost_hysteresis; //!< Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor).
double selection_prefer_initial_plan; //!< Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.
double selection_obst_cost_scale; //!< Extra scaling of obstacle cost terms just for selecting the 'best' candidate.
double selection_viapoint_cost_scale; //!< Extra scaling of via-point cost terms just for selecting the 'best' candidate.
bool selection_alternative_time_cost; //!< If true, time cost is replaced by the total transition time.
double selection_dropping_probability; //!< At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.
double switching_blocking_period; //!< Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed
int roadmap_graph_no_samples; //! < Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off.
double roadmap_graph_area_width; //!< Random keypoints/waypoints are sampled in a rectangular region between start and goal. Specify the width of that region in meters.
double roadmap_graph_area_length_scale; //!< The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!
double h_signature_prescaler; //!< Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1).
double h_signature_threshold; //!< Two h-signatures are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold.
double obstacle_keypoint_offset; //!< If simple_exploration is turned on, this parameter determines the distance on the left and right side of the obstacle at which a new keypoint will be cretead (in addition to min_obstacle_dist).
double obstacle_heading_threshold; //!< Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration [0,1]
bool viapoints_all_candidates; //!< If true, all trajectories of different topologies are attached to the current set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan.
bool visualize_hc_graph; //!< Visualize the graph that is created for exploring new homotopy classes.
double visualize_with_time_as_z_axis_scale; //!< If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.
bool delete_detours_backwards; //!< If enabled, the planner will discard the plans detouring backwards with respect to the best plan
double detours_orientation_tolerance; //!< A plan is considered a detour if its start orientation differs more than this from the best plan
double length_start_orientation_vector; //!< Length of the vector used to compute the start orientation of a plan
double max_ratio_detours_duration_best_duration; //!< Detours are discarted if their execution time / the execution time of the best teb is > this
} hcp;
//! Recovery/backup related parameters
struct Recovery
{
bool shrink_horizon_backup; //!< Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.
double shrink_horizon_min_duration; //!< Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected.
bool oscillation_recovery; //!< Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards)
double oscillation_v_eps; //!< Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected
double oscillation_omega_eps; //!< Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected
double oscillation_recovery_min_duration; //!< Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected.
double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations
bool divergence_detection_enable; //!< True to enable divergence detection.
int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.
} recovery; //!< Parameters related to recovery and backup strategies
/**
* @brief Construct the TebConfig using default values.
* @warning If the \b rosparam server or/and \b dynamic_reconfigure (rqt_reconfigure) node are used,
* the default variables will be overwritten: \n
* E.g. if \e base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a
* dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications.
* All parameters considered by the dynamic_reconfigure server (and their \b default values) are
* set in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. \n
* In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file.
* The plugin source (or a possible binary source) can call loadRosParamFromNodeHandle() to update the parameters.
* In \e summary, default parameters are loaded in the following order (the right one overrides the left ones): \n
* <b>TebConfig Constructor defaults << dynamic_reconfigure defaults << rosparam server defaults</b>
*/
TebConfig()
{
odom_topic = "odom";
map_frame = "odom";
// Trajectory
trajectory.teb_autosize = true;
trajectory.dt_ref = 0.3;
trajectory.dt_hysteresis = 0.1;
trajectory.min_samples = 3;
trajectory.max_samples = 500;
trajectory.global_plan_overwrite_orientation = true;
trajectory.allow_init_with_backwards_motion = false;
trajectory.global_plan_viapoint_sep = -1;
trajectory.via_points_ordered = false;
trajectory.max_global_plan_lookahead_dist = 1;
trajectory.global_plan_prune_distance = 1;
trajectory.exact_arc_length = false;
trajectory.force_reinit_new_goal_dist = 1;
trajectory.force_reinit_new_goal_angular = 0.5 * M_PI;
trajectory.feasibility_check_no_poses = 5;
trajectory.feasibility_check_lookahead_distance = -1;
trajectory.publish_feedback = false;
trajectory.min_resolution_collision_check_angular = M_PI;
trajectory.control_look_ahead_poses = 1;
trajectory.prevent_look_ahead_poses_near_goal = 0;
// Robot
robot.max_vel_x = 0.4;
robot.max_vel_x_backwards = 0.2;
robot.max_vel_y = 0.0;
robot.max_vel_trans = 0.0;
robot.max_vel_theta = 0.3;
robot.acc_lim_x = 0.5;
robot.acc_lim_y = 0.5;
robot.acc_lim_theta = 0.5;
robot.min_turning_radius = 0;
robot.wheelbase = 1.0;
robot.cmd_angle_instead_rotvel = false;
robot.is_footprint_dynamic = false;
robot.use_proportional_saturation = false;
// GoalTolerance
goal_tolerance.xy_goal_tolerance = 0.2;
goal_tolerance.yaw_goal_tolerance = 0.2;
goal_tolerance.free_goal_vel = false;
goal_tolerance.trans_stopped_vel = 0.1;
goal_tolerance.theta_stopped_vel = 0.1;
goal_tolerance.complete_global_plan = true;
// Obstacles
obstacles.min_obstacle_dist = 0.5;
obstacles.inflation_dist = 0.6;
obstacles.dynamic_obstacle_inflation_dist = 0.6;
obstacles.include_dynamic_obstacles = true;
obstacles.include_costmap_obstacles = true;
obstacles.costmap_obstacles_behind_robot_dist = 1.5;
obstacles.obstacle_poses_affected = 25;
obstacles.legacy_obstacle_association = false;
obstacles.obstacle_association_force_inclusion_factor = 1.5;
obstacles.obstacle_association_cutoff_factor = 5;
obstacles.costmap_converter_plugin = "";
obstacles.costmap_converter_spin_thread = true;
obstacles.costmap_converter_rate = 5;
obstacles.obstacle_proximity_ratio_max_vel = 1;
obstacles.obstacle_proximity_lower_bound = 0;
obstacles.obstacle_proximity_upper_bound = 0.5;
// Optimization
optim.no_inner_iterations = 5;
optim.no_outer_iterations = 4;
optim.optimization_activate = true;
optim.optimization_verbose = false;
optim.penalty_epsilon = 0.05;
optim.weight_max_vel_x = 2; //1
optim.weight_max_vel_y = 2;
optim.weight_max_vel_theta = 1;
optim.weight_acc_lim_x = 1;
optim.weight_acc_lim_y = 1;
optim.weight_acc_lim_theta = 1;
optim.weight_kinematics_nh = 1000;
optim.weight_kinematics_forward_drive = 1;
optim.weight_kinematics_turning_radius = 1;
optim.weight_optimaltime = 1;
optim.weight_shortest_path = 0;
optim.weight_obstacle = 50;
optim.weight_inflation = 0.1;
optim.weight_dynamic_obstacle = 50;
optim.weight_dynamic_obstacle_inflation = 0.1;
optim.weight_velocity_obstacle_ratio = 0;
optim.weight_viapoint = 1;
optim.weight_prefer_rotdir = 50;
optim.weight_adapt_factor = 2.0;
optim.obstacle_cost_exponent = 1.0;
// Homotopy Class Planner
hcp.enable_homotopy_class_planning = true;
hcp.enable_multithreading = true;
hcp.simple_exploration = false;
hcp.max_number_classes = 5;
hcp.selection_cost_hysteresis = 1.0;
hcp.selection_prefer_initial_plan = 0.95;
hcp.selection_obst_cost_scale = 100.0;
hcp.selection_viapoint_cost_scale = 1.0;
hcp.selection_alternative_time_cost = false;
hcp.selection_dropping_probability = 0.0;
hcp.obstacle_keypoint_offset = 0.1;
hcp.obstacle_heading_threshold = 0.45;
hcp.roadmap_graph_no_samples = 15;
hcp.roadmap_graph_area_width = 6; // [m]
hcp.roadmap_graph_area_length_scale = 1.0;
hcp.h_signature_prescaler = 1;
hcp.h_signature_threshold = 0.1;
hcp.switching_blocking_period = 0.0;
hcp.viapoints_all_candidates = true;
hcp.visualize_hc_graph = false;
hcp.visualize_with_time_as_z_axis_scale = 0.0;
hcp.delete_detours_backwards = true;
hcp.detours_orientation_tolerance = M_PI / 2.0;
hcp.length_start_orientation_vector = 0.4;
hcp.max_ratio_detours_duration_best_duration = 3.0;
// Recovery
recovery.shrink_horizon_backup = true;
recovery.shrink_horizon_min_duration = 10;
recovery.oscillation_recovery = true;
recovery.oscillation_v_eps = 0.1;
recovery.oscillation_omega_eps = 0.1;
recovery.oscillation_recovery_min_duration = 10;
recovery.oscillation_filter_duration = 10;
}
/**
* @brief Load parmeters from the ros param server.
* @param nh const reference to the local ros::NodeHandle
*/
void loadRosParamFromNodeHandle(const ros::NodeHandle& nh);
/**
* @brief Reconfigure parameters from the dynamic_reconfigure config.
* Change parameters dynamically (e.g. with <c>rosrun rqt_reconfigure rqt_reconfigure</c>).
* A reconfigure server needs to be instantiated that calls this method in it's callback.
* In case of the plugin \e teb_local_planner default values are defined
* in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg.
* @param cfg Config class autogenerated by dynamic_reconfigure according to the cfg-file mentioned above.
*/
void reconfigure(TebLocalPlannerReconfigureConfig& cfg);
/**
* @brief Check parameters and print warnings in case of discrepancies
*
* Call this method whenever parameters are changed using public interfaces to inform the user
* about some improper uses.
*/
void checkParameters() const;
/**
* @brief Check if some deprecated parameters are found and print warnings
* @param nh const reference to the local ros::NodeHandle
*/
void checkDeprecated(const ros::NodeHandle& nh) const;
/**
* @brief Return the internal config mutex
*/
boost::mutex& configMutex() {return config_mutex_;}
private:
boost::mutex config_mutex_; //!< Mutex for config accesses and changes
};
} // namespace teb_local_planner
#endif

457
navigations/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h Executable file
View File

@@ -0,0 +1,457 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef TEB_LOCAL_PLANNER_ROS_H_
#define TEB_LOCAL_PLANNER_ROS_H_
#include <ros/ros.h>
// base local planner base class and utilities
#include <nav_core/base_local_planner.h>
#include <mbf_costmap_core/costmap_controller.h>
#include <base_local_planner/goal_functions.h>
#include <base_local_planner/odometry_helper_ros.h>
#include <base_local_planner/costmap_model.h>
// timed-elastic-band related classes
#include <teb_local_planner/optimal_planner.h>
#include <teb_local_planner/homotopy_class_planner.h>
#include <teb_local_planner/visualization.h>
#include <teb_local_planner/recovery_behaviors.h>
// message types
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <costmap_converter/ObstacleMsg.h>
// transforms
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>
// costmap
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_converter/costmap_converter_interface.h>
// dynamic reconfigure
#include <teb_local_planner/TebLocalPlannerReconfigureConfig.h>
#include <dynamic_reconfigure/server.h>
// boost classes
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
namespace teb_local_planner
{
/**
* @class TebLocalPlannerROS
* @brief Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract
* interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF).
* @todo Escape behavior, more efficient obstacle handling
*/
class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController
{
public:
/**
* @brief Default constructor of the teb plugin
*/
TebLocalPlannerROS();
/**
* @brief Destructor of the plugin
*/
~TebLocalPlannerROS();
/**
* @brief Initializes the teb plugin
* @param name The name of the instance
* @param tf Pointer to a tf buffer
* @param costmap_ros Cost map representing occupied and free space
*/
void initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Set the plan that the teb local planner is following
* @param orig_global_plan The plan to pass to the local planner
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.
* @remark Extended version for MBF API
* @param pose the current pose of the robot.
* @param velocity the current velocity of the robot.
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base.
* @param message Optional more detailed outcome as a string
* @return Result code as described on ExePath action result:
* SUCCESS = 0
* 1..9 are reserved as plugin specific non-error results
* FAILURE = 100 Unspecified failure, only used for old, non-mfb_core based plugins
* CANCELED = 101
* NO_VALID_CMD = 102
* PAT_EXCEEDED = 103
* COLLISION = 104
* OSCILLATION = 105
* ROBOT_STUCK = 106
* MISSED_GOAL = 107
* MISSED_PATH = 108
* BLOCKED_PATH = 109
* INVALID_PATH = 110
* TF_ERROR = 111
* NOT_INITIALIZED = 112
* INVALID_PLUGIN = 113
* INTERNAL_ERROR = 114
* 121..149 are reserved as plugin specific errors
*/
uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity,
geometry_msgs::TwistStamped &cmd_vel, std::string &message);
/**
* @brief Check if the goal pose has been achieved
*
* The actual check is performed in computeVelocityCommands().
* Only the status flag is checked here.
* @return True if achieved, false otherwise
*/
bool isGoalReached();
/**
* @brief Dummy version to satisfy MBF API
*/
bool isGoalReached(double xy_tolerance, double yaw_tolerance) { return isGoalReached(); };
/**
* @brief Requests the planner to cancel, e.g. if it takes too much time
* @remark New on MBF API
* @return True if a cancel has been successfully requested, false if not implemented.
*/
bool cancel() { return false; };
/** @name Public utility functions/methods */
//@{
/**
* @brief Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities.
*
* Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component).
* @param tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle)
* @return Translational and angular velocity combined into an Eigen::Vector2d
*/
static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);
/**
* @brief Get the current robot footprint/contour model
* @param nh const reference to the local ros::NodeHandle
* @param config const reference to the current configuration
* @return Robot footprint model used for optimization
*/
static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, const TebConfig& config);
/**
* @brief Set the footprint from the given XmlRpcValue.
* @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
* @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point
* @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the
* sub-arrays should all have exactly 2 elements (x and y coordinates).
* @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came.
* It is used only for reporting errors.
* @return container of vertices describing the polygon
*/
static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);
/**
* @brief Get a number from the given XmlRpcValue.
* @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
* @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point
* @param value double value type
* @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came.
* It is used only for reporting errors.
* @returns double value
*/
static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
//@}
protected:
/**
* @brief Update internal obstacle vector based on occupied costmap cells
* @remarks All occupied cells will be added as point obstacles.
* @remarks All previous obstacles are cleared.
* @sa updateObstacleContainerWithCostmapConverter
* @todo Include temporal coherence among obstacle msgs (id vector)
* @todo Include properties for dynamic obstacles (e.g. using constant velocity model)
*/
void updateObstacleContainerWithCostmap();
/**
* @brief Update internal obstacle vector based on polygons provided by a costmap_converter plugin
* @remarks Requires a loaded costmap_converter plugin.
* @remarks All previous obstacles are cleared.
* @sa updateObstacleContainerWithCostmap
*/
void updateObstacleContainerWithCostmapConverter();
/**
* @brief Update internal obstacle vector based on custom messages received via subscriber
* @remarks All previous obstacles are NOT cleared. Call this method after other update methods.
* @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter
*/
void updateObstacleContainerWithCustomObstacles();
/**
* @brief Update internal via-point container based on the current reference plan
* @remarks All previous via-points will be cleared.
* @param transformed_plan (local) portion of the global plan (which is already transformed to the planning frame)
* @param min_separation minimum separation between two consecutive via-points
*/
void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation);
/**
* @brief Callback for the dynamic_reconfigure node.
*
* This callback allows to modify parameters dynamically at runtime without restarting the node
* @param config Reference to the dynamic reconfigure config
* @param level Dynamic reconfigure level
*/
void reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level);
/**
* @brief Callback for custom obstacles that are not obtained from the costmap
* @param obst_msg pointer to the message containing a list of polygon shaped obstacles
*/
void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
/**
* @brief Callback for custom via-points
* @param via_points_msg pointer to the message containing a list of via-points
*/
void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg);
/**
* @brief Prune global plan such that already passed poses are cut off
*
* The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform.
* If no valid transformation can be found, the method returns \c false.
* The global plan is pruned until the distance to the robot is at least \c dist_behind_robot.
* If no pose within the specified treshold \c dist_behind_robot can be found,
* nothing will be pruned and the method returns \c false.
* @remarks Do not choose \c dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned.
* @param tf A reference to a tf buffer
* @param global_pose The global pose of the robot
* @param[in,out] global_plan The plan to be transformed
* @param dist_behind_robot Distance behind the robot that should be kept [meters]
* @return \c true if the plan is pruned, \c false in case of a transform exception or if no pose cannot be found inside the threshold
*/
bool pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose,
std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot=1);
/**
* @brief Transforms the global plan of the robot from the planner frame to the local frame (modified).
*
* The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
* such that the index of the current goal pose is returned as well as
* the transformation between the global plan and the planning frame.
* @param tf A reference to a tf buffer
* @param global_plan The plan to be transformed
* @param global_pose The global pose of the robot
* @param costmap A reference to the costmap being used so the window size for transforming can be computed
* @param global_frame The frame to transform the plan to
* @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
* @param[out] transformed_plan Populated with the transformed plan
* @param[out] current_goal_idx Index of the current (local) goal pose in the global plan
* @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
* @return \c true if the global plan is transformed, \c false otherwise
*/
bool transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap,
const std::string& global_frame, double max_plan_length, std::vector<geometry_msgs::PoseStamped>& transformed_plan,
int* current_goal_idx = NULL, geometry_msgs::TransformStamped* tf_plan_to_global = NULL) const;
/**
* @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner.
*
* If the current (local) goal point is not the final one (global)
* substitute the goal orientation by the angle of the direction vector between
* the local goal and the subsequent pose of the global plan.
* This is often helpful, if the global planner does not consider orientations. \n
* A moving average filter is utilized to smooth the orientation.
* @param global_plan The global plan
* @param local_goal Current local goal
* @param current_goal_idx Index of the current (local) goal pose in the global plan
* @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
* @param moving_average_length number of future poses of the global plan to be taken into account
* @return orientation (yaw-angle) estimate
*/
double estimateLocalGoalOrientation(const std::vector<geometry_msgs::PoseStamped>& global_plan, const geometry_msgs::PoseStamped& local_goal,
int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global, int moving_average_length=3) const;
/**
* @brief Saturate the translational and angular velocity to given limits.
*
* The limit of the translational velocity for backwards driving can be changed independently.
* Do not choose max_vel_x_backwards <= 0. If no backward driving is desired, change the optimization weight for
* penalizing backwards driving instead.
* @param[in,out] vx The translational velocity that should be saturated.
* @param[in,out] vy Strafing velocity which can be nonzero for holonomic robots
* @param[in,out] omega The angular velocity that should be saturated.
* @param max_vel_x Maximum translational velocity for forward driving
* @param max_vel_y Maximum strafing velocity (for holonomic robots)
* @param max_vel_trans Maximum translational velocity for holonomic robots
* @param max_vel_theta Maximum (absolute) angular velocity
* @param max_vel_x_backwards Maximum translational velocity for backwards driving
*/
void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
double max_vel_trans, double max_vel_theta, double max_vel_x_backwards) const;
/**
* @brief Convert translational and rotational velocities to a steering angle of a carlike robot
*
* The conversion is based on the following equations:
* - The turning radius is defined by \f$ R = v/omega \f$
* - For a car like robot withe a distance L between both axles, the relation is: \f$ tan(\phi) = L/R \f$
* - phi denotes the steering angle.
* @remarks You might provide distances instead of velocities, since the temporal information is not required.
* @param v translational velocity [m/s]
* @param omega rotational velocity [rad/s]
* @param wheelbase distance between both axles (drive shaft and steering axle), the value might be negative for back_wheeled robots
* @param min_turning_radius Specify a lower bound on the turning radius
* @return Resulting steering angle in [rad] inbetween [-pi/2, pi/2]
*/
double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const;
/**
* @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint
*
* This method prints warnings if validation fails.
* @remarks Currently, we only validate the inscribed radius of the footprints
* @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization
* @param costmap_inscribed_radius Inscribed radius of the footprint model used for the costmap
* @param min_obst_dist desired distance to obstacles
*/
void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
void configureBackupModes(std::vector<geometry_msgs::PoseStamped>& transformed_plan, int& goal_idx);
private:
// Definition of member variables
// external objects (store weak pointers)
costmap_2d::Costmap2DROS* costmap_ros_; //!< Pointer to the costmap ros wrapper, received from the navigation stack
costmap_2d::Costmap2D* costmap_; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper)
tf2_ros::Buffer* tf_; //!< pointer to tf buffer
// internal objects (memory management owned)
PlannerInterfacePtr planner_; //!< Instance of the underlying optimal planner class
ObstContainer obstacles_; //!< Obstacle vector that should be considered during local trajectory optimization
ViaPointContainer via_points_; //!< Container of via-points that should be considered during local trajectory optimization
TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...)
boost::shared_ptr<base_local_planner::CostmapModel> costmap_model_;
TebConfig cfg_; //!< Config class that stores and manages all related parameters
FailureDetector failure_detector_; //!< Detect if the robot got stucked
std::vector<geometry_msgs::PoseStamped> global_plan_; //!< Store the current global plan
base_local_planner::OdometryHelperRos odom_helper_; //!< Provides an interface to receive the current velocity from the robot
pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> costmap_converter_loader_; //!< Load costmap converter plugins at runtime
boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> costmap_converter_; //!< Store the current costmap_converter
boost::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
ros::Subscriber custom_obst_sub_; //!< Subscriber for custom obstacles received via a ObstacleMsg.
boost::mutex custom_obst_mutex_; //!< Mutex that locks the obstacle array (multi-threaded)
costmap_converter::ObstacleArrayMsg custom_obstacle_msg_; //!< Copy of the most recent obstacle message
ros::Subscriber via_points_sub_; //!< Subscriber for custom via-points received via a Path msg.
bool custom_via_points_active_; //!< Keep track whether valid via-points have been received from via_points_sub_
boost::mutex via_point_mutex_; //!< Mutex that locks the via_points container (multi-threaded)
PoseSE2 robot_pose_; //!< Store current robot pose
PoseSE2 robot_goal_; //!< Store current robot goal
geometry_msgs::Twist robot_vel_; //!< Store current robot translational and angular velocity (vx, vy, omega)
bool goal_reached_; //!< store whether the goal is reached or not
ros::Time time_last_infeasible_plan_; //!< Store at which time stamp the last infeasible plan was detected
int no_infeasible_plans_; //!< Store how many times in a row the planner failed to find a feasible plan.
ros::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected
RotType last_preferred_rotdir_; //!< Store recent preferred turning direction
geometry_msgs::Twist last_cmd_; //!< Store the last control command generated in computeVelocityCommands()
std::vector<geometry_msgs::Point> footprint_spec_; //!< Store the footprint of the robot
double robot_inscribed_radius_; //!< The radius of the inscribed circle of the robot (collision possible)
double robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot
std::string global_frame_; //!< The frame in which the controller will run
std::string robot_base_frame_; //!< Used as the base frame id of the robot
std::string name_; //!< For use with the ros nodehandle
// flags
bool initialized_; //!< Keeps track about the correct initialization of this class
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}; // end namespace teb_local_planner
#endif // TEB_LOCAL_PLANNER_ROS_H_

659
navigations/teb_local_planner/include/teb_local_planner/timed_elastic_band.h Executable file
View File

@@ -0,0 +1,659 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef TIMED_ELASTIC_BAND_H_
#define TIMED_ELASTIC_BAND_H_
#include <ros/ros.h>
#include <ros/assert.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <tf/tf.h>
#include <complex>
#include <iterator>
#include <teb_local_planner/obstacles.h>
// G2O Types
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/vertex_timediff.h>
namespace teb_local_planner
{
//! Container of poses that represent the spatial part of the trajectory
typedef std::vector<VertexPose*> PoseSequence;
//! Container of time differences that define the temporal of the trajectory
typedef std::vector<VertexTimeDiff*> TimeDiffSequence;
/**
* @class TimedElasticBand
* @brief Class that defines a trajectory modeled as an elastic band with augmented tempoarl information.
*
* All trajectory related methods (initialization, modifying, ...) are implemented inside this class. \n
* Let \f$ Q = \lbrace \mathbf{s}_i \rbrace_{i=0...n},\ n \in \mathbb{N} \f$ be a sequence of poses, \n
* in which \f$ \mathbf{s}_i = [x_i, y_i, \beta_i]^T \in \mathbb{R}^2 \times S^1 \f$ denotes a single pose of the robot. \n
* The Timed Elastic Band (TEB) augments this sequence of poses by incorporating time intervals between
* two consecutive poses, resuting in a sequence of \c n-1 time intervals \f$ \Delta T_i \f$: \n
* \f$ \tau = \lbrace \Delta T_i \rbrace_{i=0...n-1} \f$. \n
* Each time interval (time diff) denotes the time that the robot requires to transit from the current configuration to the next configuration.
* The tuple of both sequences defines the underlying trajectory.
*
* Poses and time differences are wrapped into a g2o::Vertex class in order to enable the efficient optimization in TebOptimalPlanner. \n
* TebOptimalPlanner utilizes this Timed_Elastic_band class for representing an optimizable trajectory.
*
* @todo Move decision if the start or goal state should be marked as fixed or unfixed for the optimization to the TebOptimalPlanner class.
*/
class TimedElasticBand
{
public:
/**
* @brief Construct the class
*/
TimedElasticBand();
/**
* @brief Destruct the class
*/
virtual ~TimedElasticBand();
/** @name Access pose and timediff sequences */
//@{
/**
* @brief Access the complete pose sequence
* @return reference to the pose sequence
*/
PoseSequence& poses() {return pose_vec_;};
/**
* @brief Access the complete pose sequence (read-only)
* @return const reference to the pose sequence
*/
const PoseSequence& poses() const {return pose_vec_;};
/**
* @brief Access the complete timediff sequence
* @return reference to the dimediff sequence
*/
TimeDiffSequence& timediffs() {return timediff_vec_;};
/**
* @brief Access the complete timediff sequence
* @return reference to the dimediff sequence
*/
const TimeDiffSequence& timediffs() const {return timediff_vec_;};
/**
* @brief Access the time difference at pos \c index of the time sequence
* @param index element position inside the internal TimeDiffSequence
* @return reference to the time difference at pos \c index
*/
double& TimeDiff(int index)
{
ROS_ASSERT(index<sizeTimeDiffs());
return timediff_vec_.at(index)->dt();
}
/**
* @brief Access the time difference at pos \c index of the time sequence (read-only)
* @param index element position inside the internal TimeDiffSequence
* @return const reference to the time difference at pos \c index
*/
const double& TimeDiff(int index) const
{
ROS_ASSERT(index<sizeTimeDiffs());
return timediff_vec_.at(index)->dt();
}
/**
* @brief Access the pose at pos \c index of the pose sequence
* @param index element position inside the internal PoseSequence
* @return reference to the pose at pos \c index
*/
PoseSE2& Pose(int index)
{
ROS_ASSERT(index<sizePoses());
return pose_vec_.at(index)->pose();
}
/**
* @brief Access the pose at pos \c index of the pose sequence (read-only)
* @param index element position inside the internal PoseSequence
* @return const reference to the pose at pos \c index
*/
const PoseSE2& Pose(int index) const
{
ROS_ASSERT(index<sizePoses());
return pose_vec_.at(index)->pose();
}
/**
* @brief Access the last PoseSE2 in the pose sequence
*/
PoseSE2& BackPose() {return pose_vec_.back()->pose(); }
/**
* @brief Access the last PoseSE2 in the pose sequence (read-only)
*/
const PoseSE2& BackPose() const {return pose_vec_.back()->pose();}
/**
* @brief Access the last TimeDiff in the time diff sequence
*/
double& BackTimeDiff() {return timediff_vec_.back()->dt(); }
/**
* @brief Access the last TimeDiff in the time diff sequence (read-only)
*/
const double& BackTimeDiff() const {return timediff_vec_.back()->dt(); }
/**
* @brief Access the vertex of a pose at pos \c index for optimization purposes
* @param index element position inside the internal PoseSequence
* @return Weak raw pointer to the pose vertex at pos \c index
*/
VertexPose* PoseVertex(int index)
{
ROS_ASSERT(index<sizePoses());
return pose_vec_.at(index);
}
/**
* @brief Access the vertex of a time difference at pos \c index for optimization purposes
* @param index element position inside the internal TimeDiffSequence
* @return Weak raw pointer to the timediff vertex at pos \c index
*/
VertexTimeDiff* TimeDiffVertex(int index)
{
ROS_ASSERT(index<sizeTimeDiffs());
return timediff_vec_.at(index);
}
//@}
/** @name Append new elements to the pose and timediff sequences */
//@{
/**
* @brief Append a new pose vertex to the back of the pose sequence
* @param pose PoseSE2 to push back on the internal PoseSequence
* @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
*/
void addPose(const PoseSE2& pose, bool fixed=false);
/**
* @brief Append a new pose vertex to the back of the pose sequence
* @param position 2D vector representing the position part
* @param theta yaw angle representing the orientation part
* @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
*/
void addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed=false);
/**
* @brief Append a new pose vertex to the back of the pose sequence
* @param x x-coordinate of the position part
* @param y y-coordinate of the position part
* @param theta yaw angle representing the orientation part
* @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
*/
void addPose(double x, double y, double theta, bool fixed=false);
/**
* @brief Append a new time difference vertex to the back of the time diff sequence
* @param dt time difference value to push back on the internal TimeDiffSequence
* @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
*/
void addTimeDiff(double dt, bool fixed=false);
/**
* @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
* @param pose PoseSE2 to push back on the internal PoseSequence
* @param dt time difference value to push back on the internal TimeDiffSequence
* @warning Since the timediff is defined to connect two consecutive poses, this call is only
* allowed if there exist already n poses and n-1 timediffs in the sequences (n=1,2,...):
* therefore add a single pose using addPose() first!
*/
void addPoseAndTimeDiff(const PoseSE2& pose, double dt);
/**
* @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
* @param position 2D vector representing the position part
* @param theta yaw angle representing the orientation part
* @param dt time difference value to push back on the internal TimeDiffSequence
* @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt)
*/
void addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt);
/**
* @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
* @param x x-coordinate of the position part
* @param y y-coordinate of the position part
* @param theta yaw angle representing the orientation part
* @param dt time difference value to push back on the internal TimeDiffSequence
* @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt)
*/
void addPoseAndTimeDiff(double x, double y, double theta, double dt);
//@}
/** @name Insert new elements and remove elements of the pose and timediff sequences */
//@{
/**
* @brief Insert a new pose vertex at pos. \c index to the pose sequence
* @param index element position inside the internal PoseSequence
* @param pose PoseSE2 element to insert into the internal PoseSequence
*/
void insertPose(int index, const PoseSE2& pose);
/**
* @brief Insert a new pose vertex at pos. \c index to the pose sequence
* @param index element position inside the internal PoseSequence
* @param position 2D vector representing the position part
* @param theta yaw-angle representing the orientation part
*/
void insertPose(int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta);
/**
* @brief Insert a new pose vertex at pos. \c index to the pose sequence
* @param index element position inside the internal PoseSequence
* @param x x-coordinate of the position part
* @param y y-coordinate of the position part
* @param theta yaw-angle representing the orientation part
*/
void insertPose(int index, double x, double y, double theta);
/**
* @brief Insert a new timediff vertex at pos. \c index to the timediff sequence
* @param index element position inside the internal TimeDiffSequence
* @param dt timediff value
*/
void insertTimeDiff(int index, double dt);
/**
* @brief Delete pose at pos. \c index in the pose sequence
* @param index element position inside the internal PoseSequence
*/
void deletePose(int index);
/**
* @brief Delete multiple (\c number) poses starting at pos. \c index in the pose sequence
* @param index first element position inside the internal PoseSequence
* @param number number of elements that should be deleted
*/
void deletePoses(int index, int number);
/**
* @brief Delete pose at pos. \c index in the timediff sequence
* @param index element position inside the internal TimeDiffSequence
*/
void deleteTimeDiff(int index);
/**
* @brief Delete multiple (\c number) time differences starting at pos. \c index in the timediff sequence
* @param index first element position inside the internal TimeDiffSequence
* @param number number of elements that should be deleted
*/
void deleteTimeDiffs(int index, int number);
//@}
/** @name Init the trajectory */
//@{
/**
* @brief Initialize a trajectory between a given start and goal pose.
*
* The implemented algorithm subsamples the straight line between
* start and goal using a given discretiziation width. \n
* The discretization width can be defined in the euclidean space
* using the \c diststep parameter. Each time difference between two consecutive
* poses is initialized to \c timestep. \n
* If the \c diststep is chosen to be zero,
* the resulting trajectory contains the start and goal pose only.
* @param start PoseSE2 defining the start of the trajectory
* @param goal PoseSE2 defining the goal of the trajectory (final pose)
* @param diststep euclidean distance between two consecutive poses (if 0, no intermediate samples are inserted despite min_samples)
* @param max_vel_x maximum translational velocity used for determining time differences
* @param min_samples Minimum number of samples that should be initialized at least
* @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
* @return true if everything was fine, false otherwise
*/
bool initTrajectoryToGoal(const PoseSE2& start, const PoseSE2& goal, double diststep=0, double max_vel_x=0.5, int min_samples = 3, bool guess_backwards_motion = false);
/**
* @brief Initialize a trajectory from a generic 2D reference path.
*
* The temporal information is determined using a given maximum velocity
* (2D vector containing the translational and angular velocity). \n
* A constant velocity profile is implemented. \n
* A possible maximum acceleration is considered if \c max_acceleration param is provided.
*
* Since the orientation is not included in the reference path, it can be provided seperately
* (e.g. from the robot pose and robot goal). \n
* Otherwise the goal heading will be used as start and goal orientation. \n
* The orientation along the trajectory will be determined using the connection vector
* between two consecutive positions of the reference path.
*
* The reference path is provided using a start and end iterator to a container class.
* You must provide a unary function that accepts the dereferenced iterator and returns
* a copy or (const) reference to an Eigen::Vector2d type containing the 2d position.
*
* @param path_start start iterator of a generic 2d path
* @param path_end end iterator of a generic 2d path
* @param fun_position unary function that returns the Eigen::Vector2d object
* @param max_vel_x maximum translational velocity used for determining time differences
* @param max_vel_theta maximum angular velocity used for determining time differences
* @param max_acc_x specify to satisfy a maxmimum transl. acceleration and decceleration (optional)
* @param max_acc_theta specify to satisfy a maxmimum angular acceleration and decceleration (optional)
* @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
* @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
* @param min_samples Minimum number of samples that should be initialized at least
* @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
* @tparam BidirIter Bidirectional iterator type
* @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d
* @return true if everything was fine, false otherwise
* @remarks Use \c boost::none to skip optional arguments.
*/
template<typename BidirIter, typename Fun>
bool initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples = 3, bool guess_backwards_motion = false);
/**
* @brief Initialize a trajectory from a reference pose sequence (positions and orientations).
*
* This method initializes the timed elastic band using a pose container
* (e.g. as local plan from the ros navigation stack). \n
* The initial time difference between two consecutive poses can be uniformly set
* via the argument \c dt.
* @param plan vector of geometry_msgs::PoseStamped
* @param max_vel_x maximum translational velocity used for determining time differences
* @param max_vel_theta maximum rotational velocity used for determining time differences
* @param estimate_orient if \c true, calculate orientation using the straight line distance vector between consecutive poses
* (only copy start and goal orientation; recommended if no orientation data is available).
* @param min_samples Minimum number of samples that should be initialized at least
* @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot (this parameter is used only if \c estimate_orient is enabled.
* @return true if everything was fine, false otherwise
*/
bool initTrajectoryToGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient=false, int min_samples = 3, bool guess_backwards_motion = false);
ROS_DEPRECATED bool initTEBtoGoal(const PoseSE2& start, const PoseSE2& goal, double diststep=0, double timestep=1, int min_samples = 3, bool guess_backwards_motion = false)
{
ROS_WARN_ONCE("initTEBtoGoal is deprecated and has been replaced by initTrajectoryToGoal. The signature has changed: timestep has been replaced by max_vel_x. \
this deprecated method sets max_vel_x = 1. Please update your code.");
return initTrajectoryToGoal(start, goal, diststep, timestep, min_samples, guess_backwards_motion);
}
template<typename BidirIter, typename Fun>
ROS_DEPRECATED bool initTEBtoGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples = 3, bool guess_backwards_motion = false)
{
return initTrajectoryToGoal<BidirIter, Fun>(path_start, path_end, fun_position, max_vel_x, max_vel_theta,
max_acc_x, max_acc_theta, start_orientation, goal_orientation, min_samples, guess_backwards_motion);
}
ROS_DEPRECATED bool initTEBtoGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double dt, bool estimate_orient=false, int min_samples = 3, bool guess_backwards_motion = false)
{
ROS_WARN_ONCE("initTEBtoGoal is deprecated and has been replaced by initTrajectoryToGoal. The signature has changed: dt has been replaced by max_vel_x. \
this deprecated method sets max_vel = 1. Please update your code.");
return initTrajectoryToGoal(plan, 1.0, 1.0, estimate_orient, min_samples, guess_backwards_motion);
}
//@}
/** @name Update and modify the trajectory */
//@{
/**
* @brief Hot-Start from an existing trajectory with updated start and goal poses.
*
* This method updates a previously optimized trajectory with a new start and/or a new goal pose. \n
* The current simple implementation cuts of pieces of the trajectory that are already passed due to the new start. \n
* Afterwards the start and goal pose are replaced by the new ones. The resulting discontinuity will not be smoothed.
* The optimizer has to smooth the trajectory in TebOptimalPlanner. \n
*
* @todo Smooth the trajectory here and test the performance improvement of the optimization.
* @todo Implement a updateAndPruneTEB based on a new reference path / pose sequence.
*
* @param new_start New start pose (optional)
* @param new_goal New goal pose (optional)
* @param min_samples Specify the minimum number of samples that should at least remain in the trajectory
*/
void updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples = 3);
/**
* @brief Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution.
*
* Resizing the trajectory is helpful e.g. for the following scenarios:
*
* - Obstacles requires the teb to be extended in order to
* satisfy the given discritization width (plan resolution)
* and to avoid undesirable behavior due to a large/small discretization step widths \f$ \Delta T_i \f$
* After clearance of obstacles, the teb should (re-) contract to its (time-)optimal version.
* - If the distance to the goal state is getting smaller,
* dt is decreasing as well. This leads to a heavily
* fine-grained discretization in combination with many
* discrete poses. Thus, the computation time will
* be/remain high and in addition numerical instabilities
* can appear (e.g. due to the division by a small \f$ \Delta T_i \f$).
*
* The implemented strategy checks all timediffs \f$ \Delta T_i \f$ and
*
* - inserts a new sample if \f$ \Delta T_i > \Delta T_{ref} + \Delta T_{hyst} \f$
* - removes a sample if \f$ \Delta T_i < \Delta T_{ref} - \Delta T_{hyst} \f$
*
* Each call only one new sample (pose-dt-pair) is inserted or removed.
* @param dt_ref reference temporal resolution
* @param dt_hysteresis hysteresis to avoid oscillations
* @param min_samples minimum number of samples that should be remain in the trajectory after resizing
* @param max_samples maximum number of samples that should not be exceeded during resizing
* @param fast_mode if true, the trajectory is iterated once to insert or erase points; if false the trajectory
* is repeatedly iterated until no poses are added or removed anymore
*/
void autoResize(double dt_ref, double dt_hysteresis, int min_samples = 3, int max_samples=1000, bool fast_mode=false);
/**
* @brief Set a pose vertex at pos \c index of the pose sequence to be fixed or unfixed during optimization.
* @param index index to the pose vertex
* @param status if \c true, the vertex will be fixed, otherwise unfixed
*/
void setPoseVertexFixed(int index, bool status);
/**
* @brief Set a timediff vertex at pos \c index of the timediff sequence to be fixed or unfixed during optimization.
* @param index index to the timediff vertex
* @param status if \c true, the vertex will be fixed, otherwise unfixed
*/
void setTimeDiffVertexFixed(int index, bool status);
/**
* @brief clear all poses and timediffs from the trajectory.
* The pose and timediff sequences will be empty and isInit() will return \c false
*/
void clearTimedElasticBand();
//@}
/** @name Utility and status methods */
//@{
/**
* @brief Find the closest point on the trajectory w.r.t. to a provided reference point.
*
* This function can be useful to find the part of a trajectory that is close to an obstacle.
*
* @todo implement a more efficient version that first performs a coarse search.
* @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
* Allows simple comparisons starting from the middle of the trajectory.
*
* @param ref_point reference point (2D position vector)
* @param[out] distance [optional] the resulting minimum distance
* @param begin_idx start search at this pose index
* @return Index to the closest pose in the pose sequence
*/
int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance = NULL, int begin_idx=0) const;
/**
* @brief Find the closest point on the trajectory w.r.t. to a provided reference line.
*
* This function can be useful to find the part of a trajectory that is close to an (line) obstacle.
*
* @todo implement a more efficient version that first performs a coarse search.
* @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
* Allows simple comparisons starting from the middle of the trajectory.
*
* @param ref_line_start start of the reference line (2D position vector)
* @param ref_line_end end of the reference line (2D position vector)
* @param[out] distance [optional] the resulting minimum distance
* @return Index to the closest pose in the pose sequence
*/
int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_line_start, const Eigen::Ref<const Eigen::Vector2d>& ref_line_end, double* distance = NULL) const;
/**
* @brief Find the closest point on the trajectory w.r.t. to a provided reference polygon.
*
* This function can be useful to find the part of a trajectory that is close to an (polygon) obstacle.
*
* @todo implement a more efficient version that first performs a coarse search.
* @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
* Allows simple comparisons starting from the middle of the trajectory.
*
* @param vertices vertex container containing Eigen::Vector2d points (the last and first point are connected)
* @param[out] distance [optional] the resulting minimum distance
* @return Index to the closest pose in the pose sequence
*/
int findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance = NULL) const;
/**
* @brief Find the closest point on the trajectory w.r.t to a provided obstacle type
*
* This function can be useful to find the part of a trajectory that is close to an obstacle.
* The method is calculates appropriate distance metrics for point, line and polygon obstacles.
* For all unknown obstacles the centroid is used.
*
* @param obstacle Subclass of the Obstacle base class
* @param[out] distance [optional] the resulting minimum distance
* @return Index to the closest pose in the pose sequence
*/
int findClosestTrajectoryPose(const Obstacle& obstacle, double* distance = NULL) const;
/**
* @brief Get the length of the internal pose sequence
*/
int sizePoses() const {return (int)pose_vec_.size();};
/**
* @brief Get the length of the internal timediff sequence
*/
int sizeTimeDiffs() const {return (int)timediff_vec_.size();};
/**
* @brief Check whether the trajectory is initialized (nonzero pose and timediff sequences)
*/
bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();}
/**
* @brief Calculate the total transition time (sum over all time intervals of the timediff sequence)
*/
double getSumOfAllTimeDiffs() const;
/**
* @brief Calculate the estimated transition time up to the pose denoted by index
* @param index Index of the pose up to which the transition times are summed up
* @return Estimated transition time up to pose index
*/
double getSumOfTimeDiffsUpToIdx(int index) const;
/**
* @brief Calculate the length (accumulated euclidean distance) of the trajectory
*/
double getAccumulatedDistance() const;
/**
* @brief Check if all trajectory points are contained in a specific region
*
* The specific region is a circle around the current robot position (Pose(0)) with given radius \c radius.
* This method investigates a different radius for points behind the robot if \c max_dist_behind_robot >= 0.
* @param radius radius of the region with the robot position (Pose(0)) as center
* @param max_dist_behind_robot A separate radius for trajectory points behind the robot, activated if 0 or positive
* @param skip_poses If >0: the specified number of poses are skipped for the test, e.g. Pose(0), Pose(0+skip_poses+1), Pose(2*skip_poses+2), ... are tested.
* @return \c true, if all tested trajectory points are inside the specified region, \c false otherwise.
*/
bool isTrajectoryInsideRegion(double radius, double max_dist_behind_robot=-1, int skip_poses=0);
//@}
protected:
PoseSequence pose_vec_; //!< Internal container storing the sequence of optimzable pose vertices
TimeDiffSequence timediff_vec_; //!< Internal container storing the sequence of optimzable timediff vertices
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace teb_local_planner
// include template implementations / definitions
#include <teb_local_planner/timed_elastic_band.hpp>
#endif /* TIMED_ELASTIC_BAND_H_ */

189
navigations/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp Executable file
View File

@@ -0,0 +1,189 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/timed_elastic_band.h>
namespace teb_local_planner
{
template<typename BidirIter, typename Fun>
bool TimedElasticBand::initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples, bool guess_backwards_motion)
{
Eigen::Vector2d start_position = fun_position( *path_start );
Eigen::Vector2d goal_position = fun_position( *boost::prior(path_end) );
bool backwards = false;
double start_orient, goal_orient;
if (start_orientation)
{
start_orient = *start_orientation;
// check if the goal is behind the start pose (w.r.t. start orientation)
if (guess_backwards_motion && (goal_position-start_position).dot(Eigen::Vector2d(std::cos(start_orient), std::sin(start_orient))) < 0)
backwards = true;
}
else
{
Eigen::Vector2d start2goal = goal_position - start_position;
start_orient = atan2(start2goal[1],start2goal[0]);
}
double timestep = 1; // TODO: time
if (goal_orientation)
{
goal_orient = *goal_orientation;
}
else
{
goal_orient = start_orient;
}
if (!isInit())
{
addPose(start_position, start_orient, true); // add starting point and mark it as fixed for optimization
// we insert middle points now (increase start by 1 and decrease goal by 1)
std::advance(path_start,1);
std::advance(path_end,-1);
int idx=0;
for (; path_start != path_end; ++path_start) // insert middle-points
{
//Eigen::Vector2d point_to_goal = path.back()-*it;
//double dir_to_goal = atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
// Alternative: Direction from last path
Eigen::Vector2d curr_point = fun_position(*path_start);
Eigen::Vector2d diff_last = curr_point - Pose(idx).position(); // we do not use boost::prior(*path_start) for those cases,
// where fun_position() does not return a reference or is expensive.
double diff_norm = diff_last.norm();
double timestep_vel = diff_norm/max_vel_x; // constant velocity
double timestep_acc;
if (max_acc_x)
{
timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc;
else timestep = timestep_vel;
}
else timestep = timestep_vel;
if (timestep<=0) timestep=0.2; // TODO: this is an assumption
double yaw = atan2(diff_last[1],diff_last[0]);
if (backwards)
yaw = g2o::normalize_theta(yaw + M_PI);
addPoseAndTimeDiff(curr_point, yaw ,timestep);
/*
// TODO: the following code does not seem to hot-start the optimizer. Instead it recudes convergence time.
Eigen::Vector2d diff_next = fun_position(*boost::next(path_start))-curr_point; // TODO maybe store the boost::next for the following iteration
double ang_diff = std::abs( g2o::normalize_theta( atan2(diff_next[1],diff_next[0])
-atan2(diff_last[1],diff_last[0]) ) );
timestep_vel = ang_diff/max_vel_theta; // constant velocity
if (max_acc_theta)
{
timestep_acc = sqrt(2*ang_diff/(*max_acc_theta)); // constant acceleration
if (timestep_vel < timestep_acc) timestep = timestep_acc;
else timestep = timestep_vel;
}
else timestep = timestep_vel;
if (timestep<=0) timestep=0.2; // TODO: this is an assumption
yaw = atan2(diff_last[1],diff_last[0]); // TODO redundant right now, not yet finished
if (backwards)
yaw = g2o::normalize_theta(yaw + M_PI);
addPoseAndTimeDiff(curr_point, yaw ,timestep);
*/
++idx;
}
Eigen::Vector2d diff = goal_position-Pose(idx).position();
double diff_norm = diff.norm();
double timestep_vel = diff_norm/max_vel_x; // constant velocity
if (max_acc_x)
{
double timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
if (timestep_vel < timestep_acc) timestep = timestep_acc;
else timestep = timestep_vel;
}
else timestep = timestep_vel;
PoseSE2 goal(goal_position, goal_orient);
// if number of samples is not larger than min_samples, insert manually
if ( sizePoses() < min_samples-1 )
{
ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
while (sizePoses() < min_samples-1) // subtract goal point that will be added later
{
// Each inserted point bisects the remaining distance. Thus the timestep is also bisected.
timestep /= 2;
// simple strategy: interpolate between the current pose and the goal
addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep ); // let the optimier correct the timestep (TODO: better initialization
}
}
// now add goal
addPoseAndTimeDiff(goal, timestep); // add goal point
setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
}
else // size!=0
{
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)!");
ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs());
return false;
}
return true;
}
} // namespace teb_local_planner

287
navigations/teb_local_planner/include/teb_local_planner/visualization.h Executable file
View File

@@ -0,0 +1,287 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#ifndef VISUALIZATION_H_
#define VISUALIZATION_H_
// teb stuff
#include <teb_local_planner/teb_config.h>
#include <teb_local_planner/timed_elastic_band.h>
#include <teb_local_planner/robot_footprint_model.h>
// ros stuff
#include <ros/publisher.h>
#include <base_local_planner/goal_functions.h>
// boost
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
// std
#include <iterator>
// messages
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <std_msgs/ColorRGBA.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
namespace teb_local_planner
{
class TebOptimalPlanner; //!< Forward Declaration
/**
* @class TebVisualization
* @brief Visualize stuff from the teb_local_planner
*/
class TebVisualization
{
public:
/**
* @brief Default constructor
* @remarks do not forget to call initialize()
*/
TebVisualization();
/**
* @brief Constructor that initializes the class and registers topics
* @param nh local ros::NodeHandle
* @param cfg const reference to the TebConfig class for parameters
*/
TebVisualization(ros::NodeHandle& nh, const TebConfig& cfg);
/**
* @brief Initializes the class and registers topics.
*
* Call this function if only the default constructor has been called before.
* @param nh local ros::NodeHandle
* @param cfg const reference to the TebConfig class for parameters
*/
void initialize(ros::NodeHandle& nh, const TebConfig& cfg);
/** @name Publish to topics */
//@{
/**
* @brief Publish a given global plan to the ros topic \e ../../global_plan
* @param global_plan Pose array describing the global plan
*/
void publishGlobalPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan) const;
/**
* @brief Publish a given local plan to the ros topic \e ../../local_plan
* @param local_plan Pose array describing the local plan
*/
void publishLocalPlan(const std::vector<geometry_msgs::PoseStamped>& local_plan) const;
/**
* @brief Publish Timed_Elastic_Band related stuff (local plan, pose sequence).
*
* Given a Timed_Elastic_Band instance, publish the local plan to \e ../../local_plan
* and the pose sequence to \e ../../teb_poses.
* @param teb const reference to a Timed_Elastic_Band
*/
void publishLocalPlanAndPoses(const TimedElasticBand& teb) const;
/**
* @brief Publish the visualization of the robot model
*
* @param current_pose Current pose of the robot
* @param robot_model Subclass of BaseRobotFootprintModel
* @param ns Namespace for the marker objects
* @param color Color of the footprint
*/
void publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns = "RobotFootprintModel",
const std_msgs::ColorRGBA& color = toColorMsg(0.5, 0.0, 0.8, 0.0));
void publishRobotFootprint(const PoseSE2& current_pose, const std::vector<geometry_msgs::Point>& footprint,
const std::string& ns, const std_msgs::ColorRGBA &color);
/**
* @brief Publish the robot footprint model and the actual robot footprint at an infeasible pose
*
* @param infeasible_pose Infeasible pose to visualize
* @param robot_model Subclass of BaseRobotFootprintModel
* @param footprint Actual robot footprint
*/
void publishInfeasibleRobotPose(const PoseSE2& infeasible_pose, const BaseRobotFootprintModel& robot_model,
const std::vector<geometry_msgs::Point>& footprint);
/**
* @brief Publish obstacle positions to the ros topic \e ../../teb_markers
* @todo Move filling of the marker message to polygon class in order to avoid checking types.
* @param obstacles Obstacle container
* @param scale Size of the non-circular obstacles
*/
void publishObstacles(const ObstContainer& obstacles, double scale = 0.1) const;
/**
* @brief Publish via-points to the ros topic \e ../../teb_markers
* @param via_points via-point container
*/
void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns = "ViaPoints") const;
/**
* @brief Publish a boost::adjacency_list (boost's graph datatype) via markers.
* @remarks Make sure that vertices of the graph contain a member \c pos as \c Eigen::Vector2d type
* to query metric position values.
* @param graph Const reference to the boost::adjacency_list (graph)
* @param ns_prefix Namespace prefix for the marker objects (the strings "Edges" and "Vertices" will be appended)
* @tparam GraphType boost::graph object in which vertices has the field/member \c pos.
*/
template <typename GraphType>
void publishGraph(const GraphType& graph, const std::string& ns_prefix = "Graph");
/**
* @brief Publish multiple 2D paths (each path given as a point sequence) from a container class.
*
* Provide a std::vector< std::vector< T > > in which T.x() and T.y() exist
* and std::vector could be individually substituded by std::list / std::deque /...
*
* A common point-type for object T could be Eigen::Vector2d.
*
* T could be also a raw pointer std::vector< std::vector< T* > >.
*
* @code
* typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > PathType; // could be a list or deque as well ...
* std::vector<PathType> path_container(2); // init 2 empty paths; the container could be a list or deque as well ...
* // Fill path_container.at(0) with Eigen::Vector2d elements, we skip that here
* // Fill path_container.at(1) with Eigen::Vector2d elements, we skip that here
* publishPathContainer( path_container.begin(), path_container.end() );
* @endcode
*
* @remarks Actually the underlying path does not necessarily need to be a Eigen::Vector2d sequence.
* Eigen::Vector2d can be replaced with any datatype that implement public x() and y() methods.\n
* @param first Bidirectional iterator pointing to the begin of the path
* @param last Bidirectional iterator pointing to the end of the path
* @param ns Namespace for the marker objects (the strings "Edges" and "Vertices" will be appended)
* @tparam BidirIter Bidirectional iterator to a 2D path (sequence of Eigen::Vector2d elements) in a container
*/
template <typename BidirIter>
void publishPathContainer(BidirIter first, BidirIter last, const std::string& ns = "PathContainer");
/**
* @brief Publish multiple Tebs from a container class (publish as marker message).
*
* @param teb_planner Container of boost::shared_ptr< TebOptPlannerPtr >
* @param ns Namespace for the marker objects
*/
void publishTebContainer(const std::vector< boost::shared_ptr<TebOptimalPlanner> >& teb_planner, const std::string& ns = "TebContainer");
/**
* @brief Publish a feedback message (multiple trajectory version)
*
* The feedback message contains the all planned trajectory candidates (e.g. if planning in distinctive topologies is turned on).
* Each trajectory is composed of the sequence of poses, the velocity profile and temporal information.
* The feedback message also contains a list of active obstacles.
* @param teb_planners container with multiple tebs (resp. their planner instances)
* @param selected_trajectory_idx Idx of the currently selected trajectory in \c teb_planners
* @param obstacles Container of obstacles
*/
void publishFeedbackMessage(const std::vector< boost::shared_ptr<TebOptimalPlanner> >& teb_planners, unsigned int selected_trajectory_idx, const ObstContainer& obstacles);
/**
* @brief Publish a feedback message (single trajectory overload)
*
* The feedback message contains the planned trajectory
* that is composed of the sequence of poses, the velocity profile and temporal information.
* The feedback message also contains a list of active obstacles.
* @param teb_planner the planning instance
* @param obstacles Container of obstacles
*/
void publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles);
//@}
/**
* @brief Helper function to generate a color message from single values
* @param a Alpha value
* @param r Red value
* @param g Green value
* @param b Blue value
* @return Color message
*/
static std_msgs::ColorRGBA toColorMsg(double a, double r, double g, double b);
protected:
/**
* @brief Small helper function that checks if initialize() has been called and prints an error message if not.
* @return \c true if not initialized, \c false if everything is ok
*/
bool printErrorWhenNotInitialized() const;
ros::Publisher global_plan_pub_; //!< Publisher for the global plan
ros::Publisher local_plan_pub_; //!< Publisher for the local plan
ros::Publisher teb_poses_pub_; //!< Publisher for the trajectory pose sequence
ros::Publisher teb_marker_pub_; //!< Publisher for visualization markers
ros::Publisher feedback_pub_; //!< Publisher for the feedback message for analysis and debug purposes
const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
bool initialized_; //!< Keeps track about the correct initialization of this class
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
//! Abbrev. for shared instances of the TebVisualization
typedef boost::shared_ptr<TebVisualization> TebVisualizationPtr;
//! Abbrev. for shared instances of the TebVisualization (read-only)
typedef boost::shared_ptr<const TebVisualization> TebVisualizationConstPtr;
} // namespace teb_local_planner
// Include template method implementations / definitions
#include <teb_local_planner/visualization.hpp>
#endif /* VISUALIZATION_H_ */

225
navigations/teb_local_planner/include/teb_local_planner/visualization.hpp Executable file
View File

@@ -0,0 +1,225 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/visualization.h>
#include <boost/utility.hpp>
namespace teb_local_planner
{
template <typename GraphType>
void TebVisualization::publishGraph(const GraphType& graph, const std::string& ns_prefix)
{
if ( printErrorWhenNotInitialized() )
return;
typedef typename boost::graph_traits<GraphType>::vertex_iterator GraphVertexIterator;
typedef typename boost::graph_traits<GraphType>::edge_iterator GraphEdgeIterator;
// Visualize Edges
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = ns_prefix + "Edges";
marker.id = 0;
// #define TRIANGLE
#ifdef TRIANGLE
marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
#else
marker.type = visualization_msgs::Marker::LINE_LIST;
#endif
marker.action = visualization_msgs::Marker::ADD;
GraphEdgeIterator it_edge, end_edges;
for (boost::tie(it_edge,end_edges) = boost::edges(graph); it_edge!=end_edges; ++it_edge)
{
#ifdef TRIANGLE
geometry_msgs::Point point_start1;
point_start1.x = graph[boost::source(*it_edge,graph)].pos[0]+0.05;
point_start1.y = graph[boost::source(*it_edge,graph)].pos[1]-0.05;
point_start1.z = 0;
marker.points.push_back(point_start1);
geometry_msgs::Point point_start2;
point_start2.x = graph[boost::source(*it_edge,graph)].pos[0]-0.05;
point_start2.y = graph[boost::source(*it_edge,graph)].pos[1]+0.05;
point_start2.z = 0;
marker.points.push_back(point_start2);
#else
geometry_msgs::Point point_start;
point_start.x = graph[boost::source(*it_edge,graph)].pos[0];
point_start.y = graph[boost::source(*it_edge,graph)].pos[1];
point_start.z = 0;
marker.points.push_back(point_start);
#endif
geometry_msgs::Point point_end;
point_end.x = graph[boost::target(*it_edge,graph)].pos[0];
point_end.y = graph[boost::target(*it_edge,graph)].pos[1];
point_end.z = 0;
marker.points.push_back(point_end);
// add color
std_msgs::ColorRGBA color;
color.a = 1.0;
color.r = 0;
color.g = 0;
color.b = 1;
marker.colors.push_back(color);
marker.colors.push_back(color);
#ifdef TRIANGLE
marker.colors.push_back(color);
#endif
}
#ifdef TRIANGLE
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 1;
#else
marker.scale.x = 0.01;
#endif
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
// Now publish edge markers
teb_marker_pub_.publish( marker );
// Visualize vertices
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = ns_prefix + "Vertices";
marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
GraphVertexIterator it_vert, end_vert;
for (boost::tie(it_vert,end_vert) = boost::vertices(graph); it_vert!=end_vert; ++it_vert)
{
geometry_msgs::Point point;
point.x = graph[*it_vert].pos[0];
point.y = graph[*it_vert].pos[1];
point.z = 0;
marker.points.push_back(point);
// add color
std_msgs::ColorRGBA color;
color.a = 1.0;
if (it_vert==end_vert-1)
{
color.r = 1;
color.g = 0;
color.b = 0;
}
else
{
color.r = 0;
color.g = 1;
color.b = 0;
}
marker.colors.push_back(color);
}
// set first color (start vertix) to blue
if (!marker.colors.empty())
{
marker.colors.front().b = 1;
marker.colors.front().g = 0;
}
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
// Now publish vertex markers
teb_marker_pub_.publish( marker );
}
template <typename BidirIter>
void TebVisualization::publishPathContainer(BidirIter first, BidirIter last, const std::string& ns)
{
if ( printErrorWhenNotInitialized() )
return;
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_LIST;
marker.action = visualization_msgs::Marker::ADD;
typedef typename std::iterator_traits<BidirIter>::value_type PathType; // Get type of the path (point container)
// Iterate through path container
while(first != last)
{
// iterate single path points
typename PathType::const_iterator it_point, end_point;
for (it_point = first->begin(), end_point = boost::prior(first->end()); it_point != end_point; ++it_point)
{
geometry_msgs::Point point_start;
point_start.x = get_const_reference(*it_point).x();
point_start.y = get_const_reference(*it_point).y();
point_start.z = 0;
marker.points.push_back(point_start);
geometry_msgs::Point point_end;
point_end.x = get_const_reference(*boost::next(it_point)).x();
point_end.y = get_const_reference(*boost::next(it_point)).y();
point_end.z = 0;
marker.points.push_back(point_end);
}
++first;
}
marker.scale.x = 0.01;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
teb_marker_pub_.publish( marker );
}
} // namespace teb_local_planner

10
navigations/teb_local_planner/launch/test_optim_node.launch Executable file
View File

@@ -0,0 +1,10 @@
<launch>
<!--- Run optimization test node -->
<node pkg="teb_local_planner" type="test_optim_node" name="test_optim_node" output="screen" />
<!-- RVIZ -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find teb_local_planner)/cfg/rviz_test_optim.rviz" />
</launch>

15
navigations/teb_local_planner/msg/FeedbackMsg.msg Executable file
View File

@@ -0,0 +1,15 @@
# Message that contains intermediate results
# and diagnostics of the (predictive) planner.
std_msgs/Header header
# The planned trajectory (or if multiple plans exist, all of them)
teb_local_planner/TrajectoryMsg[] trajectories
# Index of the trajectory in 'trajectories' that is selected currently
uint16 selected_trajectory_idx
# List of active obstacles
costmap_converter/ObstacleArrayMsg obstacles_msg

6
navigations/teb_local_planner/msg/TrajectoryMsg.msg Executable file
View File

@@ -0,0 +1,6 @@
# Message that contains a trajectory for mobile robot navigation
std_msgs/Header header
teb_local_planner/TrajectoryPointMsg[] trajectory

21
navigations/teb_local_planner/msg/TrajectoryPointMsg.msg Executable file
View File

@@ -0,0 +1,21 @@
# Message that contains single point on a trajectory suited for mobile navigation.
# The trajectory is described by a sequence of poses, velocities,
# accelerations and temporal information.
# Why this message type?
# nav_msgs/Path describes only a path without temporal information.
# trajectory_msgs package contains only messages for joint trajectories.
# The pose of the robot
geometry_msgs/Pose pose
# Corresponding velocity
geometry_msgs/Twist velocity
# Corresponding acceleration
geometry_msgs/Twist acceleration
duration time_from_start

57
navigations/teb_local_planner/package.xml Executable file
View File

@@ -0,0 +1,57 @@
<?xml version="1.0"?>
<package format="2">
<name>teb_local_planner</name>
<version>0.9.1</version>
<description>
The teb_local_planner package implements a plugin
to the base_local_planner of the 2D navigation stack.
The underlying method called Timed Elastic Band locally optimizes
the robot's trajectory with respect to trajectory execution time,
separation from obstacles and compliance with kinodynamic constraints at runtime.
</description>
<maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
<license>BSD</license>
<url type="website">http://wiki.ros.org/teb_local_planner</url>
<author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>tf2_eigen</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<build_export_depend>message_runtime</build_export_depend>
<exec_depend>message_runtime</exec_depend>
<depend>base_local_planner</depend>
<depend>costmap_2d</depend>
<depend>costmap_converter</depend>
<depend>dynamic_reconfigure</depend>
<depend>geometry_msgs</depend>
<depend>interactive_markers</depend>
<depend>libg2o</depend>
<depend>nav_core</depend>
<depend>nav_msgs</depend>
<depend>mbf_costmap_core</depend>
<depend>mbf_msgs</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<export>
<nav_core plugin="${prefix}/teb_local_planner_plugin.xml"/>
<mbf_costmap_core plugin="${prefix}/teb_local_planner_plugin.xml"/>
</export>
</package>

65
navigations/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py Executable file
View File

@@ -0,0 +1,65 @@
#!/usr/bin/env python
# Author: christoph.roesmann@tu-dortmund.de
import rospy, math
from geometry_msgs.msg import Twist
from ackermann_msgs.msg import AckermannDriveStamped
def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase):
if omega == 0 or v == 0:
return 0
radius = v / omega
return math.atan(wheelbase / radius)
def cmd_callback(data):
global wheelbase
global ackermann_cmd_topic
global frame_id
global pub
global cmd_angle_instead_rotvel
v = data.linear.x
# if cmd_angle_instead_rotvel is true, the rotational velocity is already converted in the C++ node
# in this case this script only needs to do the msg conversion from twist to Ackermann drive
if cmd_angle_instead_rotvel:
steering = data.angular.z
else:
steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.drive.steering_angle = steering
msg.drive.speed = v
pub.publish(msg)
if __name__ == '__main__':
try:
rospy.init_node('cmd_vel_to_ackermann_drive')
twist_cmd_topic = rospy.get_param('~twist_cmd_topic', '/cmd_vel')
ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic', '/ackermann_cmd')
wheelbase = rospy.get_param('~wheelbase', 1.0)
frame_id = rospy.get_param('~frame_id', 'odom')
cmd_angle_instead_rotvel = rospy.get_param('/move_base/TebLocalPlannerROS/cmd_angle_instead_rotvel', False)
rospy.Subscriber(twist_cmd_topic, Twist, cmd_callback, queue_size=1)
pub = rospy.Publisher(ackermann_cmd_topic, AckermannDriveStamped, queue_size=1)
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)
rospy.spin()
except rospy.ROSInterruptException:
pass

112
navigations/teb_local_planner/scripts/export_to_mat.py Executable file
View File

@@ -0,0 +1,112 @@
#!/usr/bin/env python
# This small script subscribes to the FeedbackMsg message of teb_local_planner
# and exports data to a mat file.
# publish_feedback must be turned on such that the planner publishes this information.
# Author: christoph.roesmann@tu-dortmund.de
import rospy, math
from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
from tf.transformations import euler_from_quaternion
import numpy as np
import scipy.io as sio
import time
def feedback_callback(data):
global got_data
if not data.trajectories: # empty
trajectory = []
return
if got_data:
return
got_data = True
# copy trajectory
trajectories = []
for traj in data.trajectories:
trajectory = []
# # store as struct and cell array
# for point in traj.trajectory:
# (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
# pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw}
# velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z}
# time_from_start = point.time_from_start.to_sec()
# trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start})
# store as all-in-one mat
arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t
for index, point in enumerate(traj.trajectory):
arr[0,index] = point.pose.position.x
arr[1,index] = point.pose.position.y
(roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
arr[2,index] = yaw
arr[3,index] = point.velocity.linear.x
arr[4,index] = point.velocity.angular.z
arr[5,index] = point.time_from_start.to_sec()
# trajectories.append({'raw': trajectory, 'mat': arr})
trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']})
# copy obstacles
obstacles = []
for obst_id, obst in enumerate(data.obstacle_msg.obstacles):
#polygon = []
#for point in obst.polygon.points:
# polygon.append({'x': point.x, 'y': point.y, 'z': point.z})
obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y
for index, point in enumerate(obst.polygon.points):
obst_arr[0, index] = point.x
obst_arr[1, index] = point.y
obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x
obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y
#obstacles.append(polygon)
obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']})
# create main struct:
mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles}
timestr = time.strftime("%Y%m%d_%H%M%S")
filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat'
rospy.loginfo("Saving mat-file '%s'.", filename)
sio.savemat(filename, mat)
def feedback_exporter():
global got_data
rospy.init_node("export_to_mat", anonymous=True)
topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
r = rospy.Rate(2) # define rate here
while not rospy.is_shutdown():
if got_data:
rospy.loginfo("Data export completed.")
return
r.sleep()
if __name__ == '__main__':
try:
global got_data
got_data = False
feedback_exporter()
except rospy.ROSInterruptException:
pass

244
navigations/teb_local_planner/scripts/export_to_svg.py Executable file
View File

@@ -0,0 +1,244 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
========================================================================================
# This small script subscribes to the FeedbackMsg message of teb_local_planner
# and converts the current scene to a svg-image
# publish_feedback must be turned on such that the planner publishes this information.
# Author: christoph.roesmann@tu-dortmund.de
It is recommendable to start this node after initialization of TEB is completed.
Requirements:
svgwrite: A Python library to create SVG drawings. http://pypi.python.org/pypi/svgwrite
=======================================================================================
"""
import roslib;
import rospy
import svgwrite
import math
import sys
import time
import random
from svgwrite import cm, mm
from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
# ================= PARAMETERS ==================
# TODO: In case of a more general node, change parameter to ros-parameter
# Drawing parameters:
SCALE = 200 # Overall scaling: 100 pixel = 1 m
MIN_POSE_DISTANCE = 0.3 # Distance between two consecutive poses in SVG-image
SCALE_VELOCITY_VEC = 0.4 # Scaling of velocity vectors -> 1 cell = 1/SCALE_VELOCITY_VEC m/s
GRID_X_MIN = -2 # Define, how many cells your grid should contain in each direction.
GRID_X_MAX = 2
GRID_Y_MIN = -2
GRID_Y_MAX = 1
# TEB parameters:
OBSTACLE_DIST = 50 *SCALE/100 # cm
# ================= FUNCTIONS ===================
def sign(number):
"""
Signum function: get sign of a number
@param number: get sign of this number
@type number: numeric type (eg. integer)
@return: sign of number
@rtype: integer {1, -1, 0}
"""
return cmp(number,0)
def arrowMarker(color='green', orientation='auto'):
"""
Create an arrow marker with svgwrite
@return: arrow marker
@rtype: svg_write marker object
"""
arrow = svg.marker(insert=(1,5), size=(4,3), orient=orientation)
arrow.viewbox(width=10, height=10)
arrow.add(svg.polyline([(0,0),(10,5),(0,10),(1,5)], fill=color, opacity=1.0))
svg.defs.add(arrow)
return arrow
def quaternion2YawDegree(orientation):
"""
Get yaw angle [degree] from quaternion representation
@param orientation: orientation in quaternions to read from
@type orientation: geometry_msgs/Quaternion
@return: yaw angle [degree]
@rtype: float
"""
yawRad = math.atan2(2*(orientation.x*orientation.y+orientation.z*orientation.w),1-2*(pow(orientation.y,2)+pow(orientation.z,2)))
return yawRad*180/math.pi
def feedback_callback(data):
"""
Callback for receiving TEB and obstacle information
@param data: Received feedback message
@type data: visualization_msgs/Marker
@globalparam tebList: Received TEB List
@globaltype tebList: teb_local_planner/FeedbackMsg
"""
# TODO: Remove global variables
global feedbackMsg
if not feedbackMsg:
feedbackMsg = data
rospy.loginfo("TEB feedback message received...")
# ================ MAIN FUNCTION ================
if __name__ == '__main__':
rospy.init_node('export_to_svg', anonymous=True)
topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
rate = rospy.Rate(10.0)
feedbackMsg = []
timestr = time.strftime("%Y%m%d_%H%M%S")
filename_string = "teb_svg_" + timestr + '.svg'
rospy.loginfo("SVG will be written to '%s'.", filename_string)
random.seed(0)
svg=svgwrite.Drawing(filename=filename_string, debug=True)
# Create viewbox -> this box defines the size of the visible drawing
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)
# Draw grid:
hLines = svg.add(svg.g(id='hLines', stroke='black'))
hLines.add(svg.line(start=(GRID_X_MIN*SCALE, 0), end=(GRID_X_MAX*SCALE, 0)))
for y in range(GRID_Y_MAX):
hLines.add(svg.line(start=(GRID_X_MIN*SCALE, SCALE+y*SCALE), end=(GRID_X_MAX*SCALE, SCALE+y*SCALE)))
for y in range(-GRID_Y_MIN):
hLines.add(svg.line(start=(GRID_X_MIN*SCALE, -SCALE-y*SCALE), end=(GRID_X_MAX*SCALE, -SCALE-y*SCALE)))
vLines = svg.add(svg.g(id='vline', stroke='black'))
vLines.add(svg.line(start=(0, GRID_Y_MIN*SCALE), end=(0, GRID_Y_MAX*SCALE)))
for x in range(GRID_X_MAX):
vLines.add(svg.line(start=(SCALE+x*SCALE, GRID_Y_MIN*SCALE), end=(SCALE+x*SCALE, GRID_Y_MAX*SCALE)))
for x in range(-GRID_X_MIN):
vLines.add(svg.line(start=(-SCALE-x*SCALE, GRID_Y_MIN*SCALE), end=(-SCALE-x*SCALE, GRID_Y_MAX*SCALE)))
# Draw legend:
legend = svg.g(id='legend', font_size=25)
stringGeometry = "Geometry: 1 Unit = 1.0m"
legendGeometry = svg.text(stringGeometry)
legend.add(legendGeometry)
legend.translate(tx=GRID_X_MIN*SCALE, ty=GRID_Y_MAX*SCALE + 30) # Move legend to buttom left corner
svg.add(legend)
#arrow = arrowMarker() # Init arrow marker
rospy.loginfo("Initialization completed.\nWaiting for feedback message...")
# -------------------- WAIT FOR CALLBACKS --------------------------
while not rospy.is_shutdown():
if feedbackMsg:
break # Leave loop after receiving all necessary TEB information (see callbacks) to finish drawing
rate.sleep()
# ------------------------------------------------------------------
if not feedbackMsg.trajectories:
rospy.loginfo("Received message does not contain trajectories. Shutting down...")
sys.exit()
if len(feedbackMsg.trajectories[0].trajectory) < 2:
rospy.loginfo("Received message does not contain trajectories with at least two states (start and goal). Shutting down...")
sys.exit()
# iterate trajectories
for index, traj in enumerate(feedbackMsg.trajectories):
#color
traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB')
# Iterate through TEB positions -> Draw Paths
points = []
for point in traj.trajectory:
points.append( (point.pose.position.x*SCALE,-point.pose.position.y*SCALE) ) # y is negative in image coordinates
# svgwrite rotates clockwise!
if index == feedbackMsg.selected_trajectory_idx: # highlight currently selected teb
line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \
stroke_linejoin='round', opacity=1.0 ) )
else:
line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='butt', \
stroke_linejoin='round', stroke_dasharray='10,3', opacity=1.0 ) )
#marker_points = points[::7]
#markerline = svg.add( svg.polyline(points=marker_points, fill='none', stroke=traj_color, stroke_width=10, opacity=0.0 ) )
#arrow = arrowMarker(traj_color)
#markerline.set_markers( (arrow, arrow, arrow) )
#line.set_markers( (arrow, arrow, arrow) )
#line['marker-start'] = arrow.get_funciri()
# Add Start and Goal Point
start_pose = feedbackMsg.trajectories[0].trajectory[0].pose
goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose
start_position = start_pose.position
goal_position = goal_pose.position
svg.add(svg.circle(center=(start_position.x*SCALE,-start_position.y*SCALE), r=10, stroke_width=1, stroke='blue', fill ='blue'))
svg.add(svg.text("Start", (start_position.x*SCALE-70, -start_position.y*SCALE+45), font_size=35)) # Add label
svg.add(svg.circle(center=(goal_position.x*SCALE,-goal_position.y*SCALE), r=10, stroke_width=1, stroke='red', fill ='red'))
svg.add(svg.text("Goal", (goal_position.x*SCALE-40, -goal_position.y*SCALE+45), font_size=35)) # Add label
# draw start arrow
start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0)
start_arrow.translate(start_position.x*SCALE,-start_position.y*SCALE)
start_arrow.rotate( quaternion2YawDegree(start_pose.orientation) )
start_arrow.scale(3)
svg.add(start_arrow)
# draw goal arrow
goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0)
goal_arrow.translate(goal_position.x*SCALE,-goal_position.y*SCALE)
goal_arrow.rotate( quaternion2YawDegree(goal_pose.orientation) )
goal_arrow.scale(3)
svg.add(goal_arrow)
# Draw obstacles
for obstacle in feedbackMsg.obstacles:
if len(obstacle.polygon.points) == 1: # point obstacle
point = obstacle.polygon.points[0]
svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=OBSTACLE_DIST, stroke_width=1, stroke='grey', fill ='grey', opacity=0.3))
svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=15, stroke_width=1, stroke='black', fill ='black'))
svg.add(svg.text("Obstacle", (point.x*SCALE-70, -point.y*SCALE+45), font_size=35)) # Add label
if len(obstacle.polygon.points) == 2: # line obstacle
line_start = obstacle.polygon.points[0]
line_end = obstacle.polygon.points[1]
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))
svg.add(svg.text("Obstacle", (line_start.x*SCALE-70, -line_start.y*SCALE+45), font_size=35)) # Add label
if len(obstacle.polygon.points) > 2: # polygon obstacle
vertices = []
for point in obstacle.polygon.points:
vertices.append((point.x*SCALE, -point.y*SCALE))
svg.add(svg.polygon(points=vertices, stroke='black', fill='gray', stroke_width=1, opacity=1.0))
svg.add(svg.text("Obstacle", (obstacle.polygon.points[0].x*SCALE-70, -obstacle.polygon.points.y*SCALE+45), font_size=35)) # Add label
# Save svg to file (svg_output.svg) and exit node
svg.save()
rospy.loginfo("Drawing completed.")

67
navigations/teb_local_planner/scripts/publish_dynamic_obstacle.py Executable file
View File

@@ -0,0 +1,67 @@
#!/usr/bin/env python
# Author: franz.albers@tu-dortmund.de
import rospy, math, tf
from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
from geometry_msgs.msg import PolygonStamped, Point32, QuaternionStamped, Quaternion, TwistWithCovariance
from tf.transformations import quaternion_from_euler
def publish_obstacle_msg():
pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
#pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
rospy.init_node("test_obstacle_msg")
y_0 = -3.0
vel_x = 0.0
vel_y = 0.3
range_y = 6.0
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 99
obstacle_msg.obstacles[0].polygon.points = [Point32()]
obstacle_msg.obstacles[0].polygon.points[0].x = -1.5
obstacle_msg.obstacles[0].polygon.points[0].y = 0
obstacle_msg.obstacles[0].polygon.points[0].z = 0
yaw = math.atan2(vel_y, vel_x)
q = tf.transformations.quaternion_from_euler(0,0,yaw)
obstacle_msg.obstacles[0].orientation = Quaternion(*q)
obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
obstacle_msg.obstacles[0].velocities.twist.angular.z = 0
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
# Vary y component of the point obstacle
if (vel_y >= 0):
obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y
else:
obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y
t = t + 0.1
pub.publish(obstacle_msg)
r.sleep()
if __name__ == '__main__':
try:
publish_obstacle_msg()
except rospy.ROSInterruptException:
pass

76
navigations/teb_local_planner/scripts/publish_test_obstacles.py Executable file
View File

@@ -0,0 +1,76 @@
#!/usr/bin/env python
# Author: christoph.roesmann@tu-dortmund.de
import rospy, math
from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
from geometry_msgs.msg import PolygonStamped, Point32
def publish_obstacle_msg():
pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
#pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
rospy.init_node("test_obstacle_msg")
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 0
obstacle_msg.obstacles[0].polygon.points = [Point32()]
obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
obstacle_msg.obstacles[0].polygon.points[0].y = 0
obstacle_msg.obstacles[0].polygon.points[0].z = 0
# Add line obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[1].id = 1
line_start = Point32()
line_start.x = -2.5
line_start.y = 0.5
#line_start.y = -3
line_end = Point32()
line_end.x = -2.5
line_end.y = 2
#line_end.y = -4
obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
# Add polygon obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[1].id = 2
v1 = Point32()
v1.x = -1
v1.y = -1
v2 = Point32()
v2.x = -0.5
v2.y = -1.5
v3 = Point32()
v3.x = 0
v3.y = -1
obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
# Vary y component of the point obstacle
obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
t = t + 0.1
pub.publish(obstacle_msg)
r.sleep()
if __name__ == '__main__':
try:
publish_obstacle_msg()
except rospy.ROSInterruptException:
pass

46
navigations/teb_local_planner/scripts/publish_viapoints.py Executable file
View File

@@ -0,0 +1,46 @@
#!/usr/bin/env python
# Author: christoph.roesmann@tu-dortmund.de
import rospy, math
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
def publish_via_points_msg():
pub = rospy.Publisher('/test_optim_node/via_points', Path, queue_size=1)
rospy.init_node("test_via_points_msg")
via_points_msg = Path()
via_points_msg.header.stamp = rospy.Time.now()
via_points_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
# Add via-points
point1 = PoseStamped()
point1.pose.position.x = 0.0;
point1.pose.position.y = 1.5;
point2 = PoseStamped()
point2.pose.position.x = 2.0;
point2.pose.position.y = -0.5;
via_points_msg.poses = [point1, point2]
r = rospy.Rate(5) # 10hz
t = 0.0
while not rospy.is_shutdown():
pub.publish(via_points_msg)
r.sleep()
if __name__ == '__main__':
try:
publish_via_points_msg()
except rospy.ROSInterruptException:
pass

76
navigations/teb_local_planner/scripts/visualize_velocity_profile.py Executable file
View File

@@ -0,0 +1,76 @@
#!/usr/bin/env python
# This small script subscribes to the FeedbackMsg message of teb_local_planner
# and plots the current velocity.
# publish_feedback must be turned on such that the planner publishes this information.
# Author: christoph.roesmann@tu-dortmund.de
import rospy, math
from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
from geometry_msgs.msg import PolygonStamped, Point32
import numpy as np
import matplotlib.pyplot as plotter
def feedback_callback(data):
global trajectory
if not data.trajectories: # empty
trajectory = []
return
trajectory = data.trajectories[data.selected_trajectory_idx].trajectory
def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega):
ax_v.cla()
ax_v.grid()
ax_v.set_ylabel('Trans. velocity [m/s]')
ax_v.plot(t, v, '-bx')
ax_omega.cla()
ax_omega.grid()
ax_omega.set_ylabel('Rot. velocity [rad/s]')
ax_omega.set_xlabel('Time [s]')
ax_omega.plot(t, omega, '-bx')
fig.canvas.draw()
def velocity_plotter():
global trajectory
rospy.init_node("visualize_velocity_profile", anonymous=True)
topic_name = "/test_optim_node/teb_feedback"
topic_name = rospy.get_param('~feedback_topic', topic_name)
rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here!
rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name)
rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.")
# two subplots sharing the same t axis
fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)
plotter.ion()
plotter.show()
r = rospy.Rate(2) # define rate here
while not rospy.is_shutdown():
t = []
v = []
omega = []
for point in trajectory:
t.append(point.time_from_start.to_sec())
v.append(point.velocity.linear.x)
omega.append(point.velocity.angular.z)
plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))
r.sleep()
if __name__ == '__main__':
try:
trajectory = []
velocity_plotter()
except rospy.ROSInterruptException:
pass

342
navigations/teb_local_planner/src/graph_search.cpp Executable file
View File

@@ -0,0 +1,342 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Authors: Christoph Rösmann, Franz Albers
*********************************************************************/
#include <teb_local_planner/graph_search.h>
#include <teb_local_planner/homotopy_class_planner.h>
namespace teb_local_planner
{
void GraphSearchInterface::DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation,
double goal_orientation, const geometry_msgs::Twist* start_velocity, bool free_goal_vel)
{
// see http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/ for details on finding all simple paths
if ((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
return; // We do not need to search for further possible alternative homotopy classes.
HcGraphVertexType back = visited.back();
/// Examine adjacent nodes
HcGraphAdjecencyIterator it, end;
for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it)
{
if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() )
continue; // already visited
if ( *it == goal ) // goal reached
{
visited.push_back(*it);
// Add new TEB, if this path belongs to a new homotopy class
hcp_->addAndInitNewTeb(visited.begin(), visited.end(), boost::bind(getVector2dFromHcGraph, _1, boost::cref(graph_)),
start_orientation, goal_orientation, start_velocity, free_goal_vel);
visited.pop_back();
break;
}
}
/// Recursion for all adjacent vertices
for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it)
{
if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() || *it == goal)
continue; // already visited || goal reached
visited.push_back(*it);
// recursion step
DepthFirst(g, visited, goal, start_orientation, goal_orientation, start_velocity, free_goal_vel);
visited.pop_back();
}
}
void lrKeyPointGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity, bool free_goal_vel)
{
// Clear existing graph and paths
clearGraph();
if((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
return;
// Direction-vector between start and goal and normal-vector:
Eigen::Vector2d diff = goal.position()-start.position();
if (diff.norm()<cfg_->goal_tolerance.xy_goal_tolerance)
{
ROS_DEBUG("HomotopyClassPlanner::createProbRoadmapGraph(): xy-goal-tolerance already reached.");
if (hcp_->getTrajectoryContainer().empty())
{
ROS_INFO("HomotopyClassPlanner::createProbRoadmapGraph(): Initializing a small straight line to just correct orientation errors.");
hcp_->addAndInitNewTeb(start, goal, start_velocity, free_goal_vel);
}
return;
}
Eigen::Vector2d normal(-diff[1],diff[0]); // normal-vector
normal.normalize();
normal = normal*dist_to_obst; // scale with obstacle_distance;
// Insert Vertices
HcGraphVertexType start_vtx = boost::add_vertex(graph_); // start vertex
graph_[start_vtx].pos = start.position();
diff.normalize();
// store nearest obstacle keypoints -> only used if limit_obstacle_heading is enabled
std::pair<HcGraphVertexType,HcGraphVertexType> nearest_obstacle; // both vertices are stored
double min_dist = DBL_MAX;
if (hcp_->obstacles()!=NULL)
{
for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
{
// check if obstacle is placed in front of start point
Eigen::Vector2d start2obst = (*it_obst)->getCentroid() - start.position();
double dist = start2obst.norm();
if (start2obst.dot(diff)/dist<0.1)
continue;
// Add Keypoints
HcGraphVertexType u = boost::add_vertex(graph_);
graph_[u].pos = (*it_obst)->getCentroid() + normal;
HcGraphVertexType v = boost::add_vertex(graph_);
graph_[v].pos = (*it_obst)->getCentroid() - normal;
// store nearest obstacle
if (obstacle_heading_threshold && dist<min_dist)
{
min_dist = dist;
nearest_obstacle.first = u;
nearest_obstacle.second = v;
}
}
}
HcGraphVertexType goal_vtx = boost::add_vertex(graph_); // goal vertex
graph_[goal_vtx].pos = goal.position();
// Insert Edges
HcGraphVertexIterator it_i, end_i, it_j, end_j;
for (boost::tie(it_i,end_i) = boost::vertices(graph_); it_i!=end_i-1; ++it_i) // ignore goal in this loop
{
for (boost::tie(it_j,end_j) = boost::vertices(graph_); it_j!=end_j; ++it_j) // check all forward connections
{
if (it_i==it_j)
continue;
// TODO: make use of knowing in which order obstacles are inserted and that for each obstacle 2 vertices are added,
// therefore we must only check one of them.
Eigen::Vector2d distij = graph_[*it_j].pos-graph_[*it_i].pos;
distij.normalize();
// Check if the direction is backwards:
if (distij.dot(diff)<=obstacle_heading_threshold)
continue;
// Check start angle to nearest obstacle
if (obstacle_heading_threshold && *it_i==start_vtx && min_dist!=DBL_MAX)
{
if (*it_j == nearest_obstacle.first || *it_j == nearest_obstacle.second)
{
Eigen::Vector2d keypoint_dist = graph_[*it_j].pos-start.position();
keypoint_dist.normalize();
Eigen::Vector2d start_orient_vec( cos(start.theta()), sin(start.theta()) ); // already normalized
// check angle
if (start_orient_vec.dot(keypoint_dist) <= obstacle_heading_threshold)
{
ROS_DEBUG("createGraph() - deleted edge: limit_obstacle_heading");
continue;
}
}
}
// Collision Check
if (hcp_->obstacles()!=NULL)
{
bool collision = false;
for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
{
if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, 0.5*dist_to_obst) )
{
collision = true;
break;
}
}
if (collision)
continue;
}
// Create Edge
boost::add_edge(*it_i,*it_j,graph_);
}
}
// Find all paths between start and goal!
std::vector<HcGraphVertexType> visited;
visited.push_back(start_vtx);
DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel);
}
void ProbRoadmapGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity, bool free_goal_vel)
{
// Clear existing graph and paths
clearGraph();
if((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes)
return;
// Direction-vector between start and goal and normal-vector:
Eigen::Vector2d diff = goal.position()-start.position();
double start_goal_dist = diff.norm();
if (start_goal_dist<cfg_->goal_tolerance.xy_goal_tolerance)
{
ROS_DEBUG("HomotopyClassPlanner::createProbRoadmapGraph(): xy-goal-tolerance already reached.");
if (hcp_->getTrajectoryContainer().empty())
{
ROS_INFO("HomotopyClassPlanner::createProbRoadmapGraph(): Initializing a small straight line to just correct orientation errors.");
hcp_->addAndInitNewTeb(start, goal, start_velocity, free_goal_vel);
}
return;
}
Eigen::Vector2d normal(-diff.coeffRef(1),diff.coeffRef(0)); // normal-vector
normal.normalize();
// Now sample vertices between start, goal and a specified width between both sides
// Let's start with a square area between start and goal (maybe change it later to something like a circle or whatever)
double area_width = cfg_->hcp.roadmap_graph_area_width;
boost::random::uniform_real_distribution<double> distribution_x(0, start_goal_dist * cfg_->hcp.roadmap_graph_area_length_scale);
boost::random::uniform_real_distribution<double> distribution_y(0, area_width);
double phi = atan2(diff.coeffRef(1),diff.coeffRef(0)); // rotate area by this angle
Eigen::Rotation2D<double> rot_phi(phi);
Eigen::Vector2d area_origin;
if (cfg_->hcp.roadmap_graph_area_length_scale != 1.0)
area_origin = start.position() + 0.5*(1.0-cfg_->hcp.roadmap_graph_area_length_scale)*start_goal_dist*diff.normalized() - 0.5*area_width*normal; // bottom left corner of the origin
else
area_origin = start.position() - 0.5*area_width*normal; // bottom left corner of the origin
// Insert Vertices
HcGraphVertexType start_vtx = boost::add_vertex(graph_); // start vertex
graph_[start_vtx].pos = start.position();
diff.normalize(); // normalize in place
// Start sampling
for (int i=0; i < cfg_->hcp.roadmap_graph_no_samples; ++i)
{
Eigen::Vector2d sample;
// bool coll_free;
// do // sample as long as a collision free sample is found
// {
// Sample coordinates
sample = area_origin + rot_phi*Eigen::Vector2d(distribution_x(rnd_generator_), distribution_y(rnd_generator_));
// Test for collision
// we do not care for collision checking here to improve efficiency, since we perform resampling repeatedly.
// occupied vertices are ignored in the edge insertion state since they always violate the edge-obstacle collision check.
// coll_free = true;
// for (ObstContainer::const_iterator it_obst = obstacles_->begin(); it_obst != obstacles_->end(); ++it_obst)
// {
// if ( (*it_obst)->checkCollision(sample, dist_to_obst)) // TODO really keep dist_to_obst here?
// {
// coll_free = false;
// break;
// }
// }
//
// } while (!coll_free && ros::ok());
// Add new vertex
HcGraphVertexType v = boost::add_vertex(graph_);
graph_[v].pos = sample;
}
// Now add goal vertex
HcGraphVertexType goal_vtx = boost::add_vertex(graph_); // goal vertex
graph_[goal_vtx].pos = goal.position();
// Insert Edges
HcGraphVertexIterator it_i, end_i, it_j, end_j;
for (boost::tie(it_i,end_i) = boost::vertices(graph_); it_i!=boost::prior(end_i); ++it_i) // ignore goal in this loop
{
for (boost::tie(it_j,end_j) = boost::vertices(graph_); it_j!=end_j; ++it_j) // check all forward connections
{
if (it_i==it_j) // same vertex found
continue;
Eigen::Vector2d distij = graph_[*it_j].pos-graph_[*it_i].pos;
distij.normalize(); // normalize in place
// Check if the direction is backwards:
if (distij.dot(diff)<=obstacle_heading_threshold)
continue; // diff is already normalized
// Collision Check
bool collision = false;
for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst)
{
if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, dist_to_obst) )
{
collision = true;
break;
}
}
if (collision)
continue;
// Create Edge
boost::add_edge(*it_i,*it_j,graph_);
}
}
/// Find all paths between start and goal!
std::vector<HcGraphVertexType> visited;
visited.push_back(start_vtx);
DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel);
}
} // end namespace

849
navigations/teb_local_planner/src/homotopy_class_planner.cpp Executable file
View File

@@ -0,0 +1,849 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/homotopy_class_planner.h>
#include <limits>
namespace teb_local_planner
{
HomotopyClassPlanner::HomotopyClassPlanner() : cfg_(NULL), obstacles_(NULL), via_points_(NULL), robot_model_(new PointRobotFootprint()), initial_plan_(NULL), initialized_(false)
{
}
HomotopyClassPlanner::HomotopyClassPlanner(const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model,
TebVisualizationPtr visual, const ViaPointContainer* via_points) : initial_plan_(NULL)
{
initialize(cfg, obstacles, robot_model, visual, via_points);
}
HomotopyClassPlanner::~HomotopyClassPlanner()
{
}
void HomotopyClassPlanner::initialize(const TebConfig& cfg, ObstContainer* obstacles, RobotFootprintModelPtr robot_model,
TebVisualizationPtr visual, const ViaPointContainer* via_points)
{
cfg_ = &cfg;
obstacles_ = obstacles;
via_points_ = via_points;
robot_model_ = robot_model;
if (cfg_->hcp.simple_exploration)
graph_search_ = boost::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));
else
graph_search_ = boost::shared_ptr<GraphSearchInterface>(new ProbRoadmapGraph(*cfg_, this));
std::random_device rd;
random_.seed(rd());
initialized_ = true;
setVisualization(visual);
}
void HomotopyClassPlanner::updateRobotModel(RobotFootprintModelPtr robot_model )
{
robot_model_ = robot_model;
}
void HomotopyClassPlanner::setVisualization(TebVisualizationPtr visualization)
{
visualization_ = visualization;
}
bool HomotopyClassPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
// store initial plan for further initializations (must be valid for the lifetime of this object or clearPlanner() is called!)
initial_plan_ = &initial_plan;
PoseSE2 start(initial_plan.front().pose);
PoseSE2 goal(initial_plan.back().pose);
return plan(start, goal, start_vel, free_goal_vel);
}
bool HomotopyClassPlanner::plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
PoseSE2 start_pose(start);
PoseSE2 goal_pose(goal);
return plan(start_pose, goal_pose, start_vel, free_goal_vel);
}
bool HomotopyClassPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
// Update old TEBs with new start, goal and velocity
updateAllTEBs(&start, &goal, start_vel);
// Init new TEBs based on newly explored homotopy classes
exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel, free_goal_vel);
// update via-points if activated
updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates);
// Optimize all trajectories in alternative homotopy classes
optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
// Select which candidate (based on alternative homotopy classes) should be used
selectBestTeb();
initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature);
return true;
}
bool HomotopyClassPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const
{
TebOptimalPlannerConstPtr best_teb = bestTeb();
if (!best_teb)
{
vx = 0;
vy = 0;
omega = 0;
return false;
}
return best_teb->getVelocityCommand(vx, vy, omega, look_ahead_poses);
}
void HomotopyClassPlanner::visualize()
{
if (visualization_)
{
// Visualize graph
if (cfg_->hcp.visualize_hc_graph && graph_search_)
visualization_->publishGraph(graph_search_->graph_);
// Visualize active tebs as marker
visualization_->publishTebContainer(tebs_);
// Visualize best teb and feedback message if desired
TebOptimalPlannerConstPtr best_teb = bestTeb();
if (best_teb)
{
visualization_->publishLocalPlanAndPoses(best_teb->teb());
if (best_teb->teb().sizePoses() > 0) //TODO maybe store current pose (start) within plan method as class field.
visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *robot_model_);
// feedback message
if (cfg_->trajectory.publish_feedback)
{
int best_idx = bestTebIdx();
if (best_idx>=0)
visualization_->publishFeedbackMessage(tebs_, (unsigned int) best_idx, *obstacles_);
}
}
}
else ROS_DEBUG("Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before.");
}
bool HomotopyClassPlanner::hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const
{
// iterate existing h-signatures and check if there is an existing H-Signature similar the candidate
for (const std::pair<EquivalenceClassPtr, bool>& eqrel : equivalence_classes_)
{
if (eq_class->isEqual(*eqrel.first))
return true; // Found! Homotopy class already exists, therefore nothing added
}
return false;
}
bool HomotopyClassPlanner::addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock)
{
if (!eq_class)
return false;
if (!eq_class->isValid())
{
ROS_WARN("HomotopyClassPlanner: Ignoring invalid H-signature");
return false;
}
if (hasEquivalenceClass(eq_class))
{
// Allow up to configured number of Tebs that are in the same homotopy
// class as the current (best) Teb to avoid being stuck in a local minimum
if (!isInBestTebClass(eq_class) || numTebsInBestTebClass() >= cfg_->hcp.max_number_plans_in_current_class)
return false;
}
// Homotopy class not found -> Add to class-list, return that the h-signature is new
equivalence_classes_.push_back(std::make_pair(eq_class,lock));
return true;
}
void HomotopyClassPlanner::renewAndAnalyzeOldTebs(bool delete_detours)
{
// clear old h-signatures (since they could be changed due to new obstacle positions.
equivalence_classes_.clear();
// Adding the equivalence class of the latest best_teb_ first
TebOptPlannerContainer::iterator it_best_teb = best_teb_ ? std::find(tebs_.begin(), tebs_.end(), best_teb_) : tebs_.end();
bool has_best_teb = it_best_teb != tebs_.end();
if (has_best_teb)
{
std::iter_swap(tebs_.begin(), it_best_teb); // Putting the last best teb at the beginning of the container
best_teb_eq_class_ = calculateEquivalenceClass(best_teb_->teb().poses().begin(),
best_teb_->teb().poses().end(), getCplxFromVertexPosePtr , obstacles_,
best_teb_->teb().timediffs().begin(), best_teb_->teb().timediffs().end());
addEquivalenceClassIfNew(best_teb_eq_class_);
}
// Collect h-signatures for all existing TEBs and store them together with the corresponding iterator / pointer:
// typedef std::list< std::pair<TebOptPlannerContainer::iterator, std::complex<long double> > > TebCandidateType;
// TebCandidateType teb_candidates;
// get new homotopy classes and delete multiple TEBs per homotopy class. Skips the best teb if available (added before).
TebOptPlannerContainer::iterator it_teb = has_best_teb ? std::next(tebs_.begin(), 1) : tebs_.begin();
while(it_teb != tebs_.end())
{
// calculate equivalence class for the current candidate
EquivalenceClassPtr equivalence_class = calculateEquivalenceClass(it_teb->get()->teb().poses().begin(), it_teb->get()->teb().poses().end(), getCplxFromVertexPosePtr , obstacles_,
it_teb->get()->teb().timediffs().begin(), it_teb->get()->teb().timediffs().end());
// teb_candidates.push_back(std::make_pair(it_teb,H));
// WORKAROUND until the commented code below works
// Here we do not compare cost values. Just first come first serve...
bool new_flag = addEquivalenceClassIfNew(equivalence_class);
if (!new_flag)
{
it_teb = tebs_.erase(it_teb);
continue;
}
++it_teb;
}
if(delete_detours)
deletePlansDetouringBackwards(cfg_->hcp.detours_orientation_tolerance, cfg_->hcp.length_start_orientation_vector);
// Find multiple candidates and delete the one with higher cost
// TODO: this code needs to be adpated. Erasing tebs from the teb container_ could make iteratores stored in the candidate list invalid!
// TebCandidateType::reverse_iterator cand_i = teb_candidates.rbegin();
// int test_idx = 0;
// while (cand_i != teb_candidates.rend())
// {
//
// TebCandidateType::reverse_iterator cand_j = std::find_if(boost::next(cand_i),teb_candidates.rend(), boost::bind(compareH,_1,cand_i->second));
// if (cand_j != teb_candidates.rend() && cand_j != cand_i)
// {
// TebOptimalPlannerPtr pt1 = *(cand_j->first);
// TebOptimalPlannerPtr pt2 = *(cand_i->first);
// assert(pt1);
// assert(pt2);
// if ( cand_j->first->get()->getCurrentCost().sum() > cand_i->first->get()->getCurrentCost().sum() )
// {
// // found one that has higher cost, therefore erase cand_j
// tebs_.erase(cand_j->first);
// teb_candidates.erase(cand_j);
// }
// else // otherwise erase cand_i
// {
// tebs_.erase(cand_i->first);
// cand_i = teb_candidates.erase(cand_i);
// }
// }
// else
// {
// ROS_WARN_STREAM("increase cand_i");
// ++cand_i;
// }
// }
// now add the h-signatures to the internal lookup-table (but only if there is no existing duplicate)
// for (TebCandidateType::iterator cand=teb_candidates.begin(); cand!=teb_candidates.end(); ++cand)
// {
// bool new_flag = addNewHSignatureIfNew(cand->second, cfg_->hcp.h_signature_threshold);
// if (!new_flag)
// {
// // ROS_ERROR_STREAM("getAndFilterHomotopyClassesTEB() - This schould not be happen.");
// tebs_.erase(cand->first);
// }
// }
}
void HomotopyClassPlanner::updateReferenceTrajectoryViaPoints(bool all_trajectories)
{
if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || cfg_->optim.weight_viapoint <= 0)
return;
if(equivalence_classes_.size() < tebs_.size())
{
ROS_ERROR("HomotopyClassPlanner::updateReferenceTrajectoryWithViaPoints(): Number of h-signatures does not match number of trajectories.");
return;
}
if (all_trajectories)
{
// enable via-points for all tebs
for (std::size_t i=0; i < equivalence_classes_.size(); ++i)
{
tebs_[i]->setViaPoints(via_points_);
}
}
else
{
// enable via-points for teb in the same hommotopy class as the initial_plan and deactivate it for all other ones
for (std::size_t i=0; i < equivalence_classes_.size(); ++i)
{
if(initial_plan_eq_class_->isEqual(*equivalence_classes_[i].first))
tebs_[i]->setViaPoints(via_points_);
else
tebs_[i]->setViaPoints(NULL);
}
}
}
void HomotopyClassPlanner::exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
// first process old trajectories
renewAndAnalyzeOldTebs(cfg_->hcp.delete_detours_backwards);
randomlyDropTebs();
// inject initial plan if available and not yet captured
if (initial_plan_)
{
initial_plan_teb_ = addAndInitNewTeb(*initial_plan_, start_vel, free_goal_vel);
}
else
{
initial_plan_teb_.reset();
initial_plan_teb_ = getInitialPlanTEB(); // this method searches for initial_plan_eq_class_ in the teb container (-> if !initial_plan_teb_)
}
// now explore new homotopy classes and initialize tebs if new ones are found. The appropriate createGraph method is chosen via polymorphism.
graph_search_->createGraph(start,goal,dist_to_obst,cfg_->hcp.obstacle_heading_threshold, start_vel, free_goal_vel);
}
TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_velocity, bool free_goal_vel)
{
if(tebs_.size() >= cfg_->hcp.max_number_classes)
return TebOptimalPlannerPtr();
TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(*cfg_, obstacles_, robot_model_, visualization_));
candidate->teb().initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
if (start_velocity)
candidate->setVelocityStart(*start_velocity);
EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
if (free_goal_vel)
candidate->setVelocityGoalFree();
if(addEquivalenceClassIfNew(H))
{
tebs_.push_back(candidate);
return tebs_.back();
}
// If the candidate constitutes no new equivalence class, return a null pointer
return TebOptimalPlannerPtr();
}
bool HomotopyClassPlanner::isInBestTebClass(const EquivalenceClassPtr& eq_class) const
{
bool answer = false;
if (best_teb_eq_class_)
answer = best_teb_eq_class_->isEqual(*eq_class);
return answer;
}
int HomotopyClassPlanner::numTebsInClass(const EquivalenceClassPtr& eq_class) const
{
int count = 0;
for (const std::pair<EquivalenceClassPtr, bool>& eqrel : equivalence_classes_)
{
if (eq_class->isEqual(*eqrel.first))
++count;
}
return count;
}
int HomotopyClassPlanner::numTebsInBestTebClass() const
{
int count = 0;
if (best_teb_eq_class_)
count = numTebsInClass(best_teb_eq_class_);
return count;
}
TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_velocity, bool free_goal_vel)
{
if(tebs_.size() >= cfg_->hcp.max_number_classes)
return TebOptimalPlannerPtr();
TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(*cfg_, obstacles_, robot_model_, visualization_));
candidate->teb().initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
if (start_velocity)
candidate->setVelocityStart(*start_velocity);
if (free_goal_vel)
candidate->setVelocityGoalFree();
// store the h signature of the initial plan to enable searching a matching teb later.
initial_plan_eq_class_ = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
if(addEquivalenceClassIfNew(initial_plan_eq_class_, true)) // also prevent candidate from deletion
{
tebs_.push_back(candidate);
return tebs_.back();
}
// If the candidate constitutes no new equivalence class, return a null pointer
return TebOptimalPlannerPtr();
}
void HomotopyClassPlanner::updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::Twist* start_velocity)
{
// If new goal is too far away, clear all existing trajectories to let them reinitialize later.
// Since all Tebs are sharing the same fixed goal pose, just take the first candidate:
if (!tebs_.empty()
&& ((goal->position() - tebs_.front()->teb().BackPose().position()).norm() >= cfg_->trajectory.force_reinit_new_goal_dist
|| fabs(g2o::normalize_theta(goal->theta() - tebs_.front()->teb().BackPose().theta())) >= cfg_->trajectory.force_reinit_new_goal_angular))
{
ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
tebs_.clear();
equivalence_classes_.clear();
}
// hot-start from previous solutions
for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
{
it_teb->get()->teb().updateAndPruneTEB(*start, *goal);
if (start_velocity)
it_teb->get()->setVelocityStart(*start_velocity);
}
}
void HomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop)
{
// optimize TEBs in parallel since they are independend of each other
if (cfg_->hcp.enable_multithreading)
{
// Must prevent .join_all() from throwing exception if interruption was
// requested, as this can lead to multiple threads operating on the same
// TEB, which leads to SIGSEGV
boost::this_thread::disable_interruption di;
boost::thread_group teb_threads;
for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
{
teb_threads.create_thread( boost::bind(&TebOptimalPlanner::optimizeTEB, it_teb->get(), iter_innerloop, iter_outerloop,
true, cfg_->hcp.selection_obst_cost_scale, cfg_->hcp.selection_viapoint_cost_scale,
cfg_->hcp.selection_alternative_time_cost) );
}
teb_threads.join_all();
}
else
{
for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
{
it_teb->get()->optimizeTEB(iter_innerloop,iter_outerloop, true, cfg_->hcp.selection_obst_cost_scale,
cfg_->hcp.selection_viapoint_cost_scale, cfg_->hcp.selection_alternative_time_cost); // compute cost as well inside optimizeTEB (last argument = true)
}
}
}
TebOptimalPlannerPtr HomotopyClassPlanner::getInitialPlanTEB()
{
// first check stored teb object
if (initial_plan_teb_)
{
// check if the teb is still part of the teb container
if ( std::find(tebs_.begin(), tebs_.end(), initial_plan_teb_ ) != tebs_.end() )
return initial_plan_teb_;
else
{
initial_plan_teb_.reset(); // reset pointer for next call
ROS_DEBUG("initial teb not found, trying to find a match according to the cached equivalence class");
}
}
// reset the locked state for equivalence classes // TODO: this might be adapted if not only the plan containing the initial plan is locked!
for (int i=0; i<equivalence_classes_.size(); ++i)
{
equivalence_classes_[i].second = false;
}
// otherwise check if the stored reference equivalence class exist in the list of known classes
if (initial_plan_eq_class_ && initial_plan_eq_class_->isValid())
{
if (equivalence_classes_.size() == tebs_.size())
{
for (int i=0; i<equivalence_classes_.size(); ++i)
{
if (equivalence_classes_[i].first->isEqual(*initial_plan_eq_class_))
{
equivalence_classes_[i].second = true;
return tebs_[i];
}
}
}
else
ROS_ERROR("HomotopyClassPlanner::getInitialPlanTEB(): number of equivalence classes (%lu) and number of trajectories (%lu) does not match.", equivalence_classes_.size(), tebs_.size());
}
else
ROS_DEBUG("HomotopyClassPlanner::getInitialPlanTEB(): initial TEB not found in the set of available trajectories.");
return TebOptimalPlannerPtr();
}
void HomotopyClassPlanner::randomlyDropTebs()
{
if (cfg_->hcp.selection_dropping_probability == 0.0)
{
return;
}
// interate both vectors in parallel
auto it_eqrel = equivalence_classes_.begin();
auto it_teb = tebs_.begin();
while (it_teb != tebs_.end() && it_eqrel != equivalence_classes_.end())
{
if (it_teb->get() != best_teb_.get() // Always preserve the "best" teb
&& (random_() <= cfg_->hcp.selection_dropping_probability * random_.max()))
{
it_teb = tebs_.erase(it_teb);
it_eqrel = equivalence_classes_.erase(it_eqrel);
}
else
{
++it_teb;
++it_eqrel;
}
}
}
TebOptimalPlannerPtr HomotopyClassPlanner::selectBestTeb()
{
double min_cost = std::numeric_limits<double>::max(); // maximum cost
double min_cost_last_best = std::numeric_limits<double>::max();
double min_cost_initial_plan_teb = std::numeric_limits<double>::max();
TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB();
// check if last best_teb is still a valid candidate
if (best_teb_ && std::find(tebs_.begin(), tebs_.end(), best_teb_) != tebs_.end())
{
// get cost of this candidate
min_cost_last_best = best_teb_->getCurrentCost() * cfg_->hcp.selection_cost_hysteresis; // small hysteresis
last_best_teb_ = best_teb_;
}
else
{
last_best_teb_.reset();
}
if (initial_plan_teb) // the validity was already checked in getInitialPlanTEB()
{
// get cost of this candidate
min_cost_initial_plan_teb = initial_plan_teb->getCurrentCost() * cfg_->hcp.selection_prefer_initial_plan; // small hysteresis
}
best_teb_.reset(); // reset pointer
for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
{
// check if the related TEB leaves the local costmap region
// if (tebs_.size()>1 && !(*it_teb)->teb().isTrajectoryInsideRegion(20, -1, 1))
// {
// ROS_INFO("HomotopyClassPlanner::selectBestTeb(): skipping trajectories that are not inside the local costmap");
// continue;
// }
double teb_cost;
if (*it_teb == last_best_teb_)
teb_cost = min_cost_last_best; // skip already known cost value of the last best_teb
else if (*it_teb == initial_plan_teb)
teb_cost = min_cost_initial_plan_teb;
else
teb_cost = it_teb->get()->getCurrentCost();
if (teb_cost < min_cost)
{
// check if this candidate is currently not selected
best_teb_ = *it_teb;
min_cost = teb_cost;
}
}
// in case we haven't found any teb due to some previous checks, investigate list again
// if (!best_teb_ && !tebs_.empty())
// {
// ROS_DEBUG("all " << tebs_.size() << " tebs rejected previously");
// if (tebs_.size()==1)
// best_teb_ = tebs_.front();
// else // if multiple TEBs are available:
// {
// // try to use the one that relates to the initial plan
// TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB();
// if (initial_plan_teb)
// best_teb_ = initial_plan_teb;
// else
// {
// // now compute the cost for the rest (we haven't computed it before)
// for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
// {
// double teb_cost = it_teb->get()->getCurrentCost();
// if (teb_cost < min_cost)
// {
// // check if this candidate is currently not selected
// best_teb_ = *it_teb;
// min_cost = teb_cost;
// }
// }
// }
// }
// }
// check if we are allowed to change
if (last_best_teb_ && best_teb_ != last_best_teb_)
{
ros::Time now = ros::Time::now();
if ((now-last_eq_class_switching_time_).toSec() > cfg_->hcp.switching_blocking_period)
{
last_eq_class_switching_time_ = now;
}
else
{
ROS_DEBUG("HomotopyClassPlanner::selectBestTeb(): Switching equivalence classes blocked (check parameter switching_blocking_period.");
// block switching, so revert best_teb_
best_teb_ = last_best_teb_;
}
}
return best_teb_;
}
int HomotopyClassPlanner::bestTebIdx() const
{
if (tebs_.size() == 1)
return 0;
if (!best_teb_)
return -1;
int idx = 0;
for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb, ++idx)
{
if (*it_teb == best_teb_)
return idx;
}
return -1;
}
bool HomotopyClassPlanner::isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector<geometry_msgs::Point>& footprint_spec,
double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance)
{
bool feasible = false;
while(ros::ok() && !feasible && tebs_.size() > 0)
{
TebOptimalPlannerPtr best = findBestTeb();
if (!best)
{
ROS_ERROR("Couldn't retrieve the best plan");
return false;
}
feasible = best->isTrajectoryFeasible(costmap_model, footprint_spec, inscribed_radius, circumscribed_radius, look_ahead_idx, feasibility_check_lookahead_distance);
if(!feasible)
{
removeTeb(best);
if(last_best_teb_ && (last_best_teb_ == best)) // Same plan as before.
return feasible; // Not failing could result in oscillations between trajectories.
}
}
return feasible;
}
TebOptimalPlannerPtr HomotopyClassPlanner::findBestTeb()
{
if(tebs_.empty())
return TebOptimalPlannerPtr();
if (!best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end())
best_teb_ = selectBestTeb();
return best_teb_;
}
TebOptPlannerContainer::iterator HomotopyClassPlanner::removeTeb(TebOptimalPlannerPtr& teb)
{
TebOptPlannerContainer::iterator return_iterator = tebs_.end();
if(equivalence_classes_.size() != tebs_.size())
{
ROS_ERROR("removeTeb: size of eq classes != size of tebs");
return return_iterator;
}
auto it_eq_classes = equivalence_classes_.begin();
for(auto it = tebs_.begin(); it != tebs_.end(); ++it)
{
if(*it == teb)
{
return_iterator = tebs_.erase(it);
equivalence_classes_.erase(it_eq_classes);
break;
}
++it_eq_classes;
}
return return_iterator;
}
void HomotopyClassPlanner::setPreferredTurningDir(RotType dir)
{
// set preferred turning dir for all TEBs
for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
{
(*it_teb)->setPreferredTurningDir(dir);
}
}
bool HomotopyClassPlanner::hasDiverged() const
{
// Early return if there is no best trajectory initialized
if (!best_teb_)
return false;
return best_teb_->hasDiverged();
}
void HomotopyClassPlanner::computeCurrentCost(std::vector<double>& cost, double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{
for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
{
it_teb->get()->computeCurrentCost(cost, obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
}
}
void HomotopyClassPlanner::deletePlansDetouringBackwards(const double orient_threshold,
const double len_orientation_vector)
{
if (tebs_.size() < 2 || !best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end() ||
best_teb_->teb().sizePoses() < 2)
{
return; // A moving direction wasn't chosen yet
}
double current_movement_orientation;
double best_plan_duration = std::max(best_teb_->teb().getSumOfAllTimeDiffs(), 1.0);
if(!computeStartOrientation(best_teb_, len_orientation_vector, current_movement_orientation))
return; // The plan is shorter than len_orientation_vector
for(auto it_teb = tebs_.begin(); it_teb != tebs_.end();)
{
if(*it_teb == best_teb_) // The current plan should not be considered a detour
{
++it_teb;
continue;
}
if((*it_teb)->teb().sizePoses() < 2)
{
ROS_DEBUG("Discarding a plan with less than 2 poses");
it_teb = removeTeb(*it_teb);
continue;
}
double plan_orientation;
if(!computeStartOrientation(*it_teb, len_orientation_vector, plan_orientation))
{
ROS_DEBUG("Failed to compute the start orientation for one of the tebs, likely close to the target");
it_teb = removeTeb(*it_teb);
continue;
}
if(fabs(g2o::normalize_theta(plan_orientation - current_movement_orientation)) > orient_threshold)
{
it_teb = removeTeb(*it_teb); // Plan detouring backwards
continue;
}
if(!it_teb->get()->isOptimized())
{
ROS_DEBUG("Removing a teb because it's not optimized");
it_teb = removeTeb(*it_teb); // Deletes tebs that cannot be optimized (last optim call failed)
continue;
}
if(it_teb->get()->teb().getSumOfAllTimeDiffs() / best_plan_duration > cfg_->hcp.max_ratio_detours_duration_best_duration)
{
ROS_DEBUG("Removing a teb because it's duration is much longer than that of the best teb");
it_teb = removeTeb(*it_teb);
continue;
}
++it_teb;
}
}
bool HomotopyClassPlanner::computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector,
double& orientation)
{
VertexPose start_pose = plan->teb().Pose(0);
bool second_pose_found = false;
Eigen::Vector2d start_vector;
for(auto& pose : plan->teb().poses())
{
start_vector = start_pose.position() - pose->position();
if(start_vector.norm() > len_orientation_vector)
{
second_pose_found = true;
break;
}
}
if(!second_pose_found) // The current plan is too short to make assumptions on the start orientation
return false;
orientation = std::atan2(start_vector[1], start_vector[0]);
return true;
}
} // end namespace

214
navigations/teb_local_planner/src/obstacles.cpp Executable file
View File

@@ -0,0 +1,214 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/obstacles.h>
#include <ros/console.h>
#include <ros/assert.h>
// #include <teb_local_planner/misc.h>
namespace teb_local_planner
{
void PolygonObstacle::fixPolygonClosure()
{
if (vertices_.size()<2)
return;
if (vertices_.front().isApprox(vertices_.back()))
vertices_.pop_back();
}
void PolygonObstacle::calcCentroid()
{
if (vertices_.empty())
{
centroid_.setConstant(NAN);
ROS_WARN("PolygonObstacle::calcCentroid(): number of vertices is empty. the resulting centroid is a vector of NANs.");
return;
}
// if polygon is a point
if (noVertices()==1)
{
centroid_ = vertices_.front();
return;
}
// if polygon is a line:
if (noVertices()==2)
{
centroid_ = 0.5*(vertices_.front() + vertices_.back());
return;
}
// otherwise:
centroid_.setZero();
// calculate centroid (see wikipedia http://de.wikipedia.org/wiki/Geometrischer_Schwerpunkt#Polygon)
double A = 0; // A = 0.5 * sum_0_n-1 (x_i * y_{i+1} - x_{i+1} * y_i)
for (int i=0; i < noVertices()-1; ++i)
{
A += vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1);
}
A += vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1);
A *= 0.5;
if (A!=0)
{
for (int i=0; i < noVertices()-1; ++i)
{
double aux = (vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1));
centroid_ += ( vertices_.at(i) + vertices_.at(i+1) )*aux;
}
double aux = (vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1));
centroid_ += ( vertices_.at(noVertices()-1) + vertices_.at(0) )*aux;
centroid_ /= (6*A);
}
else // A == 0 -> all points are placed on a 'perfect' line
{
// seach for the two outer points of the line with the maximum distance inbetween
int i_cand = 0;
int j_cand = 0;
double max_dist = 0;
for (int i=0; i< noVertices(); ++i)
{
for (int j=i+1; j< noVertices(); ++j) // start with j=i+1
{
double dist = (vertices_[j] - vertices_[i]).norm();
if (dist > max_dist)
{
max_dist = dist;
i_cand = i;
j_cand = j;
}
}
}
// calc centroid of that line
centroid_ = 0.5*(vertices_[i_cand] + vertices_[j_cand]);
}
}
Eigen::Vector2d PolygonObstacle::getClosestPoint(const Eigen::Vector2d& position) const
{
// the polygon is a point
if (noVertices() == 1)
{
return vertices_.front();
}
if (noVertices() > 1)
{
Eigen::Vector2d new_pt = closest_point_on_line_segment_2d(position, vertices_.at(0), vertices_.at(1));
if (noVertices() > 2) // real polygon and not a line
{
double dist = (new_pt-position).norm();
Eigen::Vector2d closest_pt = new_pt;
// check each polygon edge
for (int i=1; i<noVertices()-1; ++i) // skip the first one, since we already checked it (new_pt)
{
new_pt = closest_point_on_line_segment_2d(position, vertices_.at(i), vertices_.at(i+1));
double new_dist = (new_pt-position).norm();
if (new_dist < dist)
{
dist = new_dist;
closest_pt = new_pt;
}
}
// afterwards we check the edge between goal and start (close polygon)
new_pt = closest_point_on_line_segment_2d(position, vertices_.back(), vertices_.front());
double new_dist = (new_pt-position).norm();
if (new_dist < dist)
return new_pt;
else
return closest_pt;
}
else
{
return new_pt; // closest point on line segment
}
}
ROS_ERROR("PolygonObstacle::getClosestPoint() cannot find any closest point. Polygon ill-defined?");
return Eigen::Vector2d::Zero(); // todo: maybe boost::optional?
}
bool PolygonObstacle::checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist) const
{
// Simple strategy, check all edge-line intersections until an intersection is found...
// check each polygon edge
for (int i=0; i<noVertices()-1; ++i)
{
if ( check_line_segments_intersection_2d(line_start, line_end, vertices_.at(i), vertices_.at(i+1)) )
return true;
}
if (noVertices()==2) // if polygon is a line
return false;
return check_line_segments_intersection_2d(line_start, line_end, vertices_.back(), vertices_.front()); //otherwise close polygon
}
// implements toPolygonMsg() of the base class
void PolygonObstacle::toPolygonMsg(geometry_msgs::Polygon& polygon)
{
polygon.points.resize(vertices_.size());
for (std::size_t i=0; i<vertices_.size(); ++i)
{
polygon.points[i].x = vertices_[i].x();
polygon.points[i].y = vertices_[i].y();
polygon.points[i].z = 0;
}
}
} // namespace teb_local_planner

1316
navigations/teb_local_planner/src/optimal_planner.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

118
navigations/teb_local_planner/src/recovery_behaviors.cpp Executable file
View File

@@ -0,0 +1,118 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/recovery_behaviors.h>
#include <ros/ros.h>
#include <limits>
#include <functional>
#include <numeric>
#include <g2o/stuff/misc.h>
namespace teb_local_planner
{
// ============== FailureDetector Implementation ===================
void FailureDetector::update(const geometry_msgs::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps)
{
if (buffer_.capacity() == 0)
return;
VelMeasurement measurement;
measurement.v = twist.linear.x; // just consider linear velocity in x-direction in the robot frame for now
measurement.omega = twist.angular.z;
if (measurement.v > 0 && v_max>0)
measurement.v /= v_max;
else if (measurement.v < 0 && v_backwards_max > 0)
measurement.v /= v_backwards_max;
if (omega_max > 0)
measurement.omega /= omega_max;
buffer_.push_back(measurement);
// immediately compute new state
detect(v_eps, omega_eps);
}
void FailureDetector::clear()
{
buffer_.clear();
oscillating_ = false;
}
bool FailureDetector::isOscillating() const
{
return oscillating_;
}
bool FailureDetector::detect(double v_eps, double omega_eps)
{
oscillating_ = false;
if (buffer_.size() < buffer_.capacity()/2) // we start detecting only as soon as we have the buffer filled at least half
return false;
double n = (double)buffer_.size();
// compute mean for v and omega
double v_mean=0;
double omega_mean=0;
int omega_zero_crossings = 0;
for (int i=0; i < n; ++i)
{
v_mean += buffer_[i].v;
omega_mean += buffer_[i].omega;
if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) )
++omega_zero_crossings;
}
v_mean /= n;
omega_mean /= n;
if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps && omega_zero_crossings>1 )
{
oscillating_ = true;
}
// ROS_INFO_STREAM("v: " << std::abs(v_mean) << ", omega: " << std::abs(omega_mean) << ", zero crossings: " << omega_zero_crossings);
return oscillating_;
}
} // namespace teb_local_planner

397
navigations/teb_local_planner/src/teb_config.cpp Executable file
View File

@@ -0,0 +1,397 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/teb_config.h>
namespace teb_local_planner
{
void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)
{
nh.param("odom_topic", odom_topic, odom_topic);
nh.param("map_frame", map_frame, map_frame);
// Trajectory
nh.param("teb_autosize", trajectory.teb_autosize, trajectory.teb_autosize);
nh.param("dt_ref", trajectory.dt_ref, trajectory.dt_ref);
nh.param("dt_hysteresis", trajectory.dt_hysteresis, trajectory.dt_hysteresis);
nh.param("min_samples", trajectory.min_samples, trajectory.min_samples);
nh.param("max_samples", trajectory.max_samples, trajectory.max_samples);
nh.param("global_plan_overwrite_orientation", trajectory.global_plan_overwrite_orientation, trajectory.global_plan_overwrite_orientation);
nh.param("allow_init_with_backwards_motion", trajectory.allow_init_with_backwards_motion, trajectory.allow_init_with_backwards_motion);
nh.getParam("global_plan_via_point_sep", trajectory.global_plan_viapoint_sep); // deprecated, see checkDeprecated()
if (!nh.param("global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep, trajectory.global_plan_viapoint_sep))
nh.setParam("global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep); // write deprecated value to param server
nh.param("via_points_ordered", trajectory.via_points_ordered, trajectory.via_points_ordered);
nh.param("max_global_plan_lookahead_dist", trajectory.max_global_plan_lookahead_dist, trajectory.max_global_plan_lookahead_dist);
nh.param("global_plan_prune_distance", trajectory.global_plan_prune_distance, trajectory.global_plan_prune_distance);
nh.param("exact_arc_length", trajectory.exact_arc_length, trajectory.exact_arc_length);
nh.param("force_reinit_new_goal_dist", trajectory.force_reinit_new_goal_dist, trajectory.force_reinit_new_goal_dist);
nh.param("force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular);
nh.param("feasibility_check_no_poses", trajectory.feasibility_check_no_poses, trajectory.feasibility_check_no_poses);
nh.param("feasibility_check_lookahead_distance", trajectory.feasibility_check_lookahead_distance, trajectory.feasibility_check_lookahead_distance);
nh.param("publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback);
nh.param("min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular);
nh.param("control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses);
nh.param("prevent_look_ahead_poses_near_goal", trajectory.prevent_look_ahead_poses_near_goal, trajectory.prevent_look_ahead_poses_near_goal);
// Robot
nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x);
nh.param("max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
nh.param("max_vel_y", robot.max_vel_y, robot.max_vel_y);
nh.param("max_vel_trans", robot.max_vel_trans, robot.max_vel_trans);
nh.param("max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
nh.param("acc_lim_x", robot.acc_lim_x, robot.acc_lim_x);
nh.param("acc_lim_y", robot.acc_lim_y, robot.acc_lim_y);
nh.param("acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta);
nh.param("min_turning_radius", robot.min_turning_radius, robot.min_turning_radius);
nh.param("wheelbase", robot.wheelbase, robot.wheelbase);
nh.param("cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel);
nh.param("is_footprint_dynamic", robot.is_footprint_dynamic, robot.is_footprint_dynamic);
nh.param("use_proportional_saturation", robot.use_proportional_saturation, robot.use_proportional_saturation);
nh.param("transform_tolerance", robot.transform_tolerance, robot.transform_tolerance);
// GoalTolerance
nh.param("xy_goal_tolerance", goal_tolerance.xy_goal_tolerance, goal_tolerance.xy_goal_tolerance);
nh.param("yaw_goal_tolerance", goal_tolerance.yaw_goal_tolerance, goal_tolerance.yaw_goal_tolerance);
nh.param("free_goal_vel", goal_tolerance.free_goal_vel, goal_tolerance.free_goal_vel);
nh.param("trans_stopped_vel", goal_tolerance.trans_stopped_vel, goal_tolerance.trans_stopped_vel);
nh.param("theta_stopped_vel", goal_tolerance.theta_stopped_vel, goal_tolerance.theta_stopped_vel);
nh.param("complete_global_plan", goal_tolerance.complete_global_plan, goal_tolerance.complete_global_plan);
// Obstacles
nh.param("min_obstacle_dist", obstacles.min_obstacle_dist, obstacles.min_obstacle_dist);
nh.param("inflation_dist", obstacles.inflation_dist, obstacles.inflation_dist);
nh.param("dynamic_obstacle_inflation_dist", obstacles.dynamic_obstacle_inflation_dist, obstacles.dynamic_obstacle_inflation_dist);
nh.param("include_dynamic_obstacles", obstacles.include_dynamic_obstacles, obstacles.include_dynamic_obstacles);
nh.param("include_costmap_obstacles", obstacles.include_costmap_obstacles, obstacles.include_costmap_obstacles);
nh.param("costmap_obstacles_behind_robot_dist", obstacles.costmap_obstacles_behind_robot_dist, obstacles.costmap_obstacles_behind_robot_dist);
nh.param("obstacle_poses_affected", obstacles.obstacle_poses_affected, obstacles.obstacle_poses_affected);
nh.param("legacy_obstacle_association", obstacles.legacy_obstacle_association, obstacles.legacy_obstacle_association);
nh.param("obstacle_association_force_inclusion_factor", obstacles.obstacle_association_force_inclusion_factor, obstacles.obstacle_association_force_inclusion_factor);
nh.param("obstacle_association_cutoff_factor", obstacles.obstacle_association_cutoff_factor, obstacles.obstacle_association_cutoff_factor);
nh.param("costmap_converter_plugin", obstacles.costmap_converter_plugin, obstacles.costmap_converter_plugin);
nh.param("costmap_converter_spin_thread", obstacles.costmap_converter_spin_thread, obstacles.costmap_converter_spin_thread);
nh.param("obstacle_proximity_ratio_max_vel", obstacles.obstacle_proximity_ratio_max_vel, obstacles.obstacle_proximity_ratio_max_vel);
nh.param("obstacle_proximity_lower_bound", obstacles.obstacle_proximity_lower_bound, obstacles.obstacle_proximity_lower_bound);
nh.param("obstacle_proximity_upper_bound", obstacles.obstacle_proximity_upper_bound, obstacles.obstacle_proximity_upper_bound);
// Optimization
nh.param("no_inner_iterations", optim.no_inner_iterations, optim.no_inner_iterations);
nh.param("no_outer_iterations", optim.no_outer_iterations, optim.no_outer_iterations);
nh.param("optimization_activate", optim.optimization_activate, optim.optimization_activate);
nh.param("optimization_verbose", optim.optimization_verbose, optim.optimization_verbose);
nh.param("penalty_epsilon", optim.penalty_epsilon, optim.penalty_epsilon);
nh.param("weight_max_vel_x", optim.weight_max_vel_x, optim.weight_max_vel_x);
nh.param("weight_max_vel_y", optim.weight_max_vel_y, optim.weight_max_vel_y);
nh.param("weight_max_vel_theta", optim.weight_max_vel_theta, optim.weight_max_vel_theta);
nh.param("weight_acc_lim_x", optim.weight_acc_lim_x, optim.weight_acc_lim_x);
nh.param("weight_acc_lim_y", optim.weight_acc_lim_y, optim.weight_acc_lim_y);
nh.param("weight_acc_lim_theta", optim.weight_acc_lim_theta, optim.weight_acc_lim_theta);
nh.param("weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh);
nh.param("weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive);
nh.param("weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius);
nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime);
nh.param("weight_shortest_path", optim.weight_shortest_path, optim.weight_shortest_path);
nh.param("weight_obstacle", optim.weight_obstacle, optim.weight_obstacle);
nh.param("weight_inflation", optim.weight_inflation, optim.weight_inflation);
nh.param("weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle);
nh.param("weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation);
nh.param("weight_velocity_obstacle_ratio", optim.weight_velocity_obstacle_ratio, optim.weight_velocity_obstacle_ratio);
nh.param("weight_viapoint", optim.weight_viapoint, optim.weight_viapoint);
nh.param("weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir);
nh.param("weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor);
nh.param("obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent);
// Homotopy Class Planner
nh.param("enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning);
nh.param("enable_multithreading", hcp.enable_multithreading, hcp.enable_multithreading);
nh.param("simple_exploration", hcp.simple_exploration, hcp.simple_exploration);
nh.param("max_number_classes", hcp.max_number_classes, hcp.max_number_classes);
nh.param("max_number_plans_in_current_class", hcp.max_number_plans_in_current_class, hcp.max_number_plans_in_current_class);
nh.param("selection_obst_cost_scale", hcp.selection_obst_cost_scale, hcp.selection_obst_cost_scale);
nh.param("selection_prefer_initial_plan", hcp.selection_prefer_initial_plan, hcp.selection_prefer_initial_plan);
nh.param("selection_viapoint_cost_scale", hcp.selection_viapoint_cost_scale, hcp.selection_viapoint_cost_scale);
nh.param("selection_cost_hysteresis", hcp.selection_cost_hysteresis, hcp.selection_cost_hysteresis);
nh.param("selection_alternative_time_cost", hcp.selection_alternative_time_cost, hcp.selection_alternative_time_cost);
nh.param("selection_dropping_probability", hcp.selection_dropping_probability, hcp.selection_dropping_probability);
nh.param("switching_blocking_period", hcp.switching_blocking_period, hcp.switching_blocking_period);
nh.param("roadmap_graph_samples", hcp.roadmap_graph_no_samples, hcp.roadmap_graph_no_samples);
nh.param("roadmap_graph_area_width", hcp.roadmap_graph_area_width, hcp.roadmap_graph_area_width);
nh.param("roadmap_graph_area_length_scale", hcp.roadmap_graph_area_length_scale, hcp.roadmap_graph_area_length_scale);
nh.param("h_signature_prescaler", hcp.h_signature_prescaler, hcp.h_signature_prescaler);
nh.param("h_signature_threshold", hcp.h_signature_threshold, hcp.h_signature_threshold);
nh.param("obstacle_keypoint_offset", hcp.obstacle_keypoint_offset, hcp.obstacle_keypoint_offset);
nh.param("obstacle_heading_threshold", hcp.obstacle_heading_threshold, hcp.obstacle_heading_threshold);
nh.param("viapoints_all_candidates", hcp.viapoints_all_candidates, hcp.viapoints_all_candidates);
nh.param("visualize_hc_graph", hcp.visualize_hc_graph, hcp.visualize_hc_graph);
nh.param("visualize_with_time_as_z_axis_scale", hcp.visualize_with_time_as_z_axis_scale, hcp.visualize_with_time_as_z_axis_scale);
nh.param("delete_detours_backwards", hcp.delete_detours_backwards, hcp.delete_detours_backwards);
nh.param("detours_orientation_tolerance", hcp.detours_orientation_tolerance, hcp.detours_orientation_tolerance);
nh.param("length_start_orientation_vector", hcp.length_start_orientation_vector, hcp.length_start_orientation_vector);
nh.param("max_ratio_detours_duration_best_duration", hcp.max_ratio_detours_duration_best_duration, hcp.max_ratio_detours_duration_best_duration);
// Recovery
nh.param("shrink_horizon_backup", recovery.shrink_horizon_backup, recovery.shrink_horizon_backup);
nh.param("shrink_horizon_min_duration", recovery.shrink_horizon_min_duration, recovery.shrink_horizon_min_duration);
nh.param("oscillation_recovery", recovery.oscillation_recovery, recovery.oscillation_recovery);
nh.param("oscillation_v_eps", recovery.oscillation_v_eps, recovery.oscillation_v_eps);
nh.param("oscillation_omega_eps", recovery.oscillation_omega_eps, recovery.oscillation_omega_eps);
nh.param("oscillation_recovery_min_duration", recovery.oscillation_recovery_min_duration, recovery.oscillation_recovery_min_duration);
nh.param("oscillation_filter_duration", recovery.oscillation_filter_duration, recovery.oscillation_filter_duration);
nh.param("divergence_detection", recovery.divergence_detection_enable, recovery.divergence_detection_enable);
nh.param("divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared);
checkParameters();
checkDeprecated(nh);
}
void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
{
boost::mutex::scoped_lock l(config_mutex_);
// Trajectory
trajectory.teb_autosize = cfg.teb_autosize;
trajectory.dt_ref = cfg.dt_ref;
trajectory.dt_hysteresis = cfg.dt_hysteresis;
trajectory.global_plan_overwrite_orientation = cfg.global_plan_overwrite_orientation;
trajectory.allow_init_with_backwards_motion = cfg.allow_init_with_backwards_motion;
trajectory.global_plan_viapoint_sep = cfg.global_plan_viapoint_sep;
trajectory.via_points_ordered = cfg.via_points_ordered;
trajectory.max_global_plan_lookahead_dist = cfg.max_global_plan_lookahead_dist;
trajectory.exact_arc_length = cfg.exact_arc_length;
trajectory.force_reinit_new_goal_dist = cfg.force_reinit_new_goal_dist;
trajectory.force_reinit_new_goal_angular = cfg.force_reinit_new_goal_angular;
trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses;
trajectory.feasibility_check_lookahead_distance = cfg.feasibility_check_lookahead_distance;
trajectory.publish_feedback = cfg.publish_feedback;
trajectory.control_look_ahead_poses = cfg.control_look_ahead_poses;
trajectory.prevent_look_ahead_poses_near_goal = cfg.prevent_look_ahead_poses_near_goal;
// Robot
robot.max_vel_x = cfg.max_vel_x;
robot.max_vel_x_backwards = cfg.max_vel_x_backwards;
robot.max_vel_y = cfg.max_vel_y;
robot.max_vel_theta = cfg.max_vel_theta;
robot.acc_lim_x = cfg.acc_lim_x;
robot.acc_lim_y = cfg.acc_lim_y;
robot.acc_lim_theta = cfg.acc_lim_theta;
robot.min_turning_radius = cfg.min_turning_radius;
robot.wheelbase = cfg.wheelbase;
robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel;
robot.use_proportional_saturation = cfg.use_proportional_saturation;
if (cfg.max_vel_trans == 0.0)
{
ROS_INFO_STREAM("max_vel_trans is not set, setting it equal to max_vel_x: " << robot.max_vel_x);
cfg.max_vel_trans = robot.max_vel_x;
}
robot.max_vel_trans = cfg.max_vel_trans;
// GoalTolerance
goal_tolerance.xy_goal_tolerance = cfg.xy_goal_tolerance;
goal_tolerance.yaw_goal_tolerance = cfg.yaw_goal_tolerance;
goal_tolerance.free_goal_vel = cfg.free_goal_vel;
goal_tolerance.trans_stopped_vel = cfg.trans_stopped_vel;
goal_tolerance.theta_stopped_vel = cfg.theta_stopped_vel;
// Obstacles
obstacles.min_obstacle_dist = cfg.min_obstacle_dist;
obstacles.inflation_dist = cfg.inflation_dist;
obstacles.dynamic_obstacle_inflation_dist = cfg.dynamic_obstacle_inflation_dist;
obstacles.include_dynamic_obstacles = cfg.include_dynamic_obstacles;
obstacles.include_costmap_obstacles = cfg.include_costmap_obstacles;
obstacles.legacy_obstacle_association = cfg.legacy_obstacle_association;
obstacles.obstacle_association_force_inclusion_factor = cfg.obstacle_association_force_inclusion_factor;
obstacles.obstacle_association_cutoff_factor = cfg.obstacle_association_cutoff_factor;
obstacles.costmap_obstacles_behind_robot_dist = cfg.costmap_obstacles_behind_robot_dist;
obstacles.obstacle_poses_affected = cfg.obstacle_poses_affected;
obstacles.obstacle_proximity_ratio_max_vel = cfg.obstacle_proximity_ratio_max_vel;
obstacles.obstacle_proximity_lower_bound = cfg.obstacle_proximity_lower_bound;
obstacles.obstacle_proximity_upper_bound = cfg.obstacle_proximity_upper_bound;
// Optimization
optim.no_inner_iterations = cfg.no_inner_iterations;
optim.no_outer_iterations = cfg.no_outer_iterations;
optim.optimization_activate = cfg.optimization_activate;
optim.optimization_verbose = cfg.optimization_verbose;
optim.penalty_epsilon = cfg.penalty_epsilon;
optim.weight_max_vel_x = cfg.weight_max_vel_x;
optim.weight_max_vel_y = cfg.weight_max_vel_y;
optim.weight_max_vel_theta = cfg.weight_max_vel_theta;
optim.weight_acc_lim_x = cfg.weight_acc_lim_x;
optim.weight_acc_lim_y = cfg.weight_acc_lim_y;
optim.weight_acc_lim_theta = cfg.weight_acc_lim_theta;
optim.weight_kinematics_nh = cfg.weight_kinematics_nh;
optim.weight_kinematics_forward_drive = cfg.weight_kinematics_forward_drive;
optim.weight_kinematics_turning_radius = cfg.weight_kinematics_turning_radius;
optim.weight_optimaltime = cfg.weight_optimaltime;
optim.weight_shortest_path = cfg.weight_shortest_path;
optim.weight_obstacle = cfg.weight_obstacle;
optim.weight_inflation = cfg.weight_inflation;
optim.weight_dynamic_obstacle = cfg.weight_dynamic_obstacle;
optim.weight_dynamic_obstacle_inflation = cfg.weight_dynamic_obstacle_inflation;
optim.weight_velocity_obstacle_ratio = cfg.weight_velocity_obstacle_ratio;
optim.weight_viapoint = cfg.weight_viapoint;
optim.weight_adapt_factor = cfg.weight_adapt_factor;
optim.obstacle_cost_exponent = cfg.obstacle_cost_exponent;
// Homotopy Class Planner
hcp.enable_multithreading = cfg.enable_multithreading;
hcp.max_number_classes = cfg.max_number_classes;
hcp.max_number_plans_in_current_class = cfg.max_number_plans_in_current_class;
hcp.selection_cost_hysteresis = cfg.selection_cost_hysteresis;
hcp.selection_prefer_initial_plan = cfg.selection_prefer_initial_plan;
hcp.selection_obst_cost_scale = cfg.selection_obst_cost_scale;
hcp.selection_viapoint_cost_scale = cfg.selection_viapoint_cost_scale;
hcp.selection_alternative_time_cost = cfg.selection_alternative_time_cost;
hcp.selection_dropping_probability = cfg.selection_dropping_probability;
hcp.switching_blocking_period = cfg.switching_blocking_period;
hcp.obstacle_heading_threshold = cfg.obstacle_heading_threshold;
hcp.roadmap_graph_no_samples = cfg.roadmap_graph_no_samples;
hcp.roadmap_graph_area_width = cfg.roadmap_graph_area_width;
hcp.roadmap_graph_area_length_scale = cfg.roadmap_graph_area_length_scale;
hcp.h_signature_prescaler = cfg.h_signature_prescaler;
hcp.h_signature_threshold = cfg.h_signature_threshold;
hcp.viapoints_all_candidates = cfg.viapoints_all_candidates;
hcp.visualize_hc_graph = cfg.visualize_hc_graph;
hcp.visualize_with_time_as_z_axis_scale = cfg.visualize_with_time_as_z_axis_scale;
// Recovery
recovery.shrink_horizon_backup = cfg.shrink_horizon_backup;
recovery.oscillation_recovery = cfg.oscillation_recovery;
recovery.divergence_detection_enable = cfg.divergence_detection_enable;
recovery.divergence_detection_max_chi_squared = cfg.divergence_detection_max_chi_squared;
checkParameters();
}
void TebConfig::checkParameters() const
{
// positive backward velocity?
if (robot.max_vel_x_backwards <= 0)
ROS_WARN("TebLocalPlannerROS() Param Warning: Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving.");
// bounds smaller than penalty epsilon
if (robot.max_vel_x <= optim.penalty_epsilon)
ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
if (robot.max_vel_x_backwards <= optim.penalty_epsilon)
ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
if (robot.max_vel_theta <= optim.penalty_epsilon)
ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
if (robot.acc_lim_x <= optim.penalty_epsilon)
ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
if (robot.acc_lim_theta <= optim.penalty_epsilon)
ROS_WARN("TebLocalPlannerROS() Param Warning: acc_lim_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!");
// dt_ref and dt_hyst
if (trajectory.dt_ref <= trajectory.dt_hysteresis)
ROS_WARN("TebLocalPlannerROS() Param Warning: dt_ref <= dt_hysteresis. The hysteresis is not allowed to be greater or equal!. Undefined behavior... Change at least one of them!");
// min number of samples
if (trajectory.min_samples <3)
ROS_WARN("TebLocalPlannerROS() Param Warning: parameter min_samples is smaller than 3! Sorry, I haven't enough degrees of freedom to plan a trajectory for you. Please increase ...");
// costmap obstacle behind robot
if (obstacles.costmap_obstacles_behind_robot_dist < 0)
ROS_WARN("TebLocalPlannerROS() Param Warning: parameter 'costmap_obstacles_behind_robot_dist' should be positive or zero.");
// hcp: obstacle heading threshold
if (hcp.obstacle_keypoint_offset>=1 || hcp.obstacle_keypoint_offset<=0)
ROS_WARN("TebLocalPlannerROS() Param Warning: parameter obstacle_heading_threshold must be in the interval ]0,1[. 0=0deg opening angle, 1=90deg opening angle.");
// carlike
if (robot.cmd_angle_instead_rotvel && robot.wheelbase==0)
ROS_WARN("TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but wheelbase is set to zero: undesired behavior.");
if (robot.cmd_angle_instead_rotvel && robot.min_turning_radius==0)
ROS_WARN("TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but min_turning_radius is set to zero: undesired behavior. You are mixing a carlike and a diffdrive robot");
// positive weight_adapt_factor
if (optim.weight_adapt_factor < 1.0)
ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_adapt_factor shoud be >= 1.0");
if (recovery.oscillation_filter_duration < 0)
ROS_WARN("TebLocalPlannerROS() Param Warning: parameter oscillation_filter_duration must be >= 0");
// weights
if (optim.weight_optimaltime <= 0)
ROS_WARN("TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)");
// holonomic check
if (robot.max_vel_y > 0) {
if (robot.max_vel_trans < std::min(robot.max_vel_x, robot.max_vel_trans)) {
ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_trans < min(max_vel_x, max_vel_y). Note that vel_trans = sqrt(Vx^2 + Vy^2), thus max_vel_trans will limit Vx and Vy in the optimization step.");
}
if (robot.max_vel_trans > std::max(robot.max_vel_x, robot.max_vel_y)) {
ROS_WARN("TebLocalPlannerROS() Param Warning: max_vel_trans > max(max_vel_x, max_vel_y). Robot will rotate and move diagonally to achieve max resultant vel (possibly max vel on both axis), limited by the max_vel_trans.");
}
}
}
void TebConfig::checkDeprecated(const ros::NodeHandle& nh) const
{
if (nh.hasParam("line_obstacle_poses_affected") || nh.hasParam("polygon_obstacle_poses_affected"))
ROS_WARN("TebLocalPlannerROS() Param Warning: 'line_obstacle_poses_affected' and 'polygon_obstacle_poses_affected' are deprecated. They share now the common parameter 'obstacle_poses_affected'.");
if (nh.hasParam("weight_point_obstacle") || nh.hasParam("weight_line_obstacle") || nh.hasParam("weight_poly_obstacle"))
ROS_WARN("TebLocalPlannerROS() Param Warning: 'weight_point_obstacle', 'weight_line_obstacle' and 'weight_poly_obstacle' are deprecated. They are replaced by the single param 'weight_obstacle'.");
if (nh.hasParam("costmap_obstacles_front_only"))
ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_obstacles_front_only' is deprecated. It is replaced by 'costmap_obstacles_behind_robot_dist' to define the actual area taken into account.");
if (nh.hasParam("costmap_emergency_stop_dist"))
ROS_WARN("TebLocalPlannerROS() Param Warning: 'costmap_emergency_stop_dist' is deprecated. You can safely remove it from your parameter config.");
if (nh.hasParam("alternative_time_cost"))
ROS_WARN("TebLocalPlannerROS() Param Warning: 'alternative_time_cost' is deprecated. It has been replaced by 'selection_alternative_time_cost'.");
if (nh.hasParam("global_plan_via_point_sep"))
ROS_WARN("TebLocalPlannerROS() Param Warning: 'global_plan_via_point_sep' is deprecated. It has been replaced by 'global_plan_viapoint_sep' due to consistency reasons.");
}
} // namespace teb_local_planner

1218
navigations/teb_local_planner/src/teb_local_planner_ros.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

325
navigations/teb_local_planner/src/test_optim_node.cpp Executable file
View File

@@ -0,0 +1,325 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017.
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/teb_local_planner_ros.h>
#include <interactive_markers/interactive_marker_server.h>
#include <visualization_msgs/Marker.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
using namespace teb_local_planner; // it is ok here to import everything for testing purposes
// ============= Global Variables ================
// Ok global variables are bad, but here we only have a simple testing node.
PlannerInterfacePtr planner;
TebVisualizationPtr visual;
std::vector<ObstaclePtr> obst_vector;
ViaPointContainer via_points;
TebConfig config;
boost::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg;
ros::Subscriber custom_obst_sub;
ros::Subscriber via_points_sub;
ros::Subscriber clicked_points_sub;
std::vector<ros::Subscriber> obst_vel_subs;
unsigned int no_fixed_obstacles;
// =========== Function declarations =============
void CB_mainCycle(const ros::TimerEvent& e);
void CB_publishCycle(const ros::TimerEvent& e);
void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level);
void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb);
void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg);
void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg);
void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr& twist_msg, const unsigned int id);
// =============== Main function =================
int main( int argc, char** argv )
{
ros::init(argc, argv, "test_optim_node");
ros::NodeHandle n("~");
// load ros parameters from node handle
config.loadRosParamFromNodeHandle(n);
ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle);
ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle);
// setup dynamic reconfigure
dynamic_recfg = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(n);
dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(CB_reconfigure, _1, _2);
dynamic_recfg->setCallback(cb);
// setup callback for custom obstacles
custom_obst_sub = n.subscribe("obstacles", 1, CB_customObstacle);
// setup callback for clicked points (in rviz) that are considered as via-points
clicked_points_sub = n.subscribe("/clicked_point", 5, CB_clicked_points);
// setup callback for via-points (callback overwrites previously set via-points)
via_points_sub = n.subscribe("via_points", 1, CB_via_points);
// interactive marker server for simulated dynamic obstacles
interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles");
obst_vector.push_back( boost::make_shared<PointObstacle>(-3,1) );
obst_vector.push_back( boost::make_shared<PointObstacle>(6,2) );
obst_vector.push_back( boost::make_shared<PointObstacle>(0,0.1) );
// obst_vector.push_back( boost::make_shared<LineObstacle>(1,1.5,1,-1.5) ); //90 deg
// obst_vector.push_back( boost::make_shared<LineObstacle>(1,0,-1,0) ); //180 deg
// obst_vector.push_back( boost::make_shared<PointObstacle>(-1.5,-0.5) );
// Dynamic obstacles
Eigen::Vector2d vel (0.1, -0.3);
obst_vector.at(0)->setCentroidVelocity(vel);
vel = Eigen::Vector2d(-0.3, -0.2);
obst_vector.at(1)->setCentroidVelocity(vel);
/*
PolygonObstacle* polyobst = new PolygonObstacle;
polyobst->pushBackVertex(1, -1);
polyobst->pushBackVertex(0, 1);
polyobst->pushBackVertex(1, 1);
polyobst->pushBackVertex(2, 1);
polyobst->finalizePolygon();
obst_vector.emplace_back(polyobst);
*/
for (unsigned int i=0; i<obst_vector.size(); ++i)
{
// setup callbacks for setting obstacle velocities
std::string topic = "/test_optim_node/obstacle_" + std::to_string(i) + "/cmd_vel";
obst_vel_subs.push_back(n.subscribe<geometry_msgs::Twist>(topic, 1, boost::bind(&CB_setObstacleVelocity, _1, i)));
//CreateInteractiveMarker(obst_vector.at(i)[0],obst_vector.at(i)[1],i,&marker_server, &CB_obstacle_marker);
// Add interactive markers for all point obstacles
boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(obst_vector.at(i));
if (pobst)
{
CreateInteractiveMarker(pobst->x(),pobst->y(),i, config.map_frame, &marker_server, &CB_obstacle_marker);
}
}
marker_server.applyChanges();
// Setup visualization
visual = TebVisualizationPtr(new TebVisualization(n, config));
// Setup robot shape model
RobotFootprintModelPtr robot_model = TebLocalPlannerROS::getRobotFootprintFromParamServer(n, config);
// Setup planner (homotopy class planning or just the local teb planner)
if (config.hcp.enable_homotopy_class_planning)
planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points));
else
planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points));
no_fixed_obstacles = obst_vector.size();
ros::spin();
return 0;
}
// Planning loop
void CB_mainCycle(const ros::TimerEvent& e)
{
planner->plan(PoseSE2(-4,0,0), PoseSE2(4,0,0)); // hardcoded start and goal for testing purposes
}
// Visualization loop
void CB_publishCycle(const ros::TimerEvent& e)
{
planner->visualize();
visual->publishObstacles(obst_vector);
visual->publishViaPoints(via_points);
}
void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level)
{
config.reconfigure(reconfig);
}
void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb)
{
// create an interactive marker for our server
visualization_msgs::InteractiveMarker i_marker;
i_marker.header.frame_id = frame;
i_marker.header.stamp = ros::Time::now();
std::ostringstream oss;
//oss << "obstacle" << id;
oss << id;
i_marker.name = oss.str();
i_marker.description = "Obstacle";
i_marker.pose.position.x = init_x;
i_marker.pose.position.y = init_y;
i_marker.pose.orientation.w = 1.0f; // make quaternion normalized
// create a grey box marker
visualization_msgs::Marker box_marker;
box_marker.type = visualization_msgs::Marker::CUBE;
box_marker.id = id;
box_marker.scale.x = 0.2;
box_marker.scale.y = 0.2;
box_marker.scale.z = 0.2;
box_marker.color.r = 0.5;
box_marker.color.g = 0.5;
box_marker.color.b = 0.5;
box_marker.color.a = 1.0;
box_marker.pose.orientation.w = 1.0f; // make quaternion normalized
// create a non-interactive control which contains the box
visualization_msgs::InteractiveMarkerControl box_control;
box_control.always_visible = true;
box_control.markers.push_back( box_marker );
// add the control to the interactive marker
i_marker.controls.push_back( box_control );
// create a control which will move the box, rviz will insert 2 arrows
visualization_msgs::InteractiveMarkerControl move_control;
move_control.name = "move_x";
move_control.orientation.w = 0.707107f;
move_control.orientation.x = 0;
move_control.orientation.y = 0.707107f;
move_control.orientation.z = 0;
move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
// add the control to the interactive marker
i_marker.controls.push_back(move_control);
// add the interactive marker to our collection
marker_server->insert(i_marker);
marker_server->setCallback(i_marker.name,feedback_cb);
}
void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
std::stringstream ss(feedback->marker_name);
unsigned int index;
ss >> index;
if (index>=no_fixed_obstacles)
return;
PointObstacle* pobst = static_cast<PointObstacle*>(obst_vector.at(index).get());
pobst->position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y);
}
void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg)
{
// resize such that the vector contains only the fixed obstacles specified inside the main function
obst_vector.resize(no_fixed_obstacles);
// Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame)
for (size_t i = 0; i < obst_msg->obstacles.size(); ++i)
{
if (obst_msg->obstacles.at(i).polygon.points.size() == 1 )
{
if (obst_msg->obstacles.at(i).radius == 0)
{
obst_vector.push_back(ObstaclePtr(new PointObstacle( obst_msg->obstacles.at(i).polygon.points.front().x,
obst_msg->obstacles.at(i).polygon.points.front().y )));
}
else
{
obst_vector.push_back(ObstaclePtr(new CircularObstacle( obst_msg->obstacles.at(i).polygon.points.front().x,
obst_msg->obstacles.at(i).polygon.points.front().y,
obst_msg->obstacles.at(i).radius )));
}
}
else if (obst_msg->obstacles.at(i).polygon.points.empty())
{
ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping...");
continue;
}
else
{
PolygonObstacle* polyobst = new PolygonObstacle;
for (size_t j=0; j<obst_msg->obstacles.at(i).polygon.points.size(); ++j)
{
polyobst->pushBackVertex( obst_msg->obstacles.at(i).polygon.points[j].x,
obst_msg->obstacles.at(i).polygon.points[j].y );
}
polyobst->finalizePolygon();
obst_vector.push_back(ObstaclePtr(polyobst));
}
if(!obst_vector.empty())
obst_vector.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation);
}
}
void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg)
{
// we assume for simplicity that the fixed frame is already the map/planning frame
// consider clicked points as via-points
via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) );
ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added.");
if (config.optim.weight_viapoint<=0)
ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0");
}
void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg)
{
ROS_INFO_ONCE("Via-points received. This message is printed once.");
via_points.clear();
for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses)
{
via_points.emplace_back(pose.pose.position.x, pose.pose.position.y);
}
}
void CB_setObstacleVelocity(const geometry_msgs::TwistConstPtr& twist_msg, const unsigned int id)
{
if (id >= obst_vector.size())
{
ROS_WARN("Cannot set velocity: unknown obstacle id.");
return;
}
Eigen::Vector2d vel (twist_msg->linear.x, twist_msg->linear.y);
obst_vector.at(id)->setCentroidVelocity(vel);
}

634
navigations/teb_local_planner/src/timed_elastic_band.cpp Executable file
View File

@@ -0,0 +1,634 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/timed_elastic_band.h>
#include <limits>
namespace teb_local_planner
{
namespace
{
/**
* estimate the time to move from start to end.
* Assumes constant velocity for the motion.
*/
double estimateDeltaT(const PoseSE2& start, const PoseSE2& end,
double max_vel_x, double max_vel_theta)
{
double dt_constant_motion = 0.1;
if (max_vel_x > 0) {
double trans_dist = (end.position() - start.position()).norm();
dt_constant_motion = trans_dist / max_vel_x;
}
if (max_vel_theta > 0) {
double rot_dist = std::abs(g2o::normalize_theta(end.theta() - start.theta()));
dt_constant_motion = std::max(dt_constant_motion, rot_dist / max_vel_theta);
}
return dt_constant_motion;
}
} // namespace
TimedElasticBand::TimedElasticBand()
{
}
TimedElasticBand::~TimedElasticBand()
{
ROS_DEBUG("Destructor Timed_Elastic_Band...");
clearTimedElasticBand();
}
void TimedElasticBand::addPose(const PoseSE2& pose, bool fixed)
{
VertexPose* pose_vertex = new VertexPose(pose, fixed);
pose_vec_.push_back( pose_vertex );
return;
}
void TimedElasticBand::addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed)
{
VertexPose* pose_vertex = new VertexPose(position, theta, fixed);
pose_vec_.push_back( pose_vertex );
return;
}
void TimedElasticBand::addPose(double x, double y, double theta, bool fixed)
{
VertexPose* pose_vertex = new VertexPose(x, y, theta, fixed);
pose_vec_.push_back( pose_vertex );
return;
}
void TimedElasticBand::addTimeDiff(double dt, bool fixed)
{
ROS_ASSERT_MSG(dt > 0., "Adding a timediff requires a positive dt");
VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt, fixed);
timediff_vec_.push_back( timediff_vertex );
return;
}
void TimedElasticBand::addPoseAndTimeDiff(double x, double y, double angle, double dt)
{
if (sizePoses() != sizeTimeDiffs())
{
addPose(x,y,angle,false);
addTimeDiff(dt,false);
}
else
ROS_ERROR("Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
return;
}
void TimedElasticBand::addPoseAndTimeDiff(const PoseSE2& pose, double dt)
{
if (sizePoses() != sizeTimeDiffs())
{
addPose(pose,false);
addTimeDiff(dt,false);
} else
ROS_ERROR("Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
return;
}
void TimedElasticBand::addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt)
{
if (sizePoses() != sizeTimeDiffs())
{
addPose(position, theta,false);
addTimeDiff(dt,false);
} else
ROS_ERROR("Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf");
return;
}
void TimedElasticBand::deletePose(int index)
{
ROS_ASSERT(index<pose_vec_.size());
delete pose_vec_.at(index);
pose_vec_.erase(pose_vec_.begin()+index);
}
void TimedElasticBand::deletePoses(int index, int number)
{
ROS_ASSERT(index+number<=(int)pose_vec_.size());
for (int i = index; i<index+number; ++i)
delete pose_vec_.at(i);
pose_vec_.erase(pose_vec_.begin()+index, pose_vec_.begin()+index+number);
}
void TimedElasticBand::deleteTimeDiff(int index)
{
ROS_ASSERT(index<(int)timediff_vec_.size());
delete timediff_vec_.at(index);
timediff_vec_.erase(timediff_vec_.begin()+index);
}
void TimedElasticBand::deleteTimeDiffs(int index, int number)
{
ROS_ASSERT(index+number<=timediff_vec_.size());
for (int i = index; i<index+number; ++i)
delete timediff_vec_.at(i);
timediff_vec_.erase(timediff_vec_.begin()+index, timediff_vec_.begin()+index+number);
}
void TimedElasticBand::insertPose(int index, const PoseSE2& pose)
{
VertexPose* pose_vertex = new VertexPose(pose);
pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
}
void TimedElasticBand::insertPose(int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta)
{
VertexPose* pose_vertex = new VertexPose(position, theta);
pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
}
void TimedElasticBand::insertPose(int index, double x, double y, double theta)
{
VertexPose* pose_vertex = new VertexPose(x, y, theta);
pose_vec_.insert(pose_vec_.begin()+index, pose_vertex);
}
void TimedElasticBand::insertTimeDiff(int index, double dt)
{
VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt);
timediff_vec_.insert(timediff_vec_.begin()+index, timediff_vertex);
}
void TimedElasticBand::clearTimedElasticBand()
{
for (PoseSequence::iterator pose_it = pose_vec_.begin(); pose_it != pose_vec_.end(); ++pose_it)
delete *pose_it;
pose_vec_.clear();
for (TimeDiffSequence::iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it)
delete *dt_it;
timediff_vec_.clear();
}
void TimedElasticBand::setPoseVertexFixed(int index, bool status)
{
ROS_ASSERT(index<sizePoses());
pose_vec_.at(index)->setFixed(status);
}
void TimedElasticBand::setTimeDiffVertexFixed(int index, bool status)
{
ROS_ASSERT(index<sizeTimeDiffs());
timediff_vec_.at(index)->setFixed(status);
}
void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, int min_samples, int max_samples, bool fast_mode)
{
ROS_ASSERT(sizeTimeDiffs() == 0 || sizeTimeDiffs() + 1 == sizePoses());
/// iterate through all TEB states and add/remove states!
bool modified = true;
for (int rep = 0; rep < 100 && modified; ++rep) // actually it should be while(), but we want to make sure to not get stuck in some oscillation, hence max 100 repitions.
{
modified = false;
for(int i=0; i < sizeTimeDiffs(); ++i) // TimeDiff connects Point(i) with Point(i+1)
{
if(TimeDiff(i) > dt_ref + dt_hysteresis && sizeTimeDiffs()<max_samples)
{
// Force the planner to have equal timediffs between poses (dt_ref +/- dt_hyteresis).
// (new behaviour)
if (TimeDiff(i) > 2*dt_ref)
{
double newtime = 0.5*TimeDiff(i);
TimeDiff(i) = newtime;
insertPose(i+1, PoseSE2::average(Pose(i),Pose(i+1)) );
insertTimeDiff(i+1,newtime);
i--; // check the updated pose diff again
modified = true;
}
else
{
if (i < sizeTimeDiffs() - 1)
{
timediffs().at(i+1)->dt()+= timediffs().at(i)->dt() - dt_ref;
}
timediffs().at(i)->dt() = dt_ref;
}
}
else if(TimeDiff(i) < dt_ref - dt_hysteresis && sizeTimeDiffs()>min_samples) // only remove samples if size is larger than min_samples.
{
//ROS_DEBUG("teb_local_planner: autoResize() deleting bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs());
if(i < ((int)sizeTimeDiffs()-1))
{
TimeDiff(i+1) = TimeDiff(i+1) + TimeDiff(i);
deleteTimeDiff(i);
deletePose(i+1);
i--; // check the updated pose diff again
}
else
{ // last motion should be adjusted, shift time to the interval before
TimeDiff(i-1) += TimeDiff(i);
deleteTimeDiff(i);
deletePose(i);
}
modified = true;
}
}
if (fast_mode) break;
}
}
double TimedElasticBand::getSumOfAllTimeDiffs() const
{
double time = 0;
for(TimeDiffSequence::const_iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it)
{
time += (*dt_it)->dt();
}
return time;
}
double TimedElasticBand::getSumOfTimeDiffsUpToIdx(int index) const
{
ROS_ASSERT(index<=timediff_vec_.size());
double time = 0;
for(int i = 0; i < index; ++i)
{
time += timediff_vec_.at(i)->dt();
}
return time;
}
double TimedElasticBand::getAccumulatedDistance() const
{
double dist = 0;
for(int i=1; i<sizePoses(); ++i)
{
dist += (Pose(i).position() - Pose(i-1).position()).norm();
}
return dist;
}
bool TimedElasticBand::initTrajectoryToGoal(const PoseSE2& start, const PoseSE2& goal, double diststep, double max_vel_x, int min_samples, bool guess_backwards_motion)
{
if (!isInit())
{
addPose(start); // add starting point
setPoseVertexFixed(0,true); // StartConf is a fixed constraint during optimization
double timestep = 0.1;
if (diststep!=0)
{
Eigen::Vector2d point_to_goal = goal.position()-start.position();
double dir_to_goal = std::atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
double dx = diststep*std::cos(dir_to_goal);
double dy = diststep*std::sin(dir_to_goal);
double orient_init = dir_to_goal;
// check if the goal is behind the start pose (w.r.t. start orientation)
if (guess_backwards_motion && point_to_goal.dot(start.orientationUnitVec()) < 0)
orient_init = g2o::normalize_theta(orient_init+M_PI);
// TODO: timestep ~ max_vel_x_backwards for backwards motions
double dist_to_goal = point_to_goal.norm();
double no_steps_d = dist_to_goal/std::abs(diststep); // ignore negative values
unsigned int no_steps = (unsigned int) std::floor(no_steps_d);
if (max_vel_x > 0) timestep = diststep / max_vel_x;
for (unsigned int i=1; i<=no_steps; i++) // start with 1! starting point had index 0
{
if (i==no_steps && no_steps_d==(float) no_steps)
break; // if last conf (depending on stepsize) is equal to goal conf -> leave loop
addPoseAndTimeDiff(start.x()+i*dx,start.y()+i*dy,orient_init,timestep);
}
}
// if number of samples is not larger than min_samples, insert manually
if ( sizePoses() < min_samples-1 )
{
ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
while (sizePoses() < min_samples-1) // subtract goal point that will be added later
{
// simple strategy: interpolate between the current pose and the goal
PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
if (max_vel_x > 0) timestep = (intermediate_pose.position()-BackPose().position()).norm()/max_vel_x;
addPoseAndTimeDiff( intermediate_pose, timestep ); // let the optimier correct the timestep (TODO: better initialization
}
}
// add goal
if (max_vel_x > 0) timestep = (goal.position()-BackPose().position()).norm()/max_vel_x;
addPoseAndTimeDiff(goal,timestep); // add goal point
setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
}
else // size!=0
{
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)!");
ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d",(unsigned int) sizePoses(),(unsigned int) sizeTimeDiffs());
return false;
}
return true;
}
bool TimedElasticBand::initTrajectoryToGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient, int min_samples, bool guess_backwards_motion)
{
if (!isInit())
{
PoseSE2 start(plan.front().pose);
PoseSE2 goal(plan.back().pose);
addPose(start); // add starting point with given orientation
setPoseVertexFixed(0,true); // StartConf is a fixed constraint during optimization
bool backwards = false;
if (guess_backwards_motion && (goal.position()-start.position()).dot(start.orientationUnitVec()) < 0) // check if the goal is behind the start pose (w.r.t. start orientation)
backwards = true;
// TODO: dt ~ max_vel_x_backwards for backwards motions
for (int i=1; i<(int)plan.size()-1; ++i)
{
double yaw;
if (estimate_orient)
{
// get yaw from the orientation of the distance vector between pose_{i+1} and pose_{i}
double dx = plan[i+1].pose.position.x - plan[i].pose.position.x;
double dy = plan[i+1].pose.position.y - plan[i].pose.position.y;
yaw = std::atan2(dy,dx);
if (backwards)
yaw = g2o::normalize_theta(yaw+M_PI);
}
else
{
yaw = tf::getYaw(plan[i].pose.orientation);
}
PoseSE2 intermediate_pose(plan[i].pose.position.x, plan[i].pose.position.y, yaw);
double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
addPoseAndTimeDiff(intermediate_pose, dt);
}
// if number of samples is not larger than min_samples, insert manually
if ( sizePoses() < min_samples-1 )
{
ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
while (sizePoses() < min_samples-1) // subtract goal point that will be added later
{
// simple strategy: interpolate between the current pose and the goal
PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal);
double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta);
addPoseAndTimeDiff( intermediate_pose, dt ); // let the optimier correct the timestep (TODO: better initialization
}
}
// Now add final state with given orientation
double dt = estimateDeltaT(BackPose(), goal, max_vel_x, max_vel_theta);
addPoseAndTimeDiff(goal, dt);
setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
}
else // size!=0
{
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)!");
ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs());
return false;
}
return true;
}
int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance, int begin_idx) const
{
int n = sizePoses();
if (begin_idx < 0 || begin_idx >= n)
return -1;
double min_dist_sq = std::numeric_limits<double>::max();
int min_idx = -1;
for (int i = begin_idx; i < n; i++)
{
double dist_sq = (ref_point - Pose(i).position()).squaredNorm();
if (dist_sq < min_dist_sq)
{
min_dist_sq = dist_sq;
min_idx = i;
}
}
if (distance)
*distance = std::sqrt(min_dist_sq);
return min_idx;
}
int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_line_start, const Eigen::Ref<const Eigen::Vector2d>& ref_line_end, double* distance) const
{
double min_dist = std::numeric_limits<double>::max();
int min_idx = -1;
for (int i = 0; i < sizePoses(); i++)
{
Eigen::Vector2d point = Pose(i).position();
double dist = distance_point_to_segment_2d(point, ref_line_start, ref_line_end);
if (dist < min_dist)
{
min_dist = dist;
min_idx = i;
}
}
if (distance)
*distance = min_dist;
return min_idx;
}
int TimedElasticBand::findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance) const
{
if (vertices.empty())
return 0;
else if (vertices.size() == 1)
return findClosestTrajectoryPose(vertices.front());
else if (vertices.size() == 2)
return findClosestTrajectoryPose(vertices.front(), vertices.back());
double min_dist = std::numeric_limits<double>::max();
int min_idx = -1;
for (int i = 0; i < sizePoses(); i++)
{
Eigen::Vector2d point = Pose(i).position();
double dist_to_polygon = std::numeric_limits<double>::max();
for (int j = 0; j < (int) vertices.size()-1; ++j)
{
dist_to_polygon = std::min(dist_to_polygon, distance_point_to_segment_2d(point, vertices[j], vertices[j+1]));
}
dist_to_polygon = std::min(dist_to_polygon, distance_point_to_segment_2d(point, vertices.back(), vertices.front()));
if (dist_to_polygon < min_dist)
{
min_dist = dist_to_polygon;
min_idx = i;
}
}
if (distance)
*distance = min_dist;
return min_idx;
}
int TimedElasticBand::findClosestTrajectoryPose(const Obstacle& obstacle, double* distance) const
{
const PointObstacle* pobst = dynamic_cast<const PointObstacle*>(&obstacle);
if (pobst)
return findClosestTrajectoryPose(pobst->position(), distance);
const LineObstacle* lobst = dynamic_cast<const LineObstacle*>(&obstacle);
if (lobst)
return findClosestTrajectoryPose(lobst->start(), lobst->end(), distance);
const PolygonObstacle* polyobst = dynamic_cast<const PolygonObstacle*>(&obstacle);
if (polyobst)
return findClosestTrajectoryPose(polyobst->vertices(), distance);
return findClosestTrajectoryPose(obstacle.getCentroid(), distance);
}
void TimedElasticBand::updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples)
{
// first and simple approach: change only start confs (and virtual start conf for inital velocity)
// TEST if optimizer can handle this "hard" placement
if (new_start && sizePoses()>0)
{
// find nearest state (using l2-norm) in order to prune the trajectory
// (remove already passed states)
double dist_cache = (new_start->position()- Pose(0).position()).norm();
double dist;
int lookahead = std::min<int>( sizePoses()-min_samples, 10); // satisfy min_samples, otherwise max 10 samples
int nearest_idx = 0;
for (int i = 1; i<=lookahead; ++i)
{
dist = (new_start->position()- Pose(i).position()).norm();
if (dist<dist_cache)
{
dist_cache = dist;
nearest_idx = i;
}
else break;
}
// prune trajectory at the beginning (and extrapolate sequences at the end if the horizon is fixed)
if (nearest_idx>0)
{
// nearest_idx is equal to the number of samples to be removed (since it counts from 0 ;-) )
// WARNING delete starting at pose 1, and overwrite the original pose(0) with new_start, since Pose(0) is fixed during optimization!
deletePoses(1, nearest_idx); // delete first states such that the closest state is the new first one
deleteTimeDiffs(1, nearest_idx); // delete corresponding time differences
}
// update start
Pose(0) = *new_start;
}
if (new_goal && sizePoses()>0)
{
BackPose() = *new_goal;
}
};
bool TimedElasticBand::isTrajectoryInsideRegion(double radius, double max_dist_behind_robot, int skip_poses)
{
if (sizePoses()<=0)
return true;
double radius_sq = radius*radius;
double max_dist_behind_robot_sq = max_dist_behind_robot*max_dist_behind_robot;
Eigen::Vector2d robot_orient = Pose(0).orientationUnitVec();
for (int i=1; i<sizePoses(); i=i+skip_poses+1)
{
Eigen::Vector2d dist_vec = Pose(i).position()-Pose(0).position();
double dist_sq = dist_vec.squaredNorm();
if (dist_sq > radius_sq)
{
ROS_INFO("outside robot");
return false;
}
// check behind the robot with a different distance, if specified (or >=0)
if (max_dist_behind_robot >= 0 && dist_vec.dot(robot_orient) < 0 && dist_sq > max_dist_behind_robot_sq)
{
ROS_INFO("outside robot behind");
return false;
}
}
return true;
}
} // namespace teb_local_planner

540
navigations/teb_local_planner/src/visualization.cpp Executable file
View File

@@ -0,0 +1,540 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2016,
* TU Dortmund - Institute of Control Theory and Systems Engineering.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the institute nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christoph Rösmann
*********************************************************************/
#include <teb_local_planner/visualization.h>
#include <teb_local_planner/optimal_planner.h>
#include <teb_local_planner/FeedbackMsg.h>
namespace teb_local_planner
{
TebVisualization::TebVisualization() : initialized_(false)
{
}
TebVisualization::TebVisualization(ros::NodeHandle& nh, const TebConfig& cfg) : initialized_(false)
{
initialize(nh, cfg);
}
void TebVisualization::initialize(ros::NodeHandle& nh, const TebConfig& cfg)
{
if (initialized_)
ROS_WARN("TebVisualization already initialized. Reinitalizing...");
// set config
cfg_ = &cfg;
// register topics
global_plan_pub_ = nh.advertise<nav_msgs::Path>("global_plan", 1);
local_plan_pub_ = nh.advertise<nav_msgs::Path>("local_plan",1);
teb_poses_pub_ = nh.advertise<geometry_msgs::PoseArray>("teb_poses", 100);
teb_marker_pub_ = nh.advertise<visualization_msgs::Marker>("teb_markers", 1000);
feedback_pub_ = nh.advertise<teb_local_planner::FeedbackMsg>("teb_feedback", 10);
initialized_ = true;
}
void TebVisualization::publishGlobalPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan) const
{
if ( printErrorWhenNotInitialized() ) return;
base_local_planner::publishPlan(global_plan, global_plan_pub_);
}
void TebVisualization::publishLocalPlan(const std::vector<geometry_msgs::PoseStamped>& local_plan) const
{
if ( printErrorWhenNotInitialized() )
return;
base_local_planner::publishPlan(local_plan, local_plan_pub_);
}
void TebVisualization::publishLocalPlanAndPoses(const TimedElasticBand& teb) const
{
if ( printErrorWhenNotInitialized() )
return;
// create path msg
nav_msgs::Path teb_path;
teb_path.header.frame_id = cfg_->map_frame;
teb_path.header.stamp = ros::Time::now();
// create pose_array (along trajectory)
geometry_msgs::PoseArray teb_poses;
teb_poses.header.frame_id = teb_path.header.frame_id;
teb_poses.header.stamp = teb_path.header.stamp;
// fill path msgs with teb configurations
for (int i=0; i < teb.sizePoses(); i++)
{
geometry_msgs::PoseStamped pose;
pose.header.frame_id = teb_path.header.frame_id;
pose.header.stamp = teb_path.header.stamp;
pose.pose.position.x = teb.Pose(i).x();
pose.pose.position.y = teb.Pose(i).y();
pose.pose.position.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*teb.getSumOfTimeDiffsUpToIdx(i);
pose.pose.orientation = tf::createQuaternionMsgFromYaw(teb.Pose(i).theta());
teb_path.poses.push_back(pose);
teb_poses.poses.push_back(pose.pose);
}
local_plan_pub_.publish(teb_path);
teb_poses_pub_.publish(teb_poses);
}
void TebVisualization::publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model,
const std::string& ns, const std_msgs::ColorRGBA &color)
{
if ( printErrorWhenNotInitialized() )
return;
std::vector<visualization_msgs::Marker> markers;
robot_model.visualizeRobot(current_pose, markers, color);
if (markers.empty())
return;
int idx = 0;
for (std::vector<visualization_msgs::Marker>::iterator marker_it = markers.begin(); marker_it != markers.end(); ++marker_it, ++idx)
{
marker_it->header.frame_id = cfg_->map_frame;
marker_it->header.stamp = ros::Time::now();
marker_it->action = visualization_msgs::Marker::ADD;
marker_it->ns = ns;
marker_it->id = idx;
marker_it->lifetime = ros::Duration(2.0);
teb_marker_pub_.publish(*marker_it);
}
}
void TebVisualization::publishRobotFootprint(const PoseSE2& current_pose, const std::vector<geometry_msgs::Point>& footprint,
const std::string& ns, const std_msgs::ColorRGBA &color)
{
if ( printErrorWhenNotInitialized() || footprint.empty() )
return;
visualization_msgs::Marker vertex_marker;
vertex_marker.header.frame_id = cfg_->map_frame;
vertex_marker.header.stamp = ros::Time::now();
vertex_marker.ns = ns;
vertex_marker.type = visualization_msgs::Marker::LINE_STRIP;
vertex_marker.action = visualization_msgs::Marker::ADD;
vertex_marker.color = color;
vertex_marker.scale.x = 0.01;
vertex_marker.lifetime = ros::Duration(2.0);
vertex_marker.points = footprint;
vertex_marker.points.push_back(footprint.front()); // close the polygon
current_pose.toPoseMsg(vertex_marker.pose);
teb_marker_pub_.publish(vertex_marker);
}
void TebVisualization::publishInfeasibleRobotPose(const PoseSE2& infeasible_pose,
const BaseRobotFootprintModel& robot_model,
const std::vector<geometry_msgs::Point>& footprint)
{
publishRobotFootprintModel(infeasible_pose, robot_model, "InfeasibleRobotPose/model", toColorMsg(0.5, 0.8, 0.0, 0.0));
publishRobotFootprint(infeasible_pose, footprint, "InfeasibleRobotPose/footprint", toColorMsg(0.5, 0.9, 0.7, 0.0));
}
void TebVisualization::publishObstacles(const ObstContainer& obstacles, double scale) const
{
if ( obstacles.empty() || printErrorWhenNotInitialized() )
return;
// Visualize point obstacles
{
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "PointObstacles";
marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
{
boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(*obst);
if (!pobst)
continue;
if (cfg_->hcp.visualize_with_time_as_z_axis_scale < 0.001)
{
geometry_msgs::Point point;
point.x = pobst->x();
point.y = pobst->y();
point.z = 0;
marker.points.push_back(point);
}
else // Spatiotemporally point obstacles become a line
{
marker.type = visualization_msgs::Marker::LINE_LIST;
geometry_msgs::Point start;
start.x = pobst->x();
start.y = pobst->y();
start.z = 0;
marker.points.push_back(start);
geometry_msgs::Point end;
double t = 20;
Eigen::Vector2d pred;
pobst->predictCentroidConstantVelocity(t, pred);
end.x = pred[0];
end.y = pred[1];
end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*t;
marker.points.push_back(end);
}
}
marker.scale.x = scale;
marker.scale.y = scale;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
teb_marker_pub_.publish( marker );
}
// Visualize circular obstacles
{
std::size_t idx = 0;
for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
{
boost::shared_ptr<CircularObstacle> pobst = boost::dynamic_pointer_cast<CircularObstacle>(*obst);
if (!pobst)
continue;
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "CircularObstacles";
marker.id = idx++;
marker.type = visualization_msgs::Marker::SPHERE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
geometry_msgs::Point point;
point.x = pobst->x();
point.y = pobst->y();
point.z = 0;
marker.points.push_back(point);
marker.scale.x = pobst->radius();
marker.scale.y = pobst->radius();
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
teb_marker_pub_.publish( marker );
}
}
// Visualize line obstacles
{
std::size_t idx = 0;
for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
{
boost::shared_ptr<LineObstacle> pobst = boost::dynamic_pointer_cast<LineObstacle>(*obst);
if (!pobst)
continue;
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "LineObstacles";
marker.id = idx++;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
geometry_msgs::Point start;
start.x = pobst->start().x();
start.y = pobst->start().y();
start.z = 0;
marker.points.push_back(start);
geometry_msgs::Point end;
end.x = pobst->end().x();
end.y = pobst->end().y();
end.z = 0;
marker.points.push_back(end);
marker.scale.x = scale;
marker.scale.y = scale;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
teb_marker_pub_.publish( marker );
}
}
// Visualize polygon obstacles
{
std::size_t idx = 0;
for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
{
boost::shared_ptr<PolygonObstacle> pobst = boost::dynamic_pointer_cast<PolygonObstacle>(*obst);
if (!pobst)
continue;
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "PolyObstacles";
marker.id = idx++;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
for (Point2dContainer::const_iterator vertex = pobst->vertices().begin(); vertex != pobst->vertices().end(); ++vertex)
{
geometry_msgs::Point point;
point.x = vertex->x();
point.y = vertex->y();
point.z = 0;
marker.points.push_back(point);
}
// Also add last point to close the polygon
// but only if polygon has more than 2 points (it is not a line)
if (pobst->vertices().size() > 2)
{
geometry_msgs::Point point;
point.x = pobst->vertices().front().x();
point.y = pobst->vertices().front().y();
point.z = 0;
marker.points.push_back(point);
}
marker.scale.x = scale;
marker.scale.y = scale;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
teb_marker_pub_.publish( marker );
}
}
}
void TebVisualization::publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns) const
{
if ( via_points.empty() || printErrorWhenNotInitialized() )
return;
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
for (std::size_t i=0; i < via_points.size(); ++i)
{
geometry_msgs::Point point;
point.x = via_points[i].x();
point.y = via_points[i].y();
point.z = 0;
marker.points.push_back(point);
}
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
teb_marker_pub_.publish( marker );
}
void TebVisualization::publishTebContainer(const TebOptPlannerContainer& teb_planner, const std::string& ns)
{
if ( printErrorWhenNotInitialized() )
return;
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_LIST;
marker.action = visualization_msgs::Marker::ADD;
// Iterate through teb pose sequence
for( TebOptPlannerContainer::const_iterator it_teb = teb_planner.begin(); it_teb != teb_planner.end(); ++it_teb )
{
// iterate single poses
PoseSequence::const_iterator it_pose = it_teb->get()->teb().poses().begin();
TimeDiffSequence::const_iterator it_timediff = it_teb->get()->teb().timediffs().begin();
PoseSequence::const_iterator it_pose_end = it_teb->get()->teb().poses().end();
std::advance(it_pose_end, -1); // since we are interested in line segments, reduce end iterator by one.
double time = 0;
while (it_pose != it_pose_end)
{
geometry_msgs::Point point_start;
point_start.x = (*it_pose)->x();
point_start.y = (*it_pose)->y();
point_start.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time;
marker.points.push_back(point_start);
time += (*it_timediff)->dt();
geometry_msgs::Point point_end;
point_end.x = (*boost::next(it_pose))->x();
point_end.y = (*boost::next(it_pose))->y();
point_end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time;
marker.points.push_back(point_end);
++it_pose;
++it_timediff;
}
}
marker.scale.x = 0.01;
marker.color.a = 1.0;
marker.color.r = 0.5;
marker.color.g = 1.0;
marker.color.b = 0.0;
teb_marker_pub_.publish( marker );
}
void TebVisualization::publishFeedbackMessage(const std::vector< boost::shared_ptr<TebOptimalPlanner> >& teb_planners,
unsigned int selected_trajectory_idx, const ObstContainer& obstacles)
{
FeedbackMsg msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = cfg_->map_frame;
msg.selected_trajectory_idx = selected_trajectory_idx;
msg.trajectories.resize(teb_planners.size());
// Iterate through teb pose sequence
std::size_t idx_traj = 0;
for( TebOptPlannerContainer::const_iterator it_teb = teb_planners.begin(); it_teb != teb_planners.end(); ++it_teb, ++idx_traj )
{
msg.trajectories[idx_traj].header = msg.header;
it_teb->get()->getFullTrajectory(msg.trajectories[idx_traj].trajectory);
}
// add obstacles
msg.obstacles_msg.obstacles.resize(obstacles.size());
for (std::size_t i=0; i<obstacles.size(); ++i)
{
msg.obstacles_msg.header = msg.header;
// copy polygon
msg.obstacles_msg.obstacles[i].header = msg.header;
obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
// copy id
msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
// orientation
//msg.obstacles_msg.obstacles[i].orientation =; // TODO
// copy velocities
obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
}
feedback_pub_.publish(msg);
}
void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles)
{
FeedbackMsg msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = cfg_->map_frame;
msg.selected_trajectory_idx = 0;
msg.trajectories.resize(1);
msg.trajectories.front().header = msg.header;
teb_planner.getFullTrajectory(msg.trajectories.front().trajectory);
// add obstacles
msg.obstacles_msg.obstacles.resize(obstacles.size());
for (std::size_t i=0; i<obstacles.size(); ++i)
{
msg.obstacles_msg.header = msg.header;
// copy polygon
msg.obstacles_msg.obstacles[i].header = msg.header;
obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
// copy id
msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
// orientation
//msg.obstacles_msg.obstacles[i].orientation =; // TODO
// copy velocities
obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
}
feedback_pub_.publish(msg);
}
std_msgs::ColorRGBA TebVisualization::toColorMsg(double a, double r, double g, double b)
{
std_msgs::ColorRGBA color;
color.a = a;
color.r = r;
color.g = g;
color.b = b;
return color;
}
bool TebVisualization::printErrorWhenNotInitialized() const
{
if (!initialized_)
{
ROS_ERROR("TebVisualization class not initialized. You must call initialize or an appropriate constructor");
return true;
}
return false;
}
} // namespace teb_local_planner

14
navigations/teb_local_planner/teb_local_planner_plugin.xml Executable file
View File

@@ -0,0 +1,14 @@
<library path="lib/libteb_local_planner">
<class name="teb_local_planner/TebLocalPlannerROS" type="teb_local_planner::TebLocalPlannerROS" base_class_type="nav_core::BaseLocalPlanner">
<description>
The teb_local_planner package implements a plugin
to the base_local_planner of the 2D navigation stack.
</description>
</class>
<class name="teb_local_planner/TebLocalPlannerROS" type="teb_local_planner::TebLocalPlannerROS" base_class_type="mbf_costmap_core::CostmapController">
<description>
Same plugin implemented MBF CostmapController's extended interface.
</description>
</class>
</library>

73
navigations/teb_local_planner/test/teb_basics.cpp Executable file
View File

@@ -0,0 +1,73 @@
#include <gtest/gtest.h>
#include <teb_local_planner/timed_elastic_band.h>
TEST(TEBBasic, autoResizeLargeValueAtEnd)
{
double dt = 0.1;
double dt_hysteresis = dt/3.;
teb_local_planner::TimedElasticBand teb;
teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
for (int i = 1; i < 10; ++i) {
teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
}
// add a pose with a large timediff as the last one
teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt + 2*dt_hysteresis);
// auto resize + test of the result
teb.autoResize(dt, dt_hysteresis, 3, 100, false);
for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
}
}
TEST(TEBBasic, autoResizeSmallValueAtEnd)
{
double dt = 0.1;
double dt_hysteresis = dt/3.;
teb_local_planner::TimedElasticBand teb;
teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
for (int i = 1; i < 10; ++i) {
teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
}
// add a pose with a small timediff as the last one
teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis);
// auto resize + test of the result
teb.autoResize(dt, dt_hysteresis, 3, 100, false);
for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
}
}
TEST(TEBBasic, autoResize)
{
double dt = 0.1;
double dt_hysteresis = dt/3.;
teb_local_planner::TimedElasticBand teb;
teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
for (int i = 1; i < 10; ++i) {
teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
}
// modify the timediff in the middle and add a pose with a smaller timediff as the last one
teb.TimeDiff(5) = dt + 2*dt_hysteresis;
teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis);
// auto resize
teb.autoResize(dt, dt_hysteresis, 3, 100, false);
for (int i = 0; i < teb.sizeTimeDiffs(); ++i) {
ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
}
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}