{"id":13445525,"url":"https://github.com/hbanzhaf/steering_functions","last_synced_at":"2025-03-20T21:30:48.607Z","repository":{"id":23172397,"uuid":"98341603","full_name":"hbanzhaf/steering_functions","owner":"hbanzhaf","description":null,"archived":false,"fork":false,"pushed_at":"2024-06-29T12:51:02.000Z","size":4422,"stargazers_count":226,"open_issues_count":0,"forks_count":92,"subscribers_count":7,"default_branch":"master","last_synced_at":"2024-08-01T05:14:58.928Z","etag":null,"topics":[],"latest_commit_sha":null,"homepage":null,"language":"C++","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"apache-2.0","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/hbanzhaf.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":"CONTRIBUTING.md","funding":null,"license":"LICENSE","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null}},"created_at":"2017-07-25T19:17:41.000Z","updated_at":"2024-07-27T17:19:42.000Z","dependencies_parsed_at":"2023-12-09T10:37:15.523Z","dependency_job_id":null,"html_url":"https://github.com/hbanzhaf/steering_functions","commit_stats":null,"previous_names":[],"tags_count":2,"template":false,"template_full_name":null,"repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/hbanzhaf%2Fsteering_functions","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/hbanzhaf%2Fsteering_functions/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/hbanzhaf%2Fsteering_functions/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/hbanzhaf%2Fsteering_functions/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/hbanzhaf","download_url":"https://codeload.github.com/hbanzhaf/steering_functions/tar.gz/refs/heads/master","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":221807736,"owners_count":16883639,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":[],"created_at":"2024-07-31T05:00:35.536Z","updated_at":"2025-03-20T21:30:48.602Z","avatar_url":"https://github.com/hbanzhaf.png","language":"C++","readme":"# Steering Functions for Car-Like Robots\n\n## Overview\n\nThis 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):\n\nSteering Function           | Driving Direction          | Continuity       | Optimization Criterion\n:---                        | :---                       | :---                       | :---\nDubins                      | forwards **or** backwards  | G\u003csup\u003e1\u003c/sup\u003e | path length (optimal)\nCC-Dubins                   | forwards **or** backwards  | G\u003csup\u003e2\u003c/sup\u003e | path length (suboptimal)\nReeds-Shepp                 | forwards **and** backwards | G\u003csup\u003e1\u003c/sup\u003e | path length (optimal)\nHC-Reeds-Shepp              | forwards **and** backwards | G\u003csup\u003e2\u003c/sup\u003e btw. cusps | path length (suboptimal)\nCC-Reeds-Shepp              | forwards **and** backwards | G\u003csup\u003e2\u003c/sup\u003e | path length (suboptimal)\n\n[![Steering Functions for Car-Like Robots](https://img.youtube.com/vi/YCT8ycMk6f8/0.jpg)](http://www.youtube.com/watch?v=YCT8ycMk6f8)\n\nThe package contains a [RViz] visualization, which has been tested with [ROS 1] Kinetic under Ubuntu 16.04.\n\nA video of the steering functions integrated into the general motion planner Bidirectional RRT* can be found [here](https://youtu.be/RlZZ4jnEhTM).\n\nFor contributions, please check the instructions in [CONTRIBUTING](CONTRIBUTING.md).\n\n\n## Purpose of the project\n\nThis software is a research prototype, originally developed for and published\nas part of the publication [2].\n\nThe software is not ready for production use. It has neither been developed nor\ntested for a specific use case. However, the license conditions of the\napplicable Open Source licenses allow you to adapt the software to your needs.\nBefore using it in a safety relevant setting, make sure that the software\nfulfills your requirements and adjust it according to any applicable safety\nstandards (e.g. ISO 26262).\n\n\n## Publications\nIf you use one of the above steering functions in your work, please cite the appropriate publication:\n\n[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.\n\n[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.\n\n[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.\n\n[4] J. Reeds and L. Shepp, **\"Optimal paths for a car that goes both forwards and backwards,\"** in Pacific Journal of Mathematics, 1990.\n\n[5] T. Fraichard and A. Scheuer, **\"From Reeds and Shepp's to Continuous-Curvature Paths,\"** in IEEE Transactions on Robotics, 2004.\n\n\n## License\n\nThe source code in this package is released under the Apache-2.0 License. For further details, see the [LICENSE](LICENSE) file.\n\nThe [3rdparty-licenses.txt](3rd-party-licenses.txt) contains a list of other open source components included in this package.\n\n\n## Dependencies\nThis package depends on the linear algebra library [Eigen], which can be installed by\n\n    sudo apt-get install libeigen3-dev\n\nThe [ROS 1] or [ROS 2] dependencies are listed in the package.xml and can be installed by\n\n    rosdep install steering_functions\n\n## Installation \u0026 Usage as a [ROS 1] or [ROS 2] package\n\n### Building\n\nTo build this package from source, clone it into your workspace and compile it in *Release* mode according to\n\n    cd my_ws/src\n    git clone https://github.com/hbanzhaf/steering_functions.git\n    catkin build steering_functions -DCMAKE_BUILD_TYPE=Release  # ROS 1\n    colcon build --packages-up-to steering_functions --cmake-args -DCMAKE_BUILD_TYPE=Release  # ROS 1 or ROS 2\n\nTo launch a [ROS 1] demo of the package, execute\n\n    source my_ws/devel/setup.bash\n    roslaunch steering_functions steering_functions.launch\n\n\n### Linking\n\nAdd this package as dependency to your package.xml\n\n    \u003cdepend\u003esteering_functions\u003c/depend\u003e\n\nTo link this library with another [ROS 1] package, add these lines to your package's CMakeLists.txt\n\n    find_package(catkin REQUIRED COMPONENTS steering_functions)\n    include_directories(${catkin_INCLUDE_DIRS})\n    target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})\n\nOr for [ROS 2]:\n\n    find_package(steering_functions REQUIRED)\n    target_link_libraries(your_target steering_functions::steering_functions)\n\nNow the steering functions can be used in your package by including the appropriate header, e.g.\n\n    #include \"steering_functions/hc_cc_state_space/hc00_reeds_shepp_state_space.hpp\"\n\n\n### Testing\n\nTo build the [ROS 1] unit tests, execute\n\n    catkin build steering_functions -DCMAKE_BUILD_TYPE=Release --make-args tests\n\nTo run a single test, e.g. the timing test, execute\n\n    cd my_ws/devel/lib/steering_functions\n    ./timing_test\n\n\n## Installation \u0026 Usage as a standalone library\n\n### Dependencies\nThis package depends on the linear algebra library [Eigen], which can be installed by\n\n    sudo apt-get install libeigen3-dev\n\n### Building\n\nTo build this package from source, clone it and compile it in *Release* mode without ROS support according to\n\n    git clone https://github.com/hbanzhaf/steering_functions.git\n    cd steering_functions/\n    mkdir build \u0026\u0026 cd build/\n    cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_WITH_ROS=OFF\n    cmake --build . --parallel\n    cmake --install .\n\nA shared library (libsteering_functions.so) can be built instead of the default static library (libsteering_functions.a) by\n\n    cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_WITH_ROS=OFF -DBUILD_SHARED_LIBS=ON\n\n### Linking\n\nTo link this library to another C++ library/executable, add these lines to your CMakeLists.txt\n\n    find_package(steering_functions CONFIG REQUIRED)\n    target_link_libraries(${PROJECT_NAME}\n      steering_functions::steering_functions\n    )\n\nNow the steering functions can be used in your project by including the appropriate header, e.g.\n\n    #include \u003csteering_functions/hc_cc_state_space/hc00_reeds_shepp_state_space.hpp\u003e\n\n\n\n## Documentation\n### Conventions\nIn this implementation, a path is described by *N* segments. Each segment is given by the open-loop control inputs **u**\u003csub\u003e*k*\u003c/sub\u003e = *[delta_s\u003csub\u003ek\u003c/sub\u003e, kappa\u003csub\u003ek\u003c/sub\u003e, sigma\u003csub\u003ek\u003c/sub\u003e]\u003csup\u003eT\u003c/sup\u003e*, where *k = 1...N* iterates over the *N* segments, *delta_s\u003csub\u003ek\u003c/sub\u003e* describes the signed arc length of segment *k*, *kappa\u003csub\u003ek\u003c/sub\u003e* the curvature at the beginning of segment *k*, and *sigma\u003csub\u003ek\u003c/sub\u003e* the linear change in curvature along segment *k*.\n\nThe states of the robot can be obtained with a user-specified discretization by forward integrating the open-loop controls **u**\u003csub\u003e*k*\u003c/sub\u003e. A robot state consists of **x** = *[x, y, theta, kappa, d]\u003csup\u003eT\u003c/sup\u003e*, 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}).\n\nIn 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].\n\n\n### Start and Goal State\nAll steering functions expect a start state **x**\u003csub\u003e*s*\u003c/sub\u003e = *[x\u003csub\u003es\u003c/sub\u003e, y\u003csub\u003es\u003c/sub\u003e, theta\u003csub\u003es\u003c/sub\u003e, kappa\u003csub\u003es\u003c/sub\u003e, d\u003csub\u003es\u003c/sub\u003e]\u003csup\u003eT\u003c/sup\u003e* and a goal state **x**\u003csub\u003e*g*\u003c/sub\u003e = *[x\u003csub\u003eg\u003c/sub\u003e, y\u003csub\u003eg\u003c/sub\u003e, theta\u003csub\u003eg\u003c/sub\u003e, kappa\u003csub\u003eg\u003c/sub\u003e, d\u003csub\u003eg\u003c/sub\u003e]\u003csup\u003eT\u003c/sup\u003e* 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\u003csub\u003es\u003c/sub\u003e* = *d\u003csub\u003eg\u003c/sub\u003e* = 0.\n\nDepending 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.\n\nAdditionally, 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.\n\n\n### Statistics\nA benchmark of the G\u003csup\u003e2\u003c/sup\u003e continuous steering functions against their G\u003csup\u003e1\u003c/sup\u003e continuous counterpart is illustrated in the following histograms (10\u003csup\u003e5\u003c/sup\u003e random steering procedures, max. curvature = 1 m\u003csup\u003e-1\u003c/sup\u003e, max. sharpness = 1 m\u003csup\u003e-2\u003c/sup\u003e.). The first row shows a comparison of the path length, and the second row compares the number of curvature discontinuities in the computed path:\n\n![Statistics](doc/images/statistics.png)\n\n\n### Computation Times\nThe following table shows the current computation times of the implemented steering functions, which are obtained from 10\u003csup\u003e5\u003c/sup\u003e random steering procedures on a single core of an Intel Xeon E5@3.50 GHz, 10 MB cache:\n\nSteering Function           | mean [µs] | std [µs]\n:---                        | :---      | :---\nDubins                      | 1.3       | ±0.8\nCC\u003csup\u003e±±\u003c/sup\u003e-Dubins      | 7.1       | ±3.1\nCC\u003csup\u003e±0\u003c/sup\u003e-Dubins      | 5.6       | ±1.5\nCC\u003csup\u003e0±\u003c/sup\u003e-Dubins      | 5.6       | ±1.6\nCC\u003csup\u003e00\u003c/sup\u003e-Dubins      | 4.9       | ±1.5\nCC-Dubins                   | 14.8      | ±4.3\n---                         | ---       | ---\nReeds-Shepp                 | 7.4       | ±1.7\nHC\u003csup\u003e±±\u003c/sup\u003e-Reeds-Shepp | 58.1      | ±10.1\nHC\u003csup\u003e±0\u003c/sup\u003e-Reeds-Shepp | 56.4      | ±8.8\nHC\u003csup\u003e0±\u003c/sup\u003e-Reeds-Shepp | 51.0      | ±10.2\nHC\u003csup\u003e00\u003c/sup\u003e-Reeds-Shepp | 55.9      | ±9.5\nHC-Reeds-Shepp              | 464.3     | ±72.8\nCC\u003csup\u003e00\u003c/sup\u003e-Reeds-Shepp | 53.8      | ±8.4\n\n\n### Interfacing with OMPL\nIn 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](http://ompl.kavrakilab.org/2012/03/18/geometric-planning-for-car-like-vehicles.html). OMPL requires a distance and an interpolate function, which are provided in this package.\n\n\n## Bugs \u0026 Feature Requests\nPlease use the [Issue Tracker](https://github.com/hbanzhaf/steering_functions/issues) to report bugs or request features.\n\n[ROS 1]: http://www.ros.org\n[ROS 2]: https://docs.ros.org/en/rolling/\n[RViz]: http://wiki.ros.org/rviz\n[OMPL]: http://ompl.kavrakilab.org/\n[Eigen]: http://eigen.tuxfamily.org/\n","funding_links":[],"categories":["6. Planning","Planning and Control"],"sub_categories":["3.4 High Performance Inference","Vector Map"],"project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fhbanzhaf%2Fsteering_functions","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fhbanzhaf%2Fsteering_functions","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fhbanzhaf%2Fsteering_functions/lists"}