This project is forked from https://github.com/hbanzhaf/steering_functions
Everything related with ROS has been removed to be able to use it as part as stand-alone C++ libraries.
This package contains a C++ library that implements the following steering functions for car-like robots with limited turning radius (CC = continuous curvature, HC = hybrid curvature):
| Steering Function | Driving Direction | Continuity | Optimization Criterion |
|---|---|---|---|
| Dubins | forwards or backwards | G1 | path length (optimal) |
| CC-Dubins | forwards or backwards | G2 | path length (suboptimal) |
| Reeds-Shepp | forwards and backwards | G1 | path length (optimal) |
| HC-Reeds-Shepp | forwards and backwards | G2 btw. cusps | path length (suboptimal) |
| CC-Reeds-Shepp | forwards and backwards | G2 | path length (suboptimal) |
The package contains a RViz visualization, which has been tested with ROS Kinetic under Ubuntu 16.04.
A video of the steering functions integrated into the general motion planner Bidirectional RRT* can be found here.
For contributions, please check the instructions in CONTRIBUTING.
This software is a research prototype, originally developed for and published as part of the publication [2].
The software is not ready for production use. It has neither been developed nor tested for a specific use case. However, the license conditions of the applicable Open Source licenses allow you to adapt the software to your needs. Before using it in a safety relevant setting, make sure that the software fulfills your requirements and adjust it according to any applicable safety standards (e.g. ISO 26262).
If you use one of the above steering functions in your work, please cite the appropriate publication:
[1] H. Banzhaf et al., "From Footprints to Beliefprints: Motion Planning under Uncertainty for Maneuvering Automated Vehicles in Dense Scenarios," in IEEE International Conference on Intelligent Transportation Systems, 2018.
[2] H. Banzhaf et al., "Hybrid Curvature Steer: A Novel Extend Function for Sampling-Based Nonholonomic Motion Planning in Tight Environments," in IEEE International Conference on Intelligent Transportation Systems, 2017.
[3] L. E. Dubins, "On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents," in American Journal of Mathematics, 1957.
[4] J. Reeds and L. Shepp, "Optimal paths for a car that goes both forwards and backwards," in Pacific Journal of Mathematics, 1990.
[5] T. Fraichard and A. Scheuer, "From Reeds and Shepp's to Continuous-Curvature Paths," in IEEE Transactions on Robotics, 2004.
The source code in this package is released under the Apache-2.0 License. For further details, see the LICENSE file.
The 3rdparty-licenses.txt contains a list of other open source components included in this package.
This package depends on the linear algebra library Eigen, which can be installed by
sudo apt-get install libeigen3-dev
The ROS dependencies are listed in the package.xml and can be installed by
rosdep install steering_functions
To build this package from source, clone it into your catkin workspace and compile it in Release mode according to
cd catkin_ws/src
git clone https://github.com/hbanzhaf/steering_functions.git
catkin build steering_functions -DCMAKE_BUILD_TYPE=Release
To launch a demo of the package, execute
source catkin_ws/devel/setup.bash
roslaunch steering_functions steering_functions.launch
To link this library with another ROS package, add these lines to your package's CMakeLists.txt
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
steering_functions
)
include_directories(
${catkin_INCLUDE_DIRS}
)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
and the following lines to your package's package.xml
<build_depend>steering_functions</build_depend>
<run_depend>steering_functions</run_depend>
Now the steering functions can be used in your package by including the appropriate header, e.g.
#include "steering_functions/hc_cc_state_space/hc00_reeds_shepp_state_space.hpp"
To build the unit tests, exectue
catkin build steering_functions -DCMAKE_BUILD_TYPE=Release --make-args tests
To run a single test, e.g. the timing test, execute
cd catkin_ws/devel/lib/steering_functions
./timing_test
In this implementation, a path is described by N segments. Each segment is given by the open-loop control inputs uk = [delta_sk, kappak, sigmak]T, where k = 1...N iterates over the N segments, delta_sk describes the signed arc length of segment k, kappak the curvature at the beginning of segment k, and sigmak the linear change in curvature along segment k.
The states of the robot can be obtained with a user-specified discretization by forward integrating the open-loop controls uk. A robot state consists of x = [x, y, theta, kappa, d]T, where x, y describe the center of the rear axle, theta the orientation of the robot, kappa the curvature at position x, y, and d the driving direction ({-1,0,1}).
In addition to that, this package is capable of computing a Gaussian belief along the nominal path given an initial belief, the motion and measurement noise, and a feedback controller. A belief bel(x) is described by the mean mu, the covariance of the state estimate Sigma, the distribution over state estimates Lambda, and the total covariance Sigma + Lambda. Further details can be found in [1].
All steering functions expect a start state xs = [xs, ys, thetas, kappas, ds]T and a goal state xg = [xg, yg, thetag, kappag, dg]T as input. Note that the initial and final driving direction are selected by the steering function according to the computed path. They can not be selected manually, therefore, leave ds = dg = 0.
Depending on the two superscripts in the name of the steering function (see Section Computation Times for an overview), a different curvature is applied to the start (first superscript) and goal state (second superscript). The superscript 0 denotes that zero curvature is enfored no matter which curvature is given at that state. The superscript ± indicates that either positive or negative max. curvature is selected by the steering function if the user inputs no curvature. However, if a non-zero curvature is assigned, the steering function computes that path with the corresponding signed max. curvature. This feature can be useful in sampling-based motion planners when cuvature continuity has to be ensured at the connection of two extensions.
Additionally, the steering functions CC-Dubins and HC-Reeds-Shepp compute a path that takes into account an arbitrary start and goal curvature specified by the user.
A benchmark of the G2 continuous steering functions against their G1 continuous counterpart is illustrated in the following histograms (105 random steering procedures, max. curvature = 1 m-1, max. sharpness = 1 m-2.). The first row shows a comparison of the path length, and the second row compares the number of curvature discontinuities in the computed path:
The following table shows the current computation times of the implemented steering functions, which are obtained from 105 random steering procedures on a single core of an Intel Xeon E5@3.50 GHz, 10 MB cache:
| Steering Function | mean [µs] | std [µs] |
|---|---|---|
| Dubins | 1.3 | ±0.8 |
| CC±±-Dubins | 7.1 | ±3.1 |
| CC±0-Dubins | 5.6 | ±1.5 |
| CC0±-Dubins | 5.6 | ±1.6 |
| CC00-Dubins | 4.9 | ±1.5 |
| CC-Dubins | 14.8 | ±4.3 |
| --- | --- | --- |
| Reeds-Shepp | 7.4 | ±1.7 |
| HC±±-Reeds-Shepp | 58.1 | ±10.1 |
| HC±0-Reeds-Shepp | 56.4 | ±8.8 |
| HC0±-Reeds-Shepp | 51.0 | ±10.2 |
| HC00-Reeds-Shepp | 55.9 | ±9.5 |
| HC-Reeds-Shepp | 464.3 | ±72.8 |
| CC00-Reeds-Shepp | 53.8 | ±8.4 |
In order to use the continuous and hybrid curvature state spaces along with OMPL, a new OMPL state space has to be created as described here. OMPL requires a distance and an interpolate function, which are provided in this package.
Please use the Issue Tracker to report bugs or request features.