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

The package contains a RViz visualization, which has been tested with ROS 1 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,” 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.

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.

Dependencies

This package depends on the linear algebra library Eigen, which can be installed by

sudo apt-get install libeigen3-dev

The ROS 1 or ROS 2 dependencies are listed in the package.xml and can be installed by

rosdep install steering_functions

Installation & Usage as a ROS 1 or ROS 2 package

Building

To build this package from source, clone it into your workspace and compile it in Release mode according to

cd my_ws/src
git clone https://github.com/hbanzhaf/steering_functions.git
catkin build steering_functions -DCMAKE_BUILD_TYPE=Release  # ROS 1
colcon build --packages-up-to steering_functions --cmake-args -DCMAKE_BUILD_TYPE=Release  # ROS 1 or ROS 2

To launch a ROS 1 demo of the package, execute

source my_ws/devel/setup.bash
roslaunch steering_functions steering_functions.launch

Linking

Add this package as dependency to your package.xml

<depend>steering_functions</depend>

To link this library with another ROS 1 package, add these lines to your package’s CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS steering_functions)
include_directories(${catkin_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})

Or for ROS 2:

find_package(steering_functions REQUIRED)
target_link_libraries(your_target steering_functions::steering_functions)

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 ROS 1 unit tests, execute

catkin build steering_functions -DCMAKE_BUILD_TYPE=Release --make-args tests

To run a single test, e.g. the timing test, execute

cd my_ws/devel/lib/steering_functions
./timing_test

Installation & Usage as a standalone library

Dependencies

This package depends on the linear algebra library Eigen, which can be installed by

sudo apt-get install libeigen3-dev

Building

To build this package from source, clone it and compile it in Release mode without ROS support according to

git clone https://github.com/hbanzhaf/steering_functions.git
cd steering_functions/
mkdir build && cd build/
cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_WITH_ROS=OFF
cmake --build . --parallel
cmake --install .

A shared library (libsteering_functions.so) can be built instead of the default static library (libsteering_functions.a) by

cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_WITH_ROS=OFF -DBUILD_SHARED_LIBS=ON

Linking

To link this library to another C++ library/executable, add these lines to your CMakeLists.txt

find_package(steering_functions CONFIG REQUIRED)
target_link_libraries(${PROJECT_NAME}
  steering_functions::steering_functions
)

Now the steering functions can be used in your project by including the appropriate header, e.g.

#include <steering_functions/hc_cc_state_space/hc00_reeds_shepp_state_space.hpp>

Documentation

Conventions

In this implementation, a path is described by N segments. Each segment is given by the open-loop control inputs u_k = [delta_s_k, kappa_k, sigma_k]^T, where k = 1…N iterates over the N segments, delta_s_k describes the signed arc length of segment k, kappa_k the curvature at the beginning of segment k, and sigma_k 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 u_k. 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 x_s = [x_s, y_s, theta_s, kappa_s, d_s]^T and a goal state x_g = [x_g, y_g, theta_g, kappa_g, d_g]^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 d_s = d_g = 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 enforced 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 G² continuous steering functions against their G¹ continuous counterpart is illustrated in the following histograms (10⁵ 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:

Computation Times

The following table shows the current computation times of the implemented steering functions, which are obtained from 10⁵ random steering procedures on a single core of an Intel Xeon E5@3.50 GHz, 10 MB cache:

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.

Contributing

Want to contribute? Great! You can do so through the standard GitHub pull request model. For large contributions we do encourage you to file a ticket in the GitHub issues tracking system prior to any code development to coordinate with the team early in the process. Coordinating up front helps to avoid frustration later on. Please properly document your code and follow the Google C++ Style Guide.

Your contribution must be licensed under the Apache-2.0 license, the license used by this project.

Add / retain copyright notices

Include a copyright notice and license in each new file to be contributed, consistent with the style used by this project. If your contribution contains code under the copyright of a third party, document its origin, license, and copyright holders.

Sign your work

This project tracks patch provenance and licensing using a modified Developer Certificate of Origin (DCO; from OSDL) and Signed-off-by tags initially developed by the Linux kernel project.

Developer's Certificate of Origin.  Version 1.0

By making a contribution to this project, I certify that:

(a) The contribution was created in whole or in part by me and I
    have the right to submit it under the "Apache License, Version 2.0"
    ("Apache-2.0"); or

(b) The contribution is based upon previous work that is covered by
    an appropriate open source license and I have the right under
    that license to submit that work with modifications, whether
    created in whole or in part by me, under the Apache-2.0 license;
    or

(c) The contribution was provided directly to me by some other
    person who certified (a) or (b) and I have not modified it.

(d) I understand and agree that this project and the contribution
    are public and that a record of the contribution (including all
    metadata and personal information I submit with it, including my
    sign-off) is maintained indefinitely and may be redistributed
    consistent with this project and the requirements of the Apache-2.0
    license or any open source license(s) involved, where they are
    relevant.

(e) I am granting the contribution to this project under the terms of
    Apache-2.0.

    http://www.apache.org/licenses/LICENSE-2.0

With the sign-off in a commit message you certify that you authored the patch or otherwise have the right to submit it under an open source license. The procedure is simple: To certify above this package’s Developer’s Certificate of Origin 1.0 for your contribution just append a line

Signed-off-by: Random J Developer <random@developer.example.org>

to every commit message using your real name or your pseudonym and a valid email address.

If you have set your user.name and user.email git configs you can automatically sign the commit by running the git-commit command with the -s option. There may be multiple sign-offs if more than one developer was involved in authoring the contribution.

For a more detailed description of this procedure, please see SubmittingPatches which was extracted from the Linux kernel project, and which is stored in an external repository.

Individual vs. Corporate Contributors

Often employers or academic institution have ownership over code that is written in certain circumstances, so please do due diligence to ensure that you have the right to submit the code.

If you are a developer who is authorized to contribute to this package on behalf of your employer, then please use your corporate email address in the Signed-off-by tag. Otherwise please use a personal email address.

Maintain Copyright holder / Contributor list

Each contributor is responsible for identifying themselves in the NOTICE file, the project’s list of copyright holders and authors. Please add the respective information corresponding to the Signed-off-by tag as part of your first pull request.

If you are a developer who is authorized to contribute to this package on behalf of your employer, then add your company / organization to the list of copyright holders in the NOTICE file. As author of a corporate contribution you can also add your name and corporate email address as in the Signed-off-by tag.

If your contribution is covered by this project’s DCO’s clause “(c) The contribution was provided directly to me by some other person who certified (a) or (b) and I have not modified it”, please add the appropriate copyright holder(s) to the NOTICE file as part of your contribution.