steering_functions repository

Repository Summary

Checkout URI https://github.com/hbanzhaf/steering_functions.git
VCS Type git
VCS Version master
Last Updated 2021-07-18
Dev Status MAINTAINED
CI status No Continuous Integration
Released RELEASED
Tags No category tags.
Contributing Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)

Packages

Name Version
steering_functions 0.1.0

README

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," 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.

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.

CONTRIBUTING

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.

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.

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.


Repository Summary

Checkout URI https://github.com/hbanzhaf/steering_functions.git
VCS Type git
VCS Version master
Last Updated 2021-07-18
Dev Status MAINTAINED
CI status No Continuous Integration
Released RELEASED
Tags No category tags.
Contributing Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)

Packages

Name Version
steering_functions 0.1.0

README

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," 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.

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.

CONTRIBUTING

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.

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.

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.