/steering_functions

Primary LanguageC++Apache License 2.0Apache-2.0

Steering Functions for Car-Like Robots

Overview

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)

Steering Functions for Car-Like Robots

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.

Purpose of the project

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).

Publications

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," (to be published).

[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.

License

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.

Installation & Usage

Dependencies

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

Building

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

Linking

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"

Testing

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

Documentation

Conventions

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].

Start and Goal State

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.

Statistics

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:

Statistics

Computation Times

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
CC-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
HC-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

Interfacing with OMPL

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.

Bugs & Feature Requests

Please use the Issue Tracker to report bugs or request features.