├── _config.yml ├── lib └── Injection │ ├── CMakeLists.txt │ ├── src │ └── InjectionInterface.cpp │ └── include │ └── injection │ ├── InjectionInterface.h │ └── Injection.h ├── .github ├── ISSUE_TEMPLATE │ ├── simple-improvement-proposal.md │ └── bug_report.md └── workflows │ ├── build.yml │ └── doxy.yml ├── README.md ├── src ├── CMakeLists.txt ├── Filter.h ├── AgentModel.h ├── DistanceTimeInterval.h ├── StopHorizon.h ├── VelocityHorizon.h ├── model_collection.cpp ├── model_collection.h ├── README.md ├── AgentModelInjection.cpp ├── AgentModelInjection.h ├── Interface.h └── AgentModel.cpp ├── LICENSE ├── CMakeLists.txt └── resources └── doc └── img └── horizonSketch.svg /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-cayman -------------------------------------------------------------------------------- /lib/Injection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # create executable 2 | add_library(injection 3 | src/InjectionInterface.cpp 4 | ) 5 | 6 | # include directories 7 | target_include_directories(injection PRIVATE 8 | include/injection 9 | ) -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/simple-improvement-proposal.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Simple improvement proposal 3 | about: Just a very basic template 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | ### Why 11 | 12 | ### How 13 | 14 | ### Remarks 15 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build agent_model 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | build_agentmodel: 11 | name: Build agentmodel.lib 12 | runs-on: ubuntu-18.04 13 | 14 | steps: 15 | - uses: actions/checkout@v2 16 | - name: cd to repo 17 | run: echo $GITHUB_WORKSPACE && cd $GITHUB_WORKSPACE 18 | - name: setup cmake 19 | run: mkdir build_lib && cd build_lib && cmake -G "Unix Makefiles" -DBUILD_WITH_INJECTION=ON -DCREATE_DOXYGEN_TARGET=OFF -DCMAKE_BUILD_TYPE=Debug .. 20 | - name: compile agent model 21 | run: cd build_lib && echo $(ls) && make agent_model 22 | 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![SimDriver CI](https://github.com/ika-rwth-aachen/SimDriver/workflows/SimDriver%20CI/badge.svg?branch=master) ![Deploy Doxygen](https://github.com/ika-rwth-aachen/SimDriver/workflows/Deploy%20Doxygen/badge.svg) 2 | 3 | # SimDriver 4 | 5 | A responsive driver model for traffic simulations to create exact and closed-loop microscopic traffic scenarios. 6 | 7 | A detailed description of the motivation, concept and architecture can be found [here](https://publications.rwth-aachen.de/record/816233/files/816233.pdf). 8 | 9 | ## Documentation 10 | 11 | * Capabilities are described [here](src/README.md) 12 | * A detailed API documentation can be found [here](https://ika-rwth-aachen.github.io/SimDriver/). 13 | 14 | ## Contact 15 | 16 | Main contact: Daniel Becker, daniel.becker@ika.rwth-aachen.de 17 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # if build with injection option is on, then add injection sources 2 | if (BUILD_WITH_INJECTION) 3 | 4 | # set headers and sources 5 | set(INJECTION_SRC AgentModelInjection.cpp) 6 | set(INJECTION_HDR "\;AgentModelInjection.h") 7 | 8 | else () 9 | 10 | # set headers and sources 11 | set(INJECTION_SRC) 12 | set(INJECTION_HDR) 13 | 14 | endif (BUILD_WITH_INJECTION) 15 | 16 | 17 | # define target 18 | add_library(agent_model STATIC 19 | AgentModel.cpp 20 | model_collection.cpp 21 | ${INJECTION_SRC}) 22 | 23 | 24 | if (BUILD_WITH_INJECTION) 25 | 26 | target_link_libraries(agent_model PRIVATE 27 | injection 28 | ) 29 | 30 | target_include_directories(agent_model PUBLIC 31 | ${PROJECT_SOURCE_DIR}/lib/Injection/include 32 | ) 33 | 34 | endif (BUILD_WITH_INJECTION) -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Desktop (please complete the following information):** 27 | - OS: [e.g. iOS] 28 | - Browser [e.g. chrome, safari] 29 | - Version [e.g. 22] 30 | 31 | **Smartphone (please complete the following information):** 32 | - Device: [e.g. iPhone6] 33 | - OS: [e.g. iOS8.1] 34 | - Browser [e.g. stock browser, safari] 35 | - Version [e.g. 22] 36 | 37 | **Additional context** 38 | Add any other context about the problem here. 39 | -------------------------------------------------------------------------------- /.github/workflows/doxy.yml: -------------------------------------------------------------------------------- 1 | name: Deploy Doxygen 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | gen_doxygen: 11 | name: generate doxygen for gh-pages 12 | runs-on: ubuntu-18.04 13 | 14 | steps: 15 | - uses: actions/checkout@v2 16 | with: 17 | persist-credentials: false 18 | - name: cd to repo 19 | run: echo $GITHUB_WORKSPACE && cd $GITHUB_WORKSPACE 20 | - name: install doxygen 21 | run: sudo apt-get install -y doxygen graphviz 22 | - name: setup cmake 23 | run: mkdir build_lib && cd build_lib && cmake -G "Unix Makefiles" -DBUILD_WITH_INJECTION=ON -DCREATE_DOXYGEN_TARGET=ON -DCMAKE_BUILD_TYPE=Debug .. 24 | - name: compile doumentation 25 | run: cd build_lib && echo $(make help) && make doxygen 26 | - name: Deploy 27 | uses: JamesIves/github-pages-deploy-action@releases/v3 28 | with: 29 | ACCESS_TOKEN: ${{ secrets.ACCESS_TOKEN }} 30 | BRANCH: gh-pages # The branch the action should deploy to. 31 | FOLDER: docs/html # The folder the action should deploy. 32 | 33 | 34 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019-2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | # the project 4 | project(SimDriver VERSION 0.1) 5 | set(CMAKE_CXX_STANDARD 14) 6 | 7 | # options 8 | option(CREATE_DOXYGEN_TARGET "Creates the doxygen documentation if set." OFF) 9 | option(BUILD_WITH_INJECTION "Building the agent model with injection functionality." OFF) 10 | 11 | 12 | # documentation 13 | if (CREATE_DOXYGEN_TARGET) 14 | 15 | # message 16 | message("-- Generation of doxygen target enabled") 17 | 18 | # Require dot, treat the other components as optional 19 | find_package(Doxygen 20 | REQUIRED dot 21 | OPTIONAL_COMPONENTS mscgen dia) 22 | 23 | if (DOXYGEN_FOUND) 24 | 25 | # create doc directory 26 | file(MAKE_DIRECTORY ${PROJECT_SOURCE_DIR}/docs) 27 | 28 | # settings 29 | set(DOXYGEN_GENERATE_HTML YES) 30 | set(DOXYGEN_GENERATE_MAN YES) 31 | set(DOXYGEN_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/docs) 32 | set(DOXYGEN_EXCLUDE_PATTERNS AgentModelInjection.*) 33 | set(DOXYGEN_USE_MDFILE_AS_MAINPAGE README.md) 34 | 35 | # create target 36 | doxygen_add_docs( 37 | doxygen 38 | ${PROJECT_SOURCE_DIR}/src README.md 39 | COMMENT "Generate man pages" 40 | ) 41 | 42 | endif (DOXYGEN_FOUND) 43 | 44 | # unset doxygen target 45 | set(CREATE_DOXYGEN_TARGET OFF CACHE BOOL "disabled doxygen target for submodules" FORCE) 46 | 47 | endif (CREATE_DOXYGEN_TARGET) 48 | 49 | 50 | # injection 51 | if(BUILD_WITH_INJECTION) 52 | 53 | # add injection sources 54 | add_subdirectory(lib/Injection) 55 | 56 | # add definition 57 | add_definitions(-DWITH_INJECTION=true) 58 | 59 | endif(BUILD_WITH_INJECTION) 60 | 61 | 62 | # library code 63 | if(UNIX) 64 | set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC" ) 65 | set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC" ) 66 | endif() 67 | add_subdirectory(src/) 68 | -------------------------------------------------------------------------------- /lib/Injection/src/InjectionInterface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Jens Klimke 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated 5 | * documentation files (the "Software"), to deal in the Software without restriction, including without limitation the 6 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to 7 | * permit persons to whom the Software is furnished to do so, subject to the following conditions: 8 | * 9 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the 10 | * Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 13 | * WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 14 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 15 | * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 16 | * 17 | * author: Jens Klimke 18 | * date: 2020-02-16 19 | */ 20 | 21 | #include "InjectionInterface.h" 22 | 23 | std::map> InjectionInterface::_index{}; 24 | 25 | 26 | void InjectionInterface::applyAll(const void *owner) { 27 | 28 | // iterate over elements and apply 29 | for (auto &e : _index.at(owner)) 30 | e->apply(); 31 | 32 | } 33 | 34 | 35 | void InjectionInterface::resetAll(const void *owner) { 36 | 37 | // iterate over elements and apply 38 | for (auto e : _index.at(owner)) 39 | e->reset(); 40 | 41 | } 42 | 43 | 44 | void InjectionInterface::registerInjection(const void *owner) { 45 | 46 | // create owner index 47 | if (_index.find(owner) == _index.end()) 48 | _index[owner] = std::vector(); 49 | 50 | // add this 51 | _index.at(owner).push_back(this); 52 | 53 | } 54 | 55 | 56 | void InjectionInterface::remove(const void *owner) { 57 | 58 | // delete owner 59 | if (_index.find(owner) != _index.end()) 60 | _index.erase(owner); 61 | 62 | } -------------------------------------------------------------------------------- /lib/Injection/include/injection/InjectionInterface.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2020-02-12. 22 | // Contributors: 23 | // 24 | // InjectionInterface.h 25 | 26 | #ifndef DATA_GENERATOR_REGISTRATION_INTERFACE_H 27 | #define DATA_GENERATOR_REGISTRATION_INTERFACE_H 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | /** 34 | * The InjectionInterface implements a injection for an injection object with a associated owner, handles the 35 | * index and provides a method to apply all values of a given owner. 36 | */ 37 | struct InjectionInterface { 38 | 39 | public: 40 | 41 | static std::map> _index; //!< The index of all injection owners 42 | 43 | /** 44 | * Runs the apply() method of all elements associated with the given owner 45 | * @param owner Owner of the element 46 | */ 47 | static void applyAll(const void *owner); 48 | 49 | 50 | /** 51 | * Runs the reset() method of all elements associated with the given owner 52 | * @param owner Owner of the element 53 | */ 54 | static void resetAll(const void *owner); 55 | 56 | 57 | /** 58 | * Removes the object associated with the given owner 59 | * @param owner Owner to be removed 60 | */ 61 | static void remove(const void *owner); 62 | 63 | 64 | protected: 65 | 66 | 67 | /** 68 | * Resets the injection 69 | */ 70 | virtual void reset() = 0; 71 | 72 | 73 | /** 74 | * Applies the actual value to the base value 75 | */ 76 | virtual void apply() = 0; 77 | 78 | 79 | /** 80 | * Registers an object in combination with the owner 81 | * @param owner Owner of the object 82 | */ 83 | void registerInjection(const void *owner); 84 | 85 | }; 86 | 87 | 88 | #endif // DATA_GENERATOR_REGISTRATION_INTERFACE_H -------------------------------------------------------------------------------- /lib/Injection/include/injection/Injection.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2020-02-12. 22 | // Contributors: 23 | // 24 | // Injection.h 25 | 26 | 27 | #ifndef DATA_GENERATOR_REGISTRATION_H 28 | #define DATA_GENERATOR_REGISTRATION_H 29 | 30 | #include 31 | #include "InjectionInterface.h" 32 | 33 | /** 34 | * @brief Injection class used to wrap a regular value and enable the injection of the value 35 | * @tparam T The datatype of wrapped value 36 | */ 37 | template 38 | class Injection : public InjectionInterface { 39 | 40 | public: 41 | 42 | T *_ptr; //!< The calculated actual value 43 | std::unique_ptr _inj; //!< The injected value 44 | 45 | 46 | /** 47 | * Default constructor 48 | */ 49 | Injection() = default; 50 | 51 | 52 | /** 53 | * Copy constructor 54 | * @param v The value to be injected 55 | */ 56 | explicit Injection(const T &v) { 57 | 58 | this->operator=(v); 59 | 60 | } 61 | 62 | 63 | /** 64 | * Default destructor 65 | */ 66 | virtual ~Injection() = default; 67 | 68 | 69 | /** 70 | * Creates the injection 71 | * @param pointer Pointer to the base value 72 | * @param owner Owner of the value 73 | */ 74 | void registerValue(T *pointer, const void *owner) { 75 | 76 | _ptr = pointer; 77 | registerInjection(owner); 78 | 79 | } 80 | 81 | 82 | /** 83 | * Converts the object to the actual value 84 | * @return The actual value 85 | */ 86 | explicit operator T() const { 87 | return *_ptr; 88 | } 89 | 90 | 91 | /** 92 | * Returns the point to the actual value 93 | * @return Pointer to the actual value 94 | */ 95 | T *ptr() { 96 | return _ptr; 97 | } 98 | 99 | 100 | /** 101 | * Sets the actual value 102 | * @param value The value to be set 103 | */ 104 | Injection &operator=(const T &value) { 105 | _inj.reset(new T(value)); 106 | return *this; 107 | } 108 | 109 | 110 | protected: 111 | 112 | 113 | void apply() override { 114 | 115 | *_ptr = _inj ? *_inj : *_ptr; 116 | 117 | } 118 | 119 | 120 | void reset() override { 121 | 122 | _inj.reset(nullptr); 123 | 124 | } 125 | 126 | 127 | }; 128 | 129 | #endif // DATA_GENERATOR_REGISTRATION_H -------------------------------------------------------------------------------- /src/Filter.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2020-04-04 22 | // Contributors: 23 | // 24 | 25 | 26 | #ifndef SIMDRIVER_FILTER_H 27 | #define SIMDRIVER_FILTER_H 28 | 29 | #include 30 | #include 31 | 32 | namespace agent_model { 33 | 34 | 35 | /** 36 | * @brief A class to implement a mean filter 37 | */ 38 | class Filter { 39 | 40 | protected: 41 | 42 | unsigned int n; //!< Number of elements 43 | unsigned int i; //!< Current element's index (circular buffer) 44 | 45 | std::vector _elements; //!< Element container 46 | 47 | public: 48 | 49 | /** 50 | * Initializes the filter with its length 51 | * The length is equal to the number of elements of which the mean value is calculated 52 | * @param length Length of the filter 53 | */ 54 | void init(unsigned int length) { 55 | 56 | // set length and index 57 | n = length; 58 | i = 0; 59 | 60 | // create vector 61 | if (_elements.size() != n) { 62 | _elements.clear(); 63 | _elements.reserve(n); 64 | } 65 | 66 | } 67 | 68 | 69 | /** 70 | * Returns the filtered mean value of the elements 71 | * @return Filtered mean value 72 | */ 73 | double value() { 74 | 75 | // special case 76 | if(_elements.empty()) 77 | return 0.0; 78 | 79 | // sum up 80 | auto sum = 0.0; 81 | for (auto &e : _elements) 82 | sum += e; 83 | 84 | // return average value 85 | return sum / (double) _elements.size(); 86 | 87 | } 88 | 89 | 90 | /** 91 | * Adds the current value and returns the actual filtered value 92 | * @param v Value to be added 93 | * @return The mean value 94 | */ 95 | double value(double v) { 96 | 97 | // add element 98 | if(_elements.size() < n) 99 | _elements.emplace_back(v); 100 | else 101 | _elements.at(i) = v; 102 | 103 | // increment i 104 | i = (i + 1) % n; 105 | 106 | // return mean value 107 | return value(); 108 | 109 | } 110 | 111 | }; 112 | 113 | 114 | } 115 | 116 | 117 | #endif // SIMDRIVER_FILTER_H 118 | -------------------------------------------------------------------------------- /src/AgentModel.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2019-03-23. 22 | // Contributors: 23 | // 24 | // AgentModel.h 25 | 26 | #ifndef AGENT_MODEL_H 27 | #define AGENT_MODEL_H 28 | 29 | #include "Interface.h" 30 | #include "VelocityHorizon.h" 31 | #include "StopHorizon.h" 32 | #include "Filter.h" 33 | #include "DistanceTimeInterval.h" 34 | 35 | 36 | /** 37 | * @brief The agent model main class 38 | */ 39 | class AgentModel : public agent_model::Interface { 40 | 41 | 42 | protected: 43 | 44 | agent_model::StopHorizon _stop_horizon{}; //!< attribute to store the stop points 45 | agent_model::VelocityHorizon _vel_horizon{}; //!< attribute to store the stop points 46 | agent_model::Filter _filter{}; //!< attribute to store the speed reaction filter 47 | agent_model::DistanceTimeInterval _lateral_offset_interval; //!< attribute to store the lateral offset interval 48 | agent_model::DistanceTimeInterval _lane_change_process_interval; //!< attribute to store the lane change interval 49 | 50 | 51 | public: 52 | 53 | 54 | /** 55 | * Default constructor 56 | */ 57 | AgentModel() = default; 58 | 59 | 60 | /** 61 | * Default destructor 62 | */ 63 | ~AgentModel() override = default; 64 | 65 | 66 | /** 67 | * Initializes the driver model. Shall be ran before the the first step is executed. 68 | */ 69 | void init(); 70 | 71 | 72 | /** 73 | * Performs a driver model step. 74 | * The driver model must be initializes (@see init()). 75 | * @param simulationTime The current simulation time 76 | */ 77 | void step(double simulationTime); 78 | 79 | 80 | protected: 81 | 82 | /** 83 | * Calculates process of stopping and starting 84 | */ 85 | void decisionProcessStop(); 86 | 87 | 88 | /** 89 | * Calculates the decision to perform a lane change 90 | */ 91 | void decisionLaneChange(); 92 | 93 | 94 | /** 95 | * Calculates the decision to perform a lateral offset 96 | */ 97 | void decisionLateralOffset(); 98 | 99 | 100 | /** 101 | * Calculates the target speed based on rules, the curvature of the track 102 | */ 103 | void consciousVelocity(); 104 | 105 | 106 | /** 107 | * Calculate the process of the stop maneuver 108 | */ 109 | void consciousStop(); 110 | 111 | 112 | /** 113 | * Calculates the net distance to the relevant following traffic participants 114 | */ 115 | void consciousFollow(); 116 | 117 | /** 118 | * Calculates the lane change process 119 | */ 120 | void consciousLaneChange(); 121 | 122 | 123 | /** 124 | * Calculates the offset to be added to the reference points 125 | */ 126 | void consciousLateralOffset(); 127 | 128 | 129 | /** 130 | * Calculates the reference points for the lateral motion control 131 | */ 132 | void consciousReferencePoints(); 133 | 134 | 135 | /** 136 | * Calculates the reaction for the lateral motion control based on the reference points 137 | * @return The reaction value for lateral motion control 138 | */ 139 | double subconsciousLateralControl(); 140 | 141 | 142 | /** 143 | * Calculates the reaction to follow other traffic participants 144 | * @return The reaction value to follow 145 | */ 146 | double subconsciousFollow(); 147 | 148 | 149 | /** 150 | * Calculates the reaction to stop the vehicle at the desired point 151 | * @return The reaction value to stop 152 | */ 153 | double subconsciousStop(); 154 | 155 | 156 | /** 157 | * Calculates the reaction to reach the desired speed, including predictive control 158 | * @return The reaction value to control speed 159 | */ 160 | double subconsciousSpeed(); 161 | 162 | 163 | /** 164 | * Calculates the pedal behavior when starting or stopping for sub-microscopic simulations 165 | * @return The pedal value 166 | */ 167 | double subconsciousStartStop(); 168 | 169 | 170 | }; 171 | 172 | 173 | #endif // AGENT_MODEL_H 174 | -------------------------------------------------------------------------------- /src/DistanceTimeInterval.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 07.04.20. 22 | // Contributors: 23 | // 24 | 25 | 26 | #ifndef SIMDRIVER_DISTANCETIMEINTERVAL_H 27 | #define SIMDRIVER_DISTANCETIMEINTERVAL_H 28 | 29 | #include "model_collection.h" 30 | #include 31 | 32 | 33 | namespace agent_model { 34 | 35 | class DistanceTimeInterval { 36 | 37 | protected: 38 | 39 | double _actualPosition = 0.0; //!< The actual position 40 | double _actualTime = 0.0; //!< The actual time 41 | 42 | double _startTime = INFINITY; //!< The start time 43 | double _endTime = INFINITY; //!< The end time 44 | 45 | double _startPosition = INFINITY; //!< The start position 46 | double _endPosition = INFINITY; //!< The end position 47 | 48 | double _scale = 1.0; //!< The factor to be scaled 49 | double _delta = 1.0; //!< The power to calculate the scale 50 | 51 | 52 | public: 53 | 54 | 55 | /** 56 | * Sets the power of the scale 57 | * @param delta Power of the scale 58 | */ 59 | void setDelta(double delta) { 60 | 61 | _delta = delta; 62 | 63 | } 64 | 65 | 66 | /** 67 | * Returns a flag whether the interval is set or not 68 | * @return Flag 69 | */ 70 | bool isSet() const { 71 | 72 | return !std::isinf(_startTime) || !std::isinf(_startPosition); 73 | 74 | } 75 | 76 | 77 | /** 78 | * Sets the scale for the factor 79 | * @param scale Scale 80 | */ 81 | void setScale(double scale) { 82 | 83 | _scale = scale; 84 | 85 | } 86 | 87 | 88 | /** 89 | * Returns the scale parameter 90 | * @return The scale parameter 91 | */ 92 | double getScale() const { 93 | 94 | return _scale; 95 | 96 | } 97 | 98 | 99 | /** 100 | * Update the actual state 101 | * @param position Actual position 102 | * @param time Actual time 103 | */ 104 | void update(double position, double time) { 105 | 106 | _actualPosition = position; 107 | _actualTime = time; 108 | 109 | } 110 | 111 | 112 | /** 113 | * Sets the desired time interval 114 | * @param timeInterval Time interval 115 | */ 116 | void setTimeInterval(double timeInterval) { 117 | 118 | if(std::isinf(timeInterval)) { 119 | 120 | // set start and end time to inf 121 | _startTime = INFINITY; 122 | _endTime = INFINITY; 123 | 124 | return; 125 | 126 | } 127 | 128 | _startTime = _actualTime; 129 | _endTime = _actualTime + timeInterval; 130 | 131 | } 132 | 133 | 134 | /** 135 | * Sets the desired end position of the interval 136 | * @param endPosition End position of the interval 137 | */ 138 | void setEndPosition(double endPosition) { 139 | 140 | if(std::isinf(endPosition)) { 141 | 142 | // set start and end time to inf 143 | _startPosition = INFINITY; 144 | _endPosition = INFINITY; 145 | 146 | return; 147 | 148 | } 149 | 150 | _startPosition = _actualPosition; 151 | _endPosition = endPosition; 152 | 153 | } 154 | 155 | 156 | /** 157 | * Resets the set interval 158 | * Does not reset the actual position and time 159 | */ 160 | void reset() { 161 | 162 | setTimeInterval(INFINITY); 163 | setEndPosition(INFINITY); 164 | 165 | } 166 | 167 | 168 | /** 169 | * Returns the normalized factor 170 | * @return The normalized factor 171 | */ 172 | double getFactor() const { 173 | 174 | // if not set, return 0 175 | if(!isSet()) 176 | return 0.0; 177 | 178 | // calculate factors 179 | // double ft = std::isinf(_startTime) ? 0.0 : agent_model::scale(_actualTime, _endTime, _startTime, 0.5); 180 | // double fs = std::isinf(_startPosition) ? 0.0 : agent_model::scale(_actualPosition, _endPosition, _startPosition, 0.5); 181 | 182 | double ft = std::isinf(_startTime) ? 0.0 : agent_model::scale(_actualTime, _endTime, _startTime, _delta); 183 | double fs = std::isinf(_startPosition) ? 0.0 : agent_model::scale(_actualPosition, _endPosition, _startPosition, _delta); 184 | 185 | // maximum 186 | return (std::max)(ft, fs); 187 | 188 | } 189 | 190 | 191 | /** 192 | * Returns the scaled factor of the interval 193 | * @return The scaled factor 194 | */ 195 | double getScaledFactor() const { 196 | 197 | return getFactor() * _scale; 198 | 199 | } 200 | 201 | }; 202 | 203 | } // namespace 204 | 205 | #endif //SIMDRIVER_DISTANCETIMEINTERVAL_H 206 | -------------------------------------------------------------------------------- /src/StopHorizon.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2020-04-06. 22 | // Contributors: 23 | // 24 | 25 | 26 | #ifndef SIMDRIVER_STOPHORIZON_H 27 | #define SIMDRIVER_STOPHORIZON_H 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #ifndef EPS_TIME 34 | #define EPS_TIME 1e-6 35 | #endif 36 | 37 | #ifndef EPS_DISTANCE 38 | #define EPS_DISTANCE 1e-9 39 | #endif 40 | 41 | namespace agent_model { 42 | 43 | 44 | class StopHorizon { 45 | 46 | protected: 47 | 48 | constexpr static const double DELETE_AFTER_DISTANCE = 10.0; //!< Distance after which the stop point is deleted from the list 49 | 50 | struct _StopPoint { 51 | double s = INFINITY; 52 | double sStart = INFINITY; 53 | double timeStartStanding = INFINITY; 54 | double standingTime = INFINITY; 55 | bool passed = false; 56 | }; 57 | 58 | double _sActual = 0.0; 59 | std::map _elements{}; 60 | 61 | 62 | public: 63 | 64 | /** @brief a struct to store a stop point */ 65 | struct StopPoint { 66 | unsigned long id; 67 | double ds; 68 | double interval; 69 | }; 70 | 71 | 72 | /** 73 | * Inits the stop horizon 74 | * @param s Initial distance 75 | */ 76 | void init(double s) { 77 | 78 | _sActual = s; 79 | if (_elements.size() > 0) _elements.clear(); 80 | 81 | } 82 | 83 | 84 | /** 85 | * Adds a stop point to the list if it doesn't exist already 86 | * @param id ID of the stop 87 | * @param sStop Absolute position of the stop 88 | * @param standingTime The time the vehicle shall stand at the given stop (inf: until reset) 89 | * @return Flag to indicate if the stop point was added 90 | */ 91 | bool addStopPoint(unsigned long id, double sStop, double standingTime) { 92 | 93 | if(_elements.find(id) != _elements.end() && standingTime == 0) { 94 | _elements[id].passed = true; 95 | return true; 96 | } 97 | // only add if not already added 98 | if(_elements.find(id) != _elements.end()) 99 | if (fabs(_elements[id].s - sStop) < 0.5) 100 | return false; 101 | 102 | // only add when distance is large enough 103 | if(_sActual - sStop >= DELETE_AFTER_DISTANCE - EPS_DISTANCE) 104 | return false; 105 | 106 | // add to list 107 | _elements[id] = {sStop, _sActual, INFINITY, standingTime, false}; 108 | 109 | return true; 110 | 111 | } 112 | 113 | 114 | 115 | /** 116 | * Marks the given stop as stopped 117 | * @param id ID of the stop 118 | * @param actualTime The actual simulation time 119 | * @return Returns a flag whether the time was set or not 120 | */ 121 | bool stopped(unsigned long id, double actualTime) { 122 | 123 | // only set start time if not set before 124 | if(std::isinf(_elements.at(id).timeStartStanding)) { 125 | 126 | // set start time to actual time 127 | _elements.at(id).timeStartStanding = actualTime; 128 | 129 | // return success 130 | return true; 131 | 132 | } 133 | 134 | // not set 135 | return false; 136 | 137 | } 138 | 139 | 140 | /** 141 | * Updates the actual position 142 | * @param actualPosition Actual position 143 | * @param actualTime Actual simulation time 144 | */ 145 | void update(double actualPosition, double actualTime) { 146 | 147 | _sActual = actualPosition; 148 | 149 | // iterate over elements 150 | for(auto &ke : _elements) { 151 | 152 | // get element 153 | auto &e = ke.second; 154 | 155 | // ignore passed stops 156 | if(e.passed) 157 | continue; 158 | 159 | // set passed 160 | if(actualTime - e.timeStartStanding >= e.standingTime - EPS_TIME) 161 | e.passed = true; 162 | 163 | } 164 | 165 | // clean up 166 | auto it = _elements.cbegin(); 167 | while(it != _elements.cend()) { 168 | 169 | // delete elements after passed and distance large 170 | if(it->second.passed && _sActual - it->second.s >= DELETE_AFTER_DISTANCE - EPS_DISTANCE) 171 | it = _elements.erase(it); 172 | else 173 | it++; 174 | 175 | } 176 | 177 | 178 | } 179 | 180 | 181 | /** 182 | * Returns the next stop point 183 | * @return 184 | */ 185 | StopPoint getNextStop() { 186 | 187 | // init 188 | double dsMin = INFINITY; 189 | double interval = INFINITY; 190 | unsigned long id = (std::numeric_limits::max)(); 191 | 192 | // iterate over elements 193 | for(auto &ke : _elements) { 194 | 195 | // get element 196 | auto &e = ke.second; 197 | 198 | // save distance 199 | double ds = e.s - _sActual; 200 | 201 | // ignore 202 | if(e.passed || ds > dsMin) 203 | continue; 204 | 205 | // save data 206 | dsMin = ds; 207 | id = ke.first; 208 | interval = e.s - e.sStart; 209 | 210 | } 211 | 212 | // return; 213 | return StopPoint{id, dsMin, interval}; 214 | 215 | } 216 | 217 | 218 | }; 219 | 220 | 221 | } 222 | 223 | #endif // SIMDRIVER_STOPHORIZON_H 224 | -------------------------------------------------------------------------------- /src/VelocityHorizon.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2020-04-02. 22 | // Contributors: 23 | // 24 | // VelocityHorizon.h 25 | 26 | 27 | #ifndef SIMDRIVER_VELOCITYHORIZON_H 28 | #define SIMDRIVER_VELOCITYHORIZON_H 29 | 30 | #include 31 | #include 32 | #include 33 | #include "model_collection.h" 34 | 35 | 36 | namespace agent_model { 37 | 38 | 39 | /** @brief A class to store the internal horizon */ 40 | class VelocityHorizon { 41 | 42 | protected: 43 | 44 | /** @brief A class store a prediction point */ 45 | struct PredictionPoint { 46 | size_t i; //!< Reference index of the point 47 | double s; //!< The longitudinal reference position of the point 48 | double ds; //!< The actual distance to the point 49 | double vRule; //!< The planned velocity at the point 50 | double vCont; //!< The continuous velocity (e.g. curve speed) 51 | double sCont; //!< The continuous measure point 52 | }; 53 | 54 | double _offset; 55 | double _vMax; 56 | 57 | std::deque _elements{}; 58 | 59 | 60 | 61 | public: 62 | 63 | 64 | /** 65 | * Initializes the points container 66 | * @param offset Position offset of the horizon 67 | * @param noOfElements Number of elements to be stored 68 | */ 69 | void init(double offset, unsigned int noOfElements) { 70 | 71 | // set offset 72 | _offset = std::floor(offset); 73 | 74 | // reset elements 75 | if (_elements.size() > 0) _elements.clear(); 76 | 77 | // create points 78 | for (size_t i = 0; i < noOfElements; ++i) 79 | _elements.emplace_back(newPoint(i)); 80 | 81 | } 82 | 83 | 84 | /** 85 | * @brief Updates the horizon to the new reference position 86 | * Removes all elements with a distance smaller than zero, except of the first one smaller than zero 87 | * @param s New reference position 88 | */ 89 | void update(double s) { 90 | 91 | size_t i0 = 0; // first element with positive distance 92 | 93 | for (auto &e : _elements) { 94 | 95 | // recalculate distance 96 | e.ds = e.s - s; 97 | 98 | // check distance and increment if <= 0 99 | if(e.ds <= 0.0) 100 | i0++; 101 | } 102 | 103 | // get reference index of the last element 104 | size_t ib = _elements.back().i; 105 | 106 | // remove old element 107 | for (size_t i = 0; i + 1 < i0; ++i) { 108 | 109 | // new index 110 | size_t i1 = ib + i + 1; 111 | 112 | // remove from front, add to the back 113 | _elements.pop_front(); 114 | _elements.emplace_back(newPoint(i1)); 115 | 116 | } 117 | 118 | } 119 | 120 | 121 | /** 122 | * Returns the index of the start point of the interval in which the position is 123 | * @param s Position to be searched 124 | * @return Index of the interval 125 | */ 126 | unsigned int getIndexBefore(double s) { 127 | 128 | double s0 = _elements.at(0).s; 129 | 130 | if(s <= s0) 131 | return 0.0; 132 | if(s >= _elements.back().s) 133 | return _elements.size() - 1; 134 | 135 | return (unsigned int) std::floor(s - s0); 136 | 137 | } 138 | 139 | 140 | /** 141 | * Returns the index of the end point of the interval in which the position is 142 | * @param s Position to be searched 143 | * @return Index of the interval 144 | */ 145 | unsigned int getIndexAfter(double s) { 146 | 147 | double s0 = _elements.at(0).s; 148 | 149 | if(s <= s0) 150 | return 0.0; 151 | if(s >= _elements.back().s) 152 | return _elements.size() - 1; 153 | 154 | return (unsigned int) std::ceil(s - s0); 155 | 156 | } 157 | 158 | 159 | /** 160 | * Updates the maximum total velocity 161 | * @param v Velocity to be set 162 | */ 163 | void setMaxVelocity(double v) { 164 | 165 | _vMax = v; 166 | 167 | } 168 | 169 | 170 | /** 171 | * Resets the set speed rules 172 | */ 173 | void resetSpeedRule() { 174 | 175 | for(auto &e : _elements) 176 | e.vRule = INFINITY; 177 | 178 | } 179 | 180 | 181 | /** 182 | * Updates the speed in the given interval if the speed is smaller than the already set speed 183 | * @param s0 Start of the interval 184 | * @param s1 End of the interval 185 | * @param v Velocity to be set 186 | */ 187 | void updateSpeedRuleInInterval(double s0, double s1, double v) { 188 | 189 | auto i0 = getIndexBefore(s0); 190 | auto i1 = getIndexAfter(s1); 191 | 192 | for (unsigned int i = i0; i <= i1; ++i) { 193 | 194 | // get element 195 | auto &e = _elements.at(i); 196 | 197 | // set speed if speed is smaller and point in interval 198 | if(e.vRule > v) 199 | e.vRule = v; 200 | 201 | } 202 | 203 | } 204 | 205 | 206 | /** 207 | * Updates the continuous velocity profile at the given point 208 | * @param s Point to be set 209 | * @param v Velocity to be set 210 | */ 211 | void updateContinuousPoint(double s, double v) { 212 | 213 | // get index before position 214 | auto i = getIndexAfter(s); 215 | auto &e = _elements.at(i); 216 | 217 | if(s > e.sCont) { 218 | 219 | e.sCont = s; 220 | e.vCont = v; 221 | 222 | } 223 | 224 | } 225 | 226 | 227 | 228 | /** 229 | * Calculates the mean speed within the given interval 230 | * @param s0 Start of the interval 231 | * @param s1 End of the interval 232 | * @param delta A factor shifting the influence over the interval 233 | * @return The mean value 234 | */ 235 | double mean(double s0, double s1, double delta = 1.0) { 236 | 237 | // instantiate 238 | double v = 0.0; 239 | double vMin = INFINITY; 240 | double j = 0; 241 | 242 | // get indexes 243 | auto i0 = getIndexBefore(s0); 244 | auto i1 = getIndexAfter(s1); 245 | 246 | for (unsigned int i = i0; i <= i1; ++i) { 247 | 248 | // get speed and s 249 | auto v0 = (std::min)(vMin, getSpeedAt(i)); 250 | auto s = _elements.at(i).s; 251 | 252 | // sum up with scaled factor 253 | auto f = agent_model::scale(s, s1, s0, delta); 254 | v += f * v0; 255 | 256 | // set minimum for future 257 | if(v0 < vMin) 258 | vMin = v0; 259 | 260 | // divisor 261 | j += f; 262 | 263 | } 264 | 265 | return v / (double) j; 266 | 267 | } 268 | 269 | 270 | 271 | protected: 272 | 273 | 274 | /** 275 | * Returns the minimum of the speed at the given index 276 | * @param i Index 277 | * @return Minimum speed 278 | */ 279 | double getSpeedAt(unsigned int i) { 280 | 281 | // get speed at index 282 | auto &e = _elements.at(i); 283 | return (std::min)((std::min)(e.vCont, e.vRule), _vMax); 284 | 285 | } 286 | 287 | 288 | /** 289 | * Creates a new point with the given index at the given position 290 | * @param i Index of the point 291 | * @return The new point 292 | */ 293 | PredictionPoint newPoint(size_t i) { 294 | 295 | double s = _offset + (double) i; 296 | return PredictionPoint{i, s, INFINITY, INFINITY, INFINITY, s - 1.0}; 297 | 298 | } 299 | 300 | 301 | }; 302 | 303 | 304 | } 305 | 306 | 307 | #endif //SIMDRIVER_VELOCITYHORIZON_H 308 | -------------------------------------------------------------------------------- /src/model_collection.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2019-03-24. 22 | // Contributors: 23 | // 24 | // model_collection.cpp 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | #include "model_collection.h" 31 | 32 | namespace agent_model { 33 | 34 | 35 | double IDMSpeedReaction(double v, double vTarget, double delta) { 36 | 37 | using namespace std; 38 | 39 | // v must not be negative or inf 40 | if (v < 0.0) 41 | throw invalid_argument("actual velocity must not be negative."); 42 | else if (isinf(v)) 43 | throw invalid_argument("actual velocity must be finite."); 44 | 45 | // vTarget must not be negative 46 | if (vTarget < 0) 47 | throw invalid_argument("target velocity must not be negative."); 48 | 49 | // special cases 50 | if (vTarget <= 0.0 || v >= 2 * vTarget) 51 | return 2.0; 52 | else if (isinf(vTarget)) 53 | return 0.0; 54 | 55 | // calculate result 56 | auto dv = vTarget - v; 57 | double r = pow(1.0 - std::abs(dv) / vTarget, delta); 58 | 59 | // switch for dv < 0 60 | return dv < 0.0 ? 2.0 - r : r; 61 | 62 | } 63 | 64 | 65 | double speedReaction(double v, double vTarget, double delta, const double *vStep, const double *dsStep, double TMax, 66 | double deltaP) { 67 | 68 | using namespace std; 69 | 70 | // calculate local reaction 71 | auto local = agent_model::IDMSpeedReaction(v, vTarget, delta); 72 | 73 | // max distance 74 | auto dsMax = v * TMax; 75 | 76 | // calculate factors 77 | double f0 = scale(dsStep[0], dsMax, 0.0, deltaP); 78 | double f1 = scale(dsStep[1], dsMax, 0.0, deltaP); 79 | 80 | // calculate reaction 81 | auto r0 = agent_model::IDMSpeedReaction(v, vStep[0], delta); 82 | auto r1 = agent_model::IDMSpeedReaction(v, vStep[1], delta); 83 | 84 | // calculate sum of reaction 85 | return f0 * f1 * local + (1.0 - f0) * r0 + (1.0 - f1) * r1; 86 | 87 | } 88 | 89 | 90 | double IDMFollowReaction(double ds, double vPre, double v, double T, double s0, double a, double b) { 91 | 92 | using namespace std; 93 | 94 | // v must not be negative or inf 95 | if (v < 0.0) 96 | throw invalid_argument("actual velocity must not be negative."); 97 | else if (isinf(v)) 98 | throw invalid_argument("actual velocity must be finite."); 99 | 100 | // vTarget must not be negative 101 | if (vPre < 0) 102 | throw invalid_argument("target velocity must not be negative."); 103 | 104 | // avoid division by inf 105 | if (isinf(ds)) 106 | return 0.0; 107 | 108 | // get rel. velocity and dsStar (IDM) 109 | auto dv = v - vPre; 110 | auto dsStar = s0 + v * T + 0.5 * dv * v / sqrt(a * -b); 111 | 112 | // avoid 0/0 113 | if (dsStar == 0.0 && ds == 0.0) 114 | return 1.0; 115 | 116 | // avoid negative distances 117 | if (ds <= 0.0) 118 | ds = 0.0; 119 | 120 | // return squared ratio 121 | return pow(dsStar / ds, 2.0); 122 | 123 | } 124 | 125 | 126 | double SalvucciAndGray(double x, double y, double dx, double dy, double P, double D, 127 | double &theta, double &dTheta) { 128 | 129 | using namespace std; 130 | 131 | // avoid problem with x=0, x=inf, ... 132 | if (isinf(x) || isinf(y) || y == 0) { 133 | theta = 0.0; 134 | dTheta = 0.0; 135 | return 0.0; 136 | } 137 | 138 | // calculate dTheta and Theta 139 | if (theta != 0) 140 | dTheta = theta - atan2(y, x); 141 | theta = atan2(y, x); 142 | //dTheta = (y * dx + x * dy) / (x * x + y * y); 143 | 144 | // calculate reaction 145 | return P * theta + D * dTheta; 146 | 147 | } 148 | 149 | 150 | double IDMOriginal(double v, double v0, double ds, double dv, double T, double s0, double ac, double bc) { 151 | 152 | using namespace std; 153 | 154 | // calculate acceleration 155 | auto s_star = s0 + v * T + (v * dv / (2.0 * sqrt(ac * bc))); 156 | auto acc = ac * (1.0 - pow(v / v0, 4) - pow(s_star / ds, 2)); 157 | 158 | // check for nan or inf 159 | if (isnan(acc) || isinf(acc)) 160 | acc = 0.0; 161 | 162 | return acc; 163 | 164 | } 165 | 166 | 167 | void MOBILOriginal(double &safety, double &incentive, double v, double v0, double T, double s0, double ac, 168 | double bc, double ds0f, double v0f, double ds1f, double v1f, double ds0b, double v0b, 169 | double ds1b, double v1b, double bSafe, double aThr, double p) { 170 | 171 | auto a00m = IDMOriginal(v, v0, ds0f, v - v0f, T, s0, ac, bc); // acc(M) 172 | auto a11m = IDMOriginal(v, v0, ds1f, v - v1f, T, s0, ac, bc); // acc'(M') 173 | auto a00b = IDMOriginal(v, v0, -ds0b, v0b - v, T, s0, ac, bc); // acc(B) 174 | auto a01b = IDMOriginal(v, v0, ds1f - ds1b, v1b - v1f, T, s0, ac, bc); // acc(B') 175 | auto a10b = IDMOriginal(v, v0, ds0f - ds0b, v0b - v0f, T, s0, ac, bc); // acc'(B) 176 | auto a11b = IDMOriginal(v, v0, -ds1b, v1f - v, T, s0, ac, bc); // acc'(B') 177 | 178 | /* 179 | * Original criteria: 180 | * a11b > -bSave, // safety criterion 181 | * a11m - a00m > p * (a00b + a01b - a10b - a11b) + aThr // incentive criterion 182 | */ 183 | 184 | // save safety criterion 185 | safety = (a11b + bSafe) / bSafe; 186 | 187 | // return incentive criterion 188 | incentive = (a11m - a00m - p * (a00b + a01b - a10b - a11b) - aThr) / aThr; 189 | 190 | } 191 | 192 | 193 | double interpolate(double xx, const double *x, const double *y, unsigned int n, int extrapMode) { 194 | 195 | using namespace std; 196 | 197 | // instantiate 198 | size_t i1 = 0; 199 | size_t i0 = n; // last finite value 200 | 201 | for (i1 = 0; i1 < n; ++i1) { 202 | 203 | // ignore inf values 204 | if (isinf(x[i1])) 205 | continue; 206 | else 207 | i0 = i1; 208 | 209 | // point before current sample point 210 | if (x[i1] > xx) 211 | break; 212 | 213 | } 214 | 215 | // reset behind last valid value 216 | if (i0 != n && (i1 == n || isinf(x[i1]))) 217 | i1 = i0 + 1; 218 | 219 | bool e = i1 == n || isinf(x[i1]); 220 | bool s = i1 == 0 || isinf(x[i1 - 1]); 221 | 222 | // can not find any solution 223 | if (e && std::abs(x[n - 1] - xx) < 1e-15) { 224 | 225 | return y[n - 1]; 226 | 227 | } else if (s && std::abs(x[0] - xx) < 1e-15) { 228 | 229 | return y[0]; 230 | 231 | } else if (s) { 232 | 233 | if (extrapMode == 0) 234 | return -1.0 * INFINITY; 235 | else if (extrapMode == 1 && i1 != n) 236 | i1++; 237 | else if (extrapMode == 2) 238 | return y[i1]; 239 | 240 | } else if (e) { 241 | 242 | if (extrapMode == 0) 243 | return INFINITY; 244 | else if (extrapMode == 1) 245 | i1--; 246 | else if (extrapMode == 2) 247 | return y[i1 - 1]; 248 | 249 | } 250 | 251 | // check validity 252 | if (i1 == 0 || i1 == n || x[i1 - 1] >= x[i1]) 253 | throw std::invalid_argument("interpolation not possible."); 254 | 255 | 256 | // interpolate linearly 257 | i0 = i1 - 1; 258 | return y[i0] + (xx - x[i0]) * (y[i1] - y[i0]) / (x[i1] - x[i0]); 259 | 260 | } 261 | 262 | 263 | double scale(double x) { 264 | 265 | x = std::max(0.0, std::min(1.0, x)); 266 | return 3 * x * x - 2 * x * x * x; 267 | 268 | } 269 | 270 | 271 | double linScale(double x, double xMax, double xMin) { 272 | 273 | return std::max(0.0, std::min(1.0, (x - xMin) / (xMax - xMin))); 274 | 275 | } 276 | 277 | 278 | double scale(double x, double xMax, double xMin, double delta) { 279 | 280 | // limit delta 281 | delta = std::max(0.0, delta); 282 | 283 | // step at > 0.0 284 | if(delta == 0.0) 285 | return x <= xMin ? 0.0 : 1.0; 286 | 287 | // calculate scale 288 | auto s = scale(linScale(x, xMax, xMin)); 289 | 290 | if(delta < 1.0) 291 | return 1.0 - pow(1.0 - s, 1.0 / delta); // inverted power 292 | else 293 | return pow(s, delta); // normal power 294 | 295 | } 296 | 297 | 298 | double invScale(double x, double xMax, double xMin, double delta) { 299 | 300 | return pow(scale((xMax - x) / (xMax - xMin)), delta); 301 | 302 | } 303 | 304 | 305 | double scaleInf(double x, double xMax, double xMin, double delta) { 306 | 307 | return 1.0 / invScale(x, xMax, xMin, delta); 308 | 309 | } 310 | 311 | } -------------------------------------------------------------------------------- /src/model_collection.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2019-03-24. 22 | // Contributors: 23 | // 24 | // Credits (models): 25 | // [1] Treiber, Martin; Hennecke, Ansgar; Helbing, Dirk (2000), "Congested traffic states in empirical observations 26 | // and microscopic simulations", Physical Review E, 62 (2): 1805–1824, arXiv:cond-mat/0002177, 27 | // Bibcode:2000PhRvE..62.1805T, doi:10.1103/PhysRevE.62.1805 28 | // [2] Salvucci, D. D., & Gray, R. (2004). A Two-Point Visual Control Model of Steering. Perception, 33(10), 1233–1248. 29 | // https://doi.org/10.1068/p5343 30 | // [3] Kesting, Arne & Treiber, Martin & Helbing, Dirk. (2007), "General Lane-Changing Model MOBIL for Car-Following 31 | // Models", Transportation Research Record. 1999. 86-94. 10.3141/1999-10. 32 | // https://mtreiber.de/publications/MOBIL_TRB.pdf 33 | // 34 | // 35 | // model_collection.h 36 | 37 | #ifndef AGENT_MODEL_COLLECTION_H 38 | #define AGENT_MODEL_COLLECTION_H 39 | 40 | #include 41 | #include 42 | 43 | namespace agent_model { 44 | 45 | /** 46 | * The reaction based on the current speed and the desired speed. The model is defined in 47 | * the free part of the IDM model. [1] 48 | * 49 | * Conditions: 50 | * 1. The actual velocity v must not be negative. A negative velocity leads to an exception (invalid argument). 51 | * 2. A negative target velocity vTarget leads to a result of 2. 52 | * 3. If the actual velocity v is much larger than target velocity (v >= 2 * vTarget), the result is 2. 53 | * 4. If the target velocity vTarget is infinity the result is 0. 54 | * 55 | * 56 | * @param v Current velocity (in *m/s*) 57 | * @param vTarget The target (desired) velocity (in *m/s*) 58 | * @param delta The parameter \delta (in -) 59 | * @return Return the cruise scale-down factor 60 | */ 61 | double IDMSpeedReaction(double v, double vTarget, double delta); 62 | 63 | 64 | /** 65 | * Calculates the reaction on the current speed and the desired speed with respect to the oncoming and local 66 | * situation. The local situation is described by the parameter v0, which is the desired reference speed in case 67 | * of uninfluenced driving (the desired speed). The oncoming situation is described by 68 | * 69 | * @param v The actual velocity 70 | * @param vTarget The local target velocity 71 | * @param delta The delta parameter (@see IDMSpeedReaction) 72 | * @param dsStep The distance to the velocity step 73 | * @param vStep The reference velocity at the velocity step 74 | * @param TMax The maximum prediction time headway 75 | * @param deltaP The intensity parameter for the prediction 76 | * @return Returns the reaction value 77 | */ 78 | double speedReaction(double v, double vTarget, double delta, const double *vStep, const double *dsStep, double TMax, 79 | double deltaP); 80 | 81 | 82 | /** 83 | * Calculates the reaction on vehicles during approaching and following. [1] 84 | * 85 | * @param ds The actual net distance between vehicle and target (in *m*) 86 | * @param vPre Velocity of the target vehicle (NOT the relative velocity, in *m/s*) 87 | * @param v Velocity of the ego vehicle (in *m/s*) 88 | * @param T Desired time headway (in *s*) 89 | * @param TMax Maximum relevant time headway (in *s*) 90 | * @param s0 Desired distance when stopping (in *m*) 91 | * @return The resultant acceleration and the scale down factor for cruising 92 | */ 93 | double IDMFollowReaction(double ds, double vPre, double v, double T, double s0, double a, double b); 94 | 95 | 96 | /** 97 | * Calculates the yaw rate dependent on the given reference point. [2] 98 | * 99 | * @param x The distance in the vehicle x direction 100 | * @param y The distance in the vehicle y direction 101 | * @param dx The actual velocity of the vehicle 102 | * @param dy The lateral velocity (derivative of y) 103 | * @param P The control parameter for the reference angle 104 | * @param D The control parameter for the reference angle derivative 105 | * @param theta The reference angle (will be set by function, for debugging) 106 | * @param dTheta The reference angle derivative (will be set by function, for debugging) 107 | * @return The resultant yaw rate 108 | */ 109 | double SalvucciAndGray(double x, double y, double dx, double dy, double P, double D, double &theta, double &dTheta); 110 | 111 | 112 | /** 113 | * Calculates the acceleration by the Intelligent Driver Model (original). [1] 114 | * 115 | * @param v Velocity of the agent [m/s] 116 | * @param v0 Reference velocity (the desired speed without any preceiding road participant) [m/s] 117 | * @param ds Distance to the preceiding road participant [m] 118 | * @param dv Relative velocity to the preceiding road participant (dv = vego - v_rp) [m/s] 119 | * @param T Parameter time headway [s] 120 | * @param s0 Parameter stop distance [m] 121 | * @param ac Reference acceleration (ac >= 0) [m/s^2] 122 | * @param bc Reference deceleration (bc >= 0) [m/s^2] 123 | * @return 124 | */ 125 | double IDMOriginal(double v, double v0, double ds, double dv, double T, double s0, double ac, double bc); 126 | 127 | 128 | /** 129 | * Calculates the safety criterion (safety factor > 0) and the incentive criterion (incentive factor > 0) 130 | * according to the MOBIL model [3] 131 | * 132 | * @param safety The safety factor 133 | * @param incentive The incentive factor 134 | * @param v Ego velocity (@see IDMOriginal) 135 | * @param v0 Desired ego velocity (@see IDMOriginal) 136 | * @param T Time headway parameter (@see IDMOriginal) 137 | * @param s0 Stop distance parameter (@see IDMOriginal) 138 | * @param ac Acceleration (@see IDMOriginal) 139 | * @param bc Deceleration (@see IDMOriginal) 140 | * @param ds0f Distance to the front vehicle on the original lane 141 | * @param v0f Velocity to the front vehicle on the original lane 142 | * @param ds1f Distance to the front vehicle on the target lane 143 | * @param v1f Velocity to the front vehicle on the target lane 144 | * @param ds0b Distance to the back vehicle on the original lane 145 | * @param v0b Velocity to the back vehicle on the original lane 146 | * @param ds1b Distance to the back vehicle on the target lane 147 | * @param v1b Velocity to the back vehicle on the target lane 148 | * @param bSafe Safe deceleration (bSafe > 0, e.g. 0.5) [m/s^2] 149 | * @param aThr Threshold for accepted acceleration (aThr > 0, e.g. 0.5) [m/s^2] 150 | * @param p Politeness factor, allowing to vary the motivation for lane-changing from purely egoistic to more cooperative driving behavior. (p > 0, e.g. 0.8) [-] 151 | */ 152 | void 153 | MOBILOriginal(double &safety, double &incentive, double v, double v0, double T, double s0, double ac, double bc, 154 | double ds0f, double v0f, double ds1f, double v1f, double ds0b, double v0b, double ds1b, double v1b, 155 | double bSafe, double aThr, double p); 156 | 157 | 158 | /** 159 | * @brief Helper function: interpolation. 160 | * 161 | * Interpolates the value xx on the data x,y. 162 | * 163 | * @param xx The interpolation point 164 | * @param x Sample x point 165 | * @param y Sample y values 166 | * @param n Number of sample points given 167 | * @param extrapMode 0 = -inf/inf is returned, 1 = is extrapolating, other = returns the first/last value 168 | * @return Returns the interpolated value. 169 | */ 170 | double interpolate(double xx, const double *x, const double *y, unsigned int n, int extrapMode = 1); 171 | 172 | 173 | /** 174 | * Calculates the polynomial y = 3 * x^2 - 2 * x^3 between x=[0..1]. The curve's derivations are equal to zero 175 | * at x=0 and x=1, while y=0 at x=0 and y=1 at x=1. x out of bounds are set to 0 and 1 respectively. 176 | * @param x Input value 177 | * @return Result 178 | */ 179 | double scale(double x); 180 | 181 | 182 | /** 183 | * Calculates the linear scale factor between xMin and xMax. x = xMax leads to 1, while x = xMin leads to zero. 184 | * Values out of the bounds are 1 and 0, respectively. 185 | * @param x Input value 186 | * @param xMax Maximum value 187 | * @param xMin Minimum value 188 | * @return Result 189 | */ 190 | double linScale(double x, double xMax, double xMin); 191 | 192 | 193 | /** 194 | * Calculates the scale factor between xMin and xMax. x = xMax leads to 1, while x = xMin leads to zero. 195 | * The derivations are zero at the bounds. Values out of the bounds are 1 and 0, respectively. 196 | * @param x Input value 197 | * @param xMax Maximum value 198 | * @param xMin Minimum value 199 | * @param delta Potential factor to push the curve towards the min or max value 200 | * @return Result 201 | */ 202 | double scale(double x, double xMax, double xMin, double delta = 1.0); 203 | 204 | 205 | /** 206 | * Calculates the scale factor between xMin and xMax. x = xMax leads to zero, while x = xMin leads to 1. 207 | * The derivations are zero at the bounds. Values out of the bounds are 1 and 0, respectively. 208 | * @param x Input value 209 | * @param xMax Maximum value 210 | * @param xMin Minimum value 211 | * @param delta Potential factor to push the curve towards the min or max value 212 | * @return Result 213 | */ 214 | double invScale(double x, double xMax, double xMin, double delta = 1.0); 215 | 216 | 217 | /** 218 | * Scales the input from inf (x = xMax) to 1 (x = xMin). The derivative is zero at x = xMin 219 | * @param x Input value. Values out of the bounds are inf and 1, respectively. 220 | * @param xMax Maximum value 221 | * @param xMin Minimum value 222 | * @param delta Potential factor to push the curve towards the min or max value 223 | * @return Result 224 | */ 225 | double scaleInf(double x, double xMax, double xMin, double delta = 1.0); 226 | 227 | 228 | } 229 | 230 | #endif //AGENT_MODEL_COLLECTION_H 231 | -------------------------------------------------------------------------------- /src/README.md: -------------------------------------------------------------------------------- 1 | # Model capabilities, behavior and features 2 | 3 | ## Capability description 4 | 5 | - [x] 1. Holding the vehicle in standstill by using the _brake pedal_ 6 | - [x] 2. Controlling a _desired acceleration_ by using _drive_ and _brake pedal_ 7 | - [x] 3. Controlling a _desired speed_ 8 | - [x] 4. Controlling predictively a _desired speed_ in a given _distance_ 9 | - [x] 5. Controlling the vehicle to stop within a _desired distance_ 10 | - [x] 6. Controlling a _desired distance_ to a preceding vehicle 11 | - [x] 7. Controlling a _desired curvature_ by using the _steering_ 12 | - [x] 8. Following _reference points_ 13 | - [x] 9. Following _desired track points_ (horizon) 14 | - [x] 10. Controlling a _desired lateral offset_ to a given track 15 | - [x] 11. Switching from the current path to _another_ smoothly 16 | - [x] 12. Following other _road participants_ in the traffic flow 17 | - [x] 13. Keeping a maximum comfortable speed in curves (depending on the _maximum lateral acceleration_) 18 | - [x] 14. Keeping a maximum allowed speed (defined by _speed rules_, e.g. speed limits) 19 | - [x] 15. Keeping a _maximum comfortable speed_ defined by a parameter 20 | - [x] 16. Adapting the speed predictively according to the local speeds (12., 13., 14.) 21 | - [o] 17. Making _decision_ and _performing_ a lane change 22 | - [x] 18. Making _decision_ to stop at stop signs 23 | - [ ] 19. Making _decision_ to slow down or stop due to a conflict ahead 24 | 25 | ### Capability feature 1: Holding the Vehicle in Standstill 26 | 27 | The implementation can be found in `AgentModel::subconsciousStartStop() -> double`. The function returns the target pedal value. A P-controller sets the vehicle pedal value accordingly. 28 | 29 | **Objective:** 30 | 31 | Hold the vehicle in standstill (v = 0) such that the vehicle does not move. 32 | 33 | **Initial situation:** 34 | 35 | The agent slows down (feature #4 or feature #5) to come to a stop. When a minimum velocity is reached, the agent activates this feature. 36 | 37 | Relevant initial states are, if 38 | 1. the vehicle has already stopped (v = 0), 39 | 2. the vehicle is still moving (v > 0). 40 | 41 | **Execution:** 42 | 43 | By setting the flags `state.conscious.stop.standing` or `state.conscious.follow.standing` the feature is activated. When activated, the pedal value is set to `param.stop.pedalDuringStanding` (see [Parameters](#parameters)), otherwise the pedal value is set to infinity (direct pedal control deactivated). 44 | 45 | **Behavior:** 46 | 47 | 1. The vehicle is kept in standstill if $v=0$ (Test 1.1) 48 | 2. From a velocity v < 10 m/s, the vehicle stops and reaches the target pedal value within 5 seconds within 7 seconds. The deceleration is slightly below -2 m/s2 (Test 1.2). 49 | 50 | **Output:** 51 | 52 | The output of the controller is the input pedal value for the vehicle model. 53 | 54 | ### Capability feature 2: Controlling a desired acceleration 55 | 56 | The implementation can be found in `AgentModel::step(double simulationTime)`. The function calculates the desired acceleration. A PI-controller calculates the corresponding pedal value. 57 | 58 | **Objective:** 59 | 60 | Accelerate the vehicle exactly with a desired acceleration. 61 | 62 | **Limitations:** 63 | 64 | The acceleration is limited by the dynamics of the vehicle model. Basically, the max. acceleration is a function of the maximum power, the mass and the actual velocity of the vehicle. The maximum deceleration is basically limited by the maximum brake torque. 65 | 66 | **Initial situation** 67 | 68 | The agent uses controlled acceleration when 69 | * the agents wants to reach the desired speed (feature #3), or 70 | * the agent approaches and keeps distance to a preceding road participant (feature #6), or 71 | * the agent is going to stop the vehicle (feature #5). 72 | 73 | **Execution:** 74 | 75 | By setting the signal `state.subconscious.a`, the PI pedal controller sets a pedal value and compensates errors between desired and actual acceleration by a change in the drive or brake pedal. 76 | 77 | Within the normal behavior, A sum of reaction values for velocity control, stop control and distance control during following are calculated to the resultant desired acceleration, based on the Intelligent Driver Model (IDM) [1]. 78 | 79 | **Behavior:** 80 | 81 | The agent model compensates large acceleration errors within 5 seconds to an error below 0.01 m/s2. This expression is valid for realistic accelerations within the limitations of the vehicle model. The behavior can be adapted by the PI pedal controller. A large I parameter value leads to quick and accurate control of the acceleration with an overshoot, when steps are compensated. A low I parameter value leads to no overshoot, but a very low error is reached within a larger time period. 82 | 83 | **Output:** 84 | 85 | The output of the controller is the input pedal value for the vehicle model. 86 | 87 | ### Capability feature 3: Controlling a desired speed 88 | 89 | The implementation can be found in `AgentModel::step(double simulationTime)`. The function calculates the desired acceleration. 90 | 91 | Feature chain: Desired velocity → desired acceleration (feature 2) → pedal value (pedal controller) → actual pedal position (VM) → actual acceleration (VM) → actual velocity. 92 | 93 | **Objective:** 94 | 95 | Accelerate or decelerate the vehicle to reach and hold a desired velocity. 96 | 97 | **Limitations** 98 | 99 | The velocity of teh vehicle is limited by the dynamics of the vehicle model. Basically, the max. velocity is a function of the maximum power of the vehicle (and resistance parameters). The minimum velocity is zero. Backwards driving is not implemented, yet. 100 | 101 | **Initial situation** 102 | 103 | The agent uses the velocity control continuously 104 | * to limit the desired curve speed (feature #12), and 105 | * to avoid exceeding the permitted maximum speed (feature #13), and 106 | * to avoid exceeding the max. comfortable cruise speed (feature #14). 107 | 108 | **Execution:** 109 | 110 | By setting the signals `state.conscious.velocity.comfort`, `state.conscious.velocity.curve`, `state.conscious.velocity.rule`, the agent model calculates a desired acceleration. 111 | 112 | The minimum of the stated velocities is calculated and set as local maximum velocity. Based on the IDM [1], the acceleration is calculated. 113 | 114 | **Behavior:** 115 | 116 | The agent model accelerates or decelerates to reach the desired velocity from the actual velocity (velocity step). With decreasing the step, the actual acceleration decreases as well until, when the desired velocity is reached, the acceleration is zero. The time interval in which the velocity is reached, strongly depends on the velocity step and the acceleration parameter `_param.velocity.a`. The parameter defines the maximum acceleration to compensate velocity steps. Starting from v = 0, the agent model accelerates with the maximum acceleration set by the parameter. 117 | 118 | With a maximum acceleration set to 2.0 m/s2, the agent model reaches a velocity of 100 kph (27.8 m/s) within 30 seconds with an remaining error of less than 0.1 m/s (~99.8 %). Another performance factor for this feature is, that each velocity between 0 and 30 m/s is reached by at least 0.2 m/s within the time interval equal to the step (T_0.2 = (v_desired - v_actual)) 119 | 120 | **Output:** 121 | 122 | ## Parameters 123 | TODO 124 | 125 | ### Input-Interface 126 | The inputs required by the driver model are divided into five sub-structures: 127 | * **vehicle:** represents the vehicle state 128 | * **horizon:** a corridor in which the driver model plans its behavior 129 | * **signals:** array of relevant signals along the route 130 | * **lanes:** array of surrounding lanes relative to the driver's lane 131 | * **targets:** array of all moving objects around the driver 132 | 133 | Each quantity is documented in Doxygen style within `AgentModelInterface.h`. 134 | However, in the following some remarks on important structures are described in more detail. 135 | 136 | #### Horizon 137 | The most sensitive interface is the horizon because the driver model uses it to plan its longitudinal and lateral movement. 138 | First, the entries of a single horizon knot are described followed by instructions on how the horizon knots shall be arranged around the driver model. 139 | 140 | ##### Knots 141 | Every knot is located in the lane center of the current ego lane. 142 | At each knot the $x,y$-coordinates relative to the ego coordinate system have to specified. 143 | Further, the distance $ds$ along $s$ needs to be specified. 144 | Here, often it is sufficient to take the distances along the reference line (if the horizon is generated from an OpenDRIVE map). 145 | Note: in very narrow curves this simplification might not be adequate. 146 | The same assumptions are valid for the curvature $\kappa$ at each horizon knot. 147 | Positive curvature values correspond to left turns in positive $s$-direction. 148 | The angle $\psi$ is defined as the road's heading relative to ego-coordinate x-axis. 149 | Last, the lane width of the ego lane and its left and right neighbor has to be specified at each horizon knot. 150 | 151 | In Fig. 1 an example for each entry of one horizon knot is illustrated. 152 | 153 | 154 | 155 | *Fig 1: Horizon sketch* 156 | 157 | ##### Horizon Arrangement 158 | The position and amount of horizon knot along $s$ are theoretically arbitrary. 159 | However, we provide some recommended rules to generate the horizon in order to achieve the best possible driver model behavior. 160 | 161 | 1. The number of horizon knots `NOH` is currently $32$. This has been found as a good tradeoff in terms of efficiency and accuracy 162 | 2. The maximum horizon distance is not constant but dependent on the current speed. Here, $ds_\text{max} = 20 \text{sec} * v_x$ has been proved to be good. 163 | 3. It is advised to place the first horizon a little behind the ego vehicle because of some interpolations inside the driver model. A good value is $ds_0=-1\text{sec}*v_x$ 164 | 4. The knot distribution along $s$ between the first and last horizon knot can be chosen arbitrarily. However, linear or quadratic increasing distance both leads to good behavior. 165 | 5. If horizon knots are behind the end of a road, set $ds=\infty$. The driver model then will ignore the respective knots 166 | 167 | #### Lanes 168 | Up to `NOL` lanes can be send to the driver model. 169 | `NOL` is currently $32$ but often fewer lanes are sufficient. 170 | Some remarks on the lane id: 171 | * The `id` is not just an identifier but also indicates the position relative to the ego lane. Examples: `id=` 172 | * $0$ : ego lane 173 | * $-1$ : next lane to the left relative to the ego vehicle 174 | * $1$ : next lane to the right relative to the ego vehicle 175 | * $127$ : indicates that there is no lane entry (because the interface is of static length) 176 | * Lanes do not need to sorted by their id 177 | 178 | All other entries are described in `src/AgentModelInterface.h` under `struct Lane {...}`. 179 | 180 | #### Targets 181 | The required quantities for surrounding targets is documented straightforward in `src/AgentModelInterface.h` under `struct Target {...}`. 182 | 183 | #### Signals 184 | Signals may be 185 | * Stop signs (`SIGNAL_STOP`) 186 | * Traffic lights (`SIGNAL_TLS`) 187 | * Speed limits (`SIGNAL_SPEED_LIMIT`) 188 | 189 | Additionally to its type, a signal must be equipped with a unique id, the distance along $s$ to the signal and a value. 190 | The value is irrelevant for stop signs. Speed limits are given in `m/s`. 191 | A concept for traffic lights will follow in a later development stage. 192 | 193 | ### Output-Interface 194 | There is no distinct output interface. 195 | However, the final state of the driver (behavior) model is the desired acceleration $a_\text{des}$ and the desired curvature $\dot{\kappa}_\text{des}$. 196 | These quantities are located in the structure `State` inside the `subconcious` attribute of `src/AgentModelInterface.h`. 197 | 198 | Alternatively, if a vehicle model shall be used that takes pedal and steering values as an input, a simple controller could be deployed that controls the desired subconscious longitudinal and lateral quantities. 199 | 200 | ### Injection Concept 201 | T.B.D. 202 | 203 | ## References 204 | [1] Treiber, Martin, Ansgar Hennecke, and Dirk Helbing. “Congested Traffic States in Empirical Observations and Microscopic Simulations.” Physical Review E 62.2 (2000): 1805–1824. 205 | 206 | 207 | 208 | ## TODOs: 209 | * List all parameters 210 | * References 211 | * Test: following down to low speeds (v ~= 0) 212 | -------------------------------------------------------------------------------- /src/AgentModelInjection.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // 22 | // Created by Jens Klimke on 2020-04-07. 23 | // 24 | 25 | 26 | #include "AgentModelInjection.h" 27 | 28 | namespace agent_model { 29 | 30 | 31 | void registerTree(__Position *tree, Position *data, const void *owner) { 32 | tree->registerValue(data, owner); 33 | tree->x.registerValue(&data->x, owner); 34 | tree->y.registerValue(&data->y, owner); 35 | } 36 | 37 | void registerTree(__DynamicPosition *tree, DynamicPosition *data, const void *owner) { 38 | tree->registerValue(data, owner); 39 | tree->x.registerValue(&data->x, owner); 40 | tree->y.registerValue(&data->y, owner); 41 | tree->dx.registerValue(&data->dx, owner); 42 | tree->dy.registerValue(&data->dy, owner); 43 | } 44 | 45 | void registerTree(__Point *tree, Point *data, const void *owner) { 46 | tree->registerValue(data, owner); 47 | tree->distance.registerValue(&data->distance, owner); 48 | tree->time.registerValue(&data->time, owner); 49 | tree->value.registerValue(&data->value, owner); 50 | } 51 | 52 | void registerTree(__Dimensions *tree, Dimensions *data, const void *owner) { 53 | tree->registerValue(data, owner); 54 | tree->width.registerValue(&data->width, owner); 55 | tree->length.registerValue(&data->length, owner); 56 | } 57 | 58 | void registerTree(__VehicleState *tree, VehicleState *data, const void *owner) { 59 | tree->registerValue(data, owner); 60 | tree->v.registerValue(&data->v, owner); 61 | tree->a.registerValue(&data->a, owner); 62 | tree->psi.registerValue(&data->psi, owner); 63 | tree->dPsi.registerValue(&data->dPsi, owner); 64 | tree->s.registerValue(&data->s, owner); 65 | tree->d.registerValue(&data->d, owner); 66 | tree->pedal.registerValue(&data->pedal, owner); 67 | tree->steering.registerValue(&data->steering, owner); 68 | } 69 | 70 | void registerTree(__Horizon *tree, Horizon *data, const void *owner) { 71 | tree->registerValue(data, owner); 72 | registerArray(&tree->ds[0], &data->ds[0], owner, {NOH}); 73 | registerArray(&tree->x[0], &data->x[0], owner, {NOH}); 74 | registerArray(&tree->y[0], &data->y[0], owner, {NOH}); 75 | registerArray(&tree->psi[0], &data->psi[0], owner, {NOH}); 76 | registerArray(&tree->kappa[0], &data->kappa[0], owner, {NOH}); 77 | registerArray(&tree->egoLaneWidth[0], &data->egoLaneWidth[0], owner, {NOH}); 78 | registerArray(&tree->rightLaneOffset[0], &data->rightLaneOffset[0], owner, {NOH}); 79 | registerArray(&tree->leftLaneOffset[0], &data->leftLaneOffset[0], owner, {NOH}); 80 | } 81 | 82 | void registerTree(__Lane *tree, Lane *data, const void *owner) { 83 | tree->registerValue(data, owner); 84 | tree->id.registerValue(&data->id, owner); 85 | tree->width.registerValue(&data->width, owner); 86 | tree->route.registerValue(&data->route, owner); 87 | tree->closed.registerValue(&data->closed, owner); 88 | tree->dir.registerValue(&data->dir, owner); 89 | tree->access.registerValue(&data->access, owner); 90 | } 91 | 92 | void registerTree(__ControlPath *tree, ControlPath *data, const void *owner) { 93 | tree->registerValue(data, owner); 94 | tree->offset.registerValue(&data->offset, owner); 95 | tree->factor.registerValue(&data->factor, owner); 96 | registerStructArray(&tree->refPoints[0], &data->refPoints[0], owner, {NORP}); 97 | } 98 | 99 | void registerTree(__Signal *tree, Signal *data, const void *owner) { 100 | tree->registerValue(data, owner); 101 | tree->id.registerValue(&data->id, owner); 102 | tree->ds.registerValue(&data->ds, owner); 103 | tree->type.registerValue(&data->type, owner); 104 | tree->value.registerValue(&data->value, owner); 105 | } 106 | 107 | void registerTree(__Target *tree, Target *data, const void *owner) { 108 | tree->registerValue(data, owner); 109 | tree->id.registerValue(&data->id, owner); 110 | tree->ds.registerValue(&data->ds, owner); 111 | registerTree(&tree->xy, &data->xy, owner); 112 | tree->v.registerValue(&data->v, owner); 113 | tree->a.registerValue(&data->a, owner); 114 | tree->d.registerValue(&data->d, owner); 115 | tree->psi.registerValue(&data->psi, owner); 116 | tree->lane.registerValue(&data->lane, owner); 117 | registerTree(&tree->size, &data->size, owner); 118 | } 119 | 120 | void registerTree(__DecisionStopping *tree, DecisionStopping *data, const void *owner) { 121 | tree->registerValue(data, owner); 122 | tree->id.registerValue(&data->id, owner); 123 | tree->position.registerValue(&data->position, owner); 124 | tree->standingTime.registerValue(&data->standingTime, owner); 125 | } 126 | 127 | void registerTree(__Decisions *tree, Decisions *data, const void *owner) { 128 | tree->registerValue(data, owner); 129 | tree->laneChange.registerValue(&data->laneChangeInt, owner); 130 | tree->laneChange.registerValue(&data->laneChangeDec, owner); 131 | registerTree(&tree->lateral, &data->lateral, owner); 132 | registerTree(&tree->signal, &data->signal, owner); 133 | registerTree(&tree->target, &data->target, owner); 134 | registerTree(&tree->destination, &data->destination, owner); 135 | //registerStructArray(&tree->stopping[0], &data->stopping[0], owner, {NOS}); 136 | } 137 | 138 | void registerTree(__ConsciousVelocity *tree, ConsciousVelocity *data, const void *owner) { 139 | tree->registerValue(data, owner); 140 | tree->local.registerValue(&data->local, owner); 141 | tree->prediction.registerValue(&data->prediction, owner); 142 | } 143 | 144 | void registerTree(__ConsciousStop *tree, ConsciousStop *data, const void *owner) { 145 | tree->registerValue(data, owner); 146 | tree->ds.registerValue(&data->ds, owner); 147 | tree->dsMax.registerValue(&data->dsMax, owner); 148 | tree->standing.registerValue(&data->standing, owner); 149 | } 150 | 151 | void registerTree(__ConsciousFollow *tree, ConsciousFollow *data, const void *owner) { 152 | tree->registerValue(data, owner); 153 | tree->distance.registerValue(&data->targets[0].distance, owner); 154 | tree->velocity.registerValue(&data->targets[0].velocity, owner); 155 | tree->standing.registerValue(&data->standing, owner); 156 | } 157 | 158 | void registerTree(__ConsciousLateral *tree, ConsciousLateral *data, const void *owner) { 159 | tree->registerValue(data, owner); 160 | registerStructArray(&tree->paths[0], &data->paths[0], owner, {NOCP}); 161 | } 162 | 163 | void registerTree(__Conscious *tree, Conscious *data, const void *owner) { 164 | tree->registerValue(data, owner); 165 | registerTree(&tree->velocity, &data->velocity, owner); 166 | registerTree(&tree->stop, &data->stop, owner); 167 | registerTree(&tree->follow, &data->follow, owner); 168 | registerTree(&tree->lateral, &data->lateral, owner); 169 | } 170 | 171 | void registerTree(__Subconscious *tree, Subconscious *data, const void *owner) { 172 | tree->registerValue(data, owner); 173 | tree->a.registerValue(&data->a, owner); 174 | tree->dPsi.registerValue(&data->dPsi, owner); 175 | tree->kappa.registerValue(&data->kappa, owner); 176 | tree->pedal.registerValue(&data->pedal, owner); 177 | tree->steering.registerValue(&data->steering, owner); 178 | } 179 | 180 | void registerTree(__MemoryVehicle *tree, MemoryVehicle *data, const void *owner) { 181 | tree->registerValue(data, owner); 182 | tree->s.registerValue(&data->s, owner); 183 | } 184 | 185 | void registerTree(__MemoryLateral *tree, MemoryLateral *data, const void *owner) { 186 | tree->registerValue(data, owner); 187 | tree->time.registerValue(&data->time, owner); 188 | tree->startTime.registerValue(&data->startTime, owner); 189 | tree->distance.registerValue(&data->distance, owner); 190 | tree->startDistance.registerValue(&data->startDistance, owner); 191 | tree->offset.registerValue(&data->offset, owner); 192 | } 193 | 194 | void registerTree(__MemoryLaneChange *tree, MemoryLaneChange *data, const void *owner) { 195 | tree->registerValue(data, owner); 196 | tree->switchLane.registerValue(&data->switchLane, owner); 197 | tree->decision.registerValue(&data->decision, owner); 198 | tree->startTime.registerValue(&data->startTime, owner); 199 | } 200 | 201 | void registerTree(__ParameterVelocityControl *tree, ParameterVelocityControl *data, const void *owner) { 202 | tree->registerValue(data, owner); 203 | tree->thwMax.registerValue(&data->thwMax, owner); 204 | tree->delta.registerValue(&data->delta, owner); 205 | tree->deltaPred.registerValue(&data->deltaPred, owner); 206 | tree->a.registerValue(&data->a, owner); 207 | tree->b.registerValue(&data->b, owner); 208 | tree->vScale.registerValue(&data->vScale, owner); 209 | tree->ayMax.registerValue(&data->ayMax, owner); 210 | tree->vComfort.registerValue(&data->vComfort, owner); 211 | } 212 | 213 | void registerTree(__ParameterFollowing *tree, ParameterFollowing *data, const void *owner) { 214 | tree->registerValue(data, owner); 215 | tree->timeHeadway.registerValue(&data->timeHeadway, owner); 216 | tree->dsStopped.registerValue(&data->dsStopped, owner); 217 | tree->thwMax.registerValue(&data->thwMax, owner); 218 | } 219 | 220 | void registerTree(__ParameterVehicle *tree, ParameterVehicle *data, const void *owner) { 221 | tree->registerValue(data, owner); 222 | registerTree(&tree->size, &data->size, owner); 223 | registerTree(&tree->pos, &data->pos, owner); 224 | } 225 | 226 | void registerTree(__ParameterSteering *tree, ParameterSteering *data, const void *owner) { 227 | tree->registerValue(data, owner); 228 | registerArray(&tree->thw[0], &data->thw[0], owner, {NORP}); 229 | registerArray(&tree->dsMin[0], &data->dsMin[0], owner, {NORP}); 230 | registerArray(&tree->P[0], &data->P[0], owner, {NORP}); 231 | registerArray(&tree->D[0], &data->D[0], owner, {NORP}); 232 | } 233 | 234 | void registerTree(__ParameterStopping *tree, ParameterStopping *data, const void *owner) { 235 | tree->registerValue(data, owner); 236 | tree->dsGap.registerValue(&data->dsGap, owner); 237 | tree->TMax.registerValue(&data->TMax, owner); 238 | tree->dsMax.registerValue(&data->dsMax, owner); 239 | tree->T.registerValue(&data->T, owner); 240 | tree->tSign.registerValue(&data->tSign, owner); 241 | tree->vStopped.registerValue(&data->vStopped, owner); 242 | tree->pedalDuringStanding.registerValue(&data->pedalDuringStanding, owner); 243 | } 244 | 245 | void registerTree(__ParameterLaneChange *tree, ParameterLaneChange *data, const void *owner) { 246 | tree->registerValue(data, owner); 247 | tree->bSafe.registerValue(&data->bSafe, owner); 248 | tree->aThreshold.registerValue(&data->aThreshold, owner); 249 | tree->politenessFactor.registerValue(&data->politenessFactor, owner); 250 | tree->time.registerValue(&data->time, owner); 251 | } 252 | 253 | void registerTree(__Input *tree, Input *data, const void *owner) { 254 | tree->registerValue(data, owner); 255 | registerTree(&tree->vehicle, &data->vehicle, owner); 256 | registerTree(&tree->horizon, &data->horizon, owner); 257 | registerStructArray(&tree->signals[0], &data->signals[0], owner, {NOS}); 258 | registerStructArray(&tree->lanes[0], &data->lanes[0], owner, {NOL}); 259 | registerStructArray(&tree->targets[0], &data->targets[0], owner, {NOT}); 260 | } 261 | 262 | void registerTree(__State *tree, State *data, const void *owner) { 263 | tree->registerValue(data, owner); 264 | tree->simulationTime.registerValue(&data->simulationTime, owner); 265 | registerTree(&tree->decisions, &data->decisions, owner); 266 | registerTree(&tree->conscious, &data->conscious, owner); 267 | registerTree(&tree->subconscious, &data->subconscious, owner); 268 | registerArray(&tree->aux[0], &data->aux[0], owner, {NOA}); 269 | } 270 | 271 | void registerTree(__Memory *tree, Memory *data, const void *owner) { 272 | tree->registerValue(data, owner); 273 | registerTree(&tree->vehicle, &data->vehicle, owner); 274 | tree->velocity.registerValue(&data->velocity, owner); 275 | registerTree(&tree->lateral, &data->lateral, owner); 276 | registerTree(&tree->laneChange, &data->laneChange, owner); 277 | } 278 | 279 | void registerTree(__Parameters *tree, Parameters *data, const void *owner) { 280 | tree->registerValue(data, owner); 281 | registerTree(&tree->vehicle, &data->vehicle, owner); 282 | registerTree(&tree->laneChange, &data->laneChange, owner); 283 | registerTree(&tree->stop, &data->stop, owner); 284 | registerTree(&tree->velocity, &data->velocity, owner); 285 | registerTree(&tree->follow, &data->follow, owner); 286 | registerTree(&tree->steering, &data->steering, owner); 287 | } 288 | 289 | 290 | 291 | } // namespace 292 | 293 | -------------------------------------------------------------------------------- /src/AgentModelInjection.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // 22 | // Created by Jens Klimke on 2020-04-07. 23 | // 24 | 25 | 26 | #ifndef AGENT_MODEL_REGISTRATION_H 27 | #define AGENT_MODEL_REGISTRATION_H 28 | 29 | #include 30 | #include "Interface.h" 31 | 32 | namespace agent_model { 33 | 34 | struct __Position : public Injection { 35 | using Injection::operator =; 36 | Injection x; 37 | Injection y; 38 | }; 39 | 40 | struct __DynamicPosition : public Injection { 41 | using Injection::operator =; 42 | Injection x; 43 | Injection y; 44 | Injection dx; 45 | Injection dy; 46 | }; 47 | 48 | struct __Point : public Injection { 49 | using Injection::operator =; 50 | Injection distance; 51 | Injection time; 52 | Injection value; 53 | }; 54 | 55 | struct __Dimensions : public Injection { 56 | using Injection::operator =; 57 | Injection width; 58 | Injection length; 59 | }; 60 | 61 | struct __VehicleState : public Injection { 62 | using Injection::operator =; 63 | Injection v; 64 | Injection a; 65 | Injection psi; 66 | Injection dPsi; 67 | Injection s; 68 | Injection d; 69 | Injection pedal; 70 | Injection steering; 71 | }; 72 | 73 | struct __Horizon : public Injection { 74 | using Injection::operator =; 75 | Injection ds[NOH]; 76 | Injection x[NOH]; 77 | Injection y[NOH]; 78 | Injection psi[NOH]; 79 | Injection kappa[NOH]; 80 | Injection egoLaneWidth[NOH]; 81 | Injection rightLaneOffset[NOH]; 82 | Injection leftLaneOffset[NOH]; 83 | }; 84 | 85 | struct __Lane : public Injection { 86 | using Injection::operator =; 87 | Injection id; 88 | Injection width; 89 | Injection route; 90 | Injection closed; 91 | Injection dir; 92 | Injection access; 93 | }; 94 | 95 | struct __ControlPath : public Injection { 96 | using Injection::operator =; 97 | Injection offset; 98 | Injection factor; 99 | __DynamicPosition refPoints[NORP]; 100 | }; 101 | 102 | struct __Signal : public Injection { 103 | using Injection::operator =; 104 | Injection id; 105 | Injection ds; 106 | Injection type; 107 | Injection value; 108 | }; 109 | 110 | struct __Target : public Injection { 111 | using Injection::operator =; 112 | Injection id; 113 | Injection ds; 114 | Injection v; 115 | Injection a; 116 | Injection d; 117 | Injection psi; 118 | Injection lane; 119 | __Position xy; 120 | __Dimensions size; 121 | }; 122 | 123 | struct __DecisionStopping : public Injection { 124 | using Injection::operator =; 125 | Injection id; 126 | Injection position; 127 | Injection standingTime; 128 | }; 129 | 130 | struct __Decisions : public Injection { 131 | using Injection::operator =; 132 | Injection laneChange; 133 | __Point lateral; 134 | __DecisionStopping signal; 135 | __DecisionStopping target; 136 | __DecisionStopping destination; 137 | }; 138 | 139 | struct __ConsciousVelocity : public Injection { 140 | using Injection::operator =; 141 | Injection local; 142 | Injection prediction; 143 | }; 144 | 145 | struct __ConsciousStop : public Injection { 146 | using Injection::operator =; 147 | Injection ds; 148 | Injection dsMax; 149 | Injection standing; 150 | }; 151 | 152 | struct __ConsciousFollow : public Injection { 153 | using Injection::operator =; 154 | Injection distance; 155 | Injection velocity; 156 | Injection standing; 157 | }; 158 | 159 | struct __ConsciousLateral : public Injection { 160 | using Injection::operator =; 161 | __ControlPath paths[NOCP]; 162 | }; 163 | 164 | struct __Conscious : public Injection { 165 | using Injection::operator =; 166 | __ConsciousVelocity velocity; 167 | __ConsciousStop stop; 168 | __ConsciousFollow follow; 169 | __ConsciousLateral lateral; 170 | }; 171 | 172 | struct __Subconscious : public Injection { 173 | using Injection::operator =; 174 | Injection a; 175 | Injection dPsi; 176 | Injection kappa; 177 | Injection pedal; 178 | Injection steering; 179 | }; 180 | 181 | struct __MemoryVehicle : public Injection { 182 | using Injection::operator =; 183 | Injection s; 184 | }; 185 | 186 | struct __MemoryLateral : public Injection { 187 | using Injection::operator =; 188 | Injection time; 189 | Injection startTime; 190 | Injection distance; 191 | Injection startDistance; 192 | Injection offset; 193 | }; 194 | 195 | struct __MemoryLaneChange : public Injection { 196 | using Injection::operator =; 197 | Injection switchLane; 198 | Injection decision; 199 | Injection startTime; 200 | }; 201 | 202 | struct __ParameterVelocityControl : public Injection { 203 | using Injection::operator =; 204 | Injection thwMax; 205 | Injection delta; 206 | Injection deltaPred; 207 | Injection a; 208 | Injection b; 209 | Injection vScale; 210 | Injection ayMax; 211 | Injection vComfort; 212 | }; 213 | 214 | struct __ParameterFollowing : public Injection { 215 | using Injection::operator =; 216 | Injection timeHeadway; 217 | Injection dsStopped; 218 | Injection thwMax; 219 | }; 220 | 221 | struct __ParameterVehicle : public Injection { 222 | using Injection::operator =; 223 | __Dimensions size; 224 | __Position pos; 225 | }; 226 | 227 | struct __ParameterSteering : public Injection { 228 | using Injection::operator =; 229 | Injection thw[NORP]; 230 | Injection dsMin[NORP]; 231 | Injection P[NORP]; 232 | Injection D[NORP]; 233 | }; 234 | 235 | struct __ParameterStopping : public Injection { 236 | using Injection::operator =; 237 | Injection dsGap; 238 | Injection TMax; 239 | Injection dsMax; 240 | Injection T; 241 | Injection tSign; 242 | Injection vStopped; 243 | Injection pedalDuringStanding; 244 | }; 245 | 246 | struct __ParameterLaneChange : public Injection { 247 | using Injection::operator =; 248 | Injection bSafe; 249 | Injection aThreshold; 250 | Injection politenessFactor; 251 | Injection time; 252 | }; 253 | 254 | struct __Input : public Injection { 255 | using Injection::operator =; 256 | __VehicleState vehicle; 257 | __Horizon horizon; 258 | __Signal signals[NOS]; 259 | __Lane lanes[NOL]; 260 | __Target targets[NOT]; 261 | }; 262 | 263 | struct __State : public Injection { 264 | using Injection::operator =; 265 | Injection simulationTime; 266 | Injection aux[NOA]; 267 | __Decisions decisions; 268 | __Conscious conscious; 269 | __Subconscious subconscious; 270 | }; 271 | 272 | struct __Memory : public Injection { 273 | using Injection::operator =; 274 | Injection velocity; 275 | __MemoryVehicle vehicle; 276 | __MemoryLateral lateral; 277 | __MemoryLaneChange laneChange; 278 | }; 279 | 280 | struct __Parameters : public Injection { 281 | using Injection::operator =; 282 | __ParameterVehicle vehicle; 283 | __ParameterLaneChange laneChange; 284 | __ParameterStopping stop; 285 | __ParameterVelocityControl velocity; 286 | __ParameterFollowing follow; 287 | __ParameterSteering steering; 288 | }; 289 | 290 | 291 | void registerTree(__Position *tree, Position *data, const void *owner); 292 | void registerTree(__DynamicPosition *tree, DynamicPosition *data, const void *owner); 293 | void registerTree(__Point *tree, Point *data, const void *owner); 294 | void registerTree(__Dimensions *tree, Dimensions *data, const void *owner); 295 | void registerTree(__VehicleState *tree, VehicleState *data, const void *owner); 296 | void registerTree(__Horizon *tree, Horizon *data, const void *owner); 297 | void registerTree(__Lane *tree, Lane *data, const void *owner); 298 | void registerTree(__ControlPath *tree, ControlPath *data, const void *owner); 299 | void registerTree(__Signal *tree, Signal *data, const void *owner); 300 | void registerTree(__Target *tree, Target *data, const void *owner); 301 | void registerTree(__DecisionStopping *tree, DecisionStopping *data, const void *owner); 302 | void registerTree(__Decisions *tree, Decisions *data, const void *owner); 303 | void registerTree(__ConsciousVelocity *tree, ConsciousVelocity *data, const void *owner); 304 | void registerTree(__ConsciousStop *tree, ConsciousStop *data, const void *owner); 305 | void registerTree(__ConsciousFollow *tree, ConsciousFollow *data, const void *owner); 306 | void registerTree(__ConsciousLateral *tree, ConsciousLateral *data, const void *owner); 307 | void registerTree(__Conscious *tree, Conscious *data, const void *owner); 308 | void registerTree(__Subconscious *tree, Subconscious *data, const void *owner); 309 | void registerTree(__MemoryVehicle *tree, MemoryVehicle *data, const void *owner); 310 | void registerTree(__MemoryLateral *tree, MemoryLateral *data, const void *owner); 311 | void registerTree(__MemoryLaneChange *tree, MemoryLaneChange *data, const void *owner); 312 | void registerTree(__ParameterVelocityControl *tree, ParameterVelocityControl *data, const void *owner); 313 | void registerTree(__ParameterFollowing *tree, ParameterFollowing *data, const void *owner); 314 | void registerTree(__ParameterVehicle *tree, ParameterVehicle *data, const void *owner); 315 | void registerTree(__ParameterSteering *tree, ParameterSteering *data, const void *owner); 316 | void registerTree(__ParameterStopping *tree, ParameterStopping *data, const void *owner); 317 | void registerTree(__ParameterLaneChange *tree, ParameterLaneChange *data, const void *owner); 318 | void registerTree(__Input *tree, Input *data, const void *owner); 319 | void registerTree(__State *tree, State *data, const void *owner); 320 | void registerTree(__Memory *tree, Memory *data, const void *owner); 321 | void registerTree(__Parameters *tree, Parameters *data, const void *owner); 322 | 323 | 324 | template 325 | void registerStructArray(T *pointer, D *data, const void *owner, std::vector sizes, unsigned long j = 0) { 326 | 327 | // last iteration 328 | if(j == sizes.size() - 1) { 329 | 330 | // iterate over j-th size 331 | for(unsigned long i = 0; i < sizes[j]; ++i) { 332 | registerTree(pointer, data, owner); 333 | pointer++; data++; 334 | } 335 | 336 | return; 337 | 338 | } 339 | 340 | // calculate step size 341 | unsigned long steps = 1; 342 | for(unsigned long i = j + 1; i < sizes.size(); ++i) 343 | steps *= sizes[i]; 344 | 345 | // iterate over j-th size 346 | for(unsigned long i = 0; i < sizes[j]; ++i) { 347 | registerStructArray(pointer, data, owner, sizes, j + 1); 348 | pointer += steps; 349 | data += steps; 350 | } 351 | 352 | } 353 | 354 | template 355 | void registerArray(T *pointer, D *data, const void *owner, std::vector sizes, unsigned long j = 0) { 356 | 357 | // last iteration 358 | if(j == sizes.size() - 1) { 359 | 360 | // iterate over j-th size 361 | for(unsigned long i = 0; i < sizes[j]; ++i) { 362 | pointer->registerValue(data, owner); 363 | pointer++; data++; 364 | } 365 | 366 | return; 367 | 368 | } 369 | 370 | // calculate step size 371 | unsigned long steps = 1; 372 | for(unsigned long i = j + 1; i < sizes.size(); ++i) 373 | steps *= sizes[i]; 374 | 375 | // iterate over j-th size 376 | for(unsigned long i = 0; i < sizes[j]; ++i) { 377 | registerArray(pointer, data, owner, sizes, j + 1); 378 | pointer += steps; 379 | data += steps; 380 | } 381 | 382 | } 383 | 384 | 385 | } // namespace 386 | 387 | #endif // AGENT_MODEL_REGISTRATION_H 388 | -------------------------------------------------------------------------------- /src/Interface.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // 22 | // Created by Jens Klimke on 2020-04-07. 23 | // 24 | 25 | 26 | #ifndef AGENT_MODEL_INTERFACE_H 27 | #define AGENT_MODEL_INTERFACE_H 28 | 29 | 30 | namespace agent_model { 31 | 32 | static const unsigned int NOT = 32; //!< Maximum defined number of targets. 33 | static const unsigned int NOL = 32; //!< Maximum defined number of lanes. 34 | static const unsigned int NOS = 32; //!< Maximum defined number of signals. 35 | static const unsigned int NOH = 32; //!< Maximum defined number of horizon sample points. 36 | static const unsigned int NORP = 2; //!< Number of reference points per control path. 37 | static const unsigned int NOCP = 3; //!< Number of reference points per control path. 38 | static const unsigned int NOA = 32; //!< Number of auxiliary states. 39 | 40 | static const unsigned int NOTL = 32; //!< Maximum defined number of traffic lights. 41 | 42 | 43 | /*!< This enum describes a access state of an area. */ 44 | enum Accessibility { ACC_NOT_SET, ACC_ACCESSIBLE, ACC_NOT_ALLOWED, ACC_NOT_ACCESSIBLE }; 45 | 46 | /*!< This enum defines the driving direction of a lane. */ 47 | enum DrivingDirection { DD_FORWARDS, DD_BACKWARDS, DD_BOTH, DD_NONE, DD_NOT_SET }; 48 | 49 | /*!< This enum describes the type of a road signal. */ 50 | enum SignalType { SIGNAL_NOT_SET, SIGNAL_STOP, SIGNAL_TLS, SIGNAL_SPEED_LIMIT, SIGNAL_YIELD, SIGNAL_PRIORITY }; 51 | 52 | /*!< This enum describes the color of the traffic lights. */ //For now just colors.Todo: Add different icon color combinations 53 | enum TrafficLightColor { COLOR_RED, COLOR_YELLOW, COLOR_GREEN }; 54 | 55 | /*!< This enum describes the icon of the traffic lights. */ //For now just colors.Todo: Add different icon color combinations 56 | enum TrafficLightIcon { ICON_OTHER, ICON_NONE, ICON_ARROW_STRAIGHT_AHEAD, ICON_ARROW_LEFT, ICON_ARROW_RIGHT }; 57 | 58 | /*!< This enum describes the priority of a target. */ 59 | enum TargetPriority { TARGET_ON_PRIORITY_LANE, TARGET_ON_GIVE_WAY_LANE, TARGET_PRIORITY_NOT_SET }; 60 | 61 | /*!< This enum describes the priority of a target. */ 62 | enum TargetPosition { TARGET_NOT_RELEVANT, TARGET_ON_PATH, TARGET_ON_INTERSECTION, TARGET_ON_OPPOSITE, TARGET_ON_RIGHT, TARGET_ON_LEFT }; 63 | 64 | /*!< This enum describes the maneuver performed by the host. */ 65 | enum Maneuver { STRAIGHT, TURN_LEFT, TURN_RIGHT }; 66 | 67 | 68 | 69 | /*!< A 2D position class. */ 70 | struct Position { 71 | double x; //!< The x ordinate. (in *m*) 72 | double y; //!< The y ordinate. (in *m*) 73 | Position(): x(0.0), y(0.0) {} 74 | Position(double pX, double pY) : x(pX), y(pY) {} 75 | 76 | bool operator==(const Position& other) 77 | { 78 | return x == other.x && y == other.y; 79 | } 80 | }; 81 | 82 | 83 | 84 | /*!< A 2D position with motion and a influence factor. */ 85 | struct DynamicPosition { 86 | double x; //!< The x ordinate. (in *m*) 87 | double y; //!< The y ordinate. (in *m*) 88 | double dx; //!< The derivative of the x component. (in *m/s*) 89 | double dy; //!< The derivative of the y component. (in *m/s*) 90 | }; 91 | 92 | /*!< A point class, consisting of a distance and a value. */ 93 | struct Point { 94 | double distance; //!< The distance at which the value applies. (in *m*) 95 | double time; //!< The relative time at which the value applies. (in *s*) 96 | double value; //!< The value of the point. 97 | }; 98 | 99 | /*!< A dimensions class, saving width and length of an object. */ 100 | struct Dimensions { 101 | double width; //!< The width of an object. (in *m*) 102 | double length; //!< The length of an object. (in *m*) 103 | }; 104 | 105 | /*!< A class to save a vehicle state. */ 106 | struct VehicleState { 107 | double v; //!< The velocity of the vehicle in x direction. (in *m/s*) 108 | double a; //!< The acceleration of the vehicle in x direction. (in *m/s^2*) 109 | double psi; //!< The yaw angle of the vehicle which is the angle between the vehicle x axis and heading of the current lane in mathematical positive direction. (in *rad*) 110 | double dPsi; //!< The time derivative of the yaw angle (yaw rate). (in *rad/s*) 111 | double s; //!< The distance, the vehicle travelled since the last reset. (in *m*) 112 | double d; //!< The lateral offset of the vehicle to the current reference line of the track (e.g. lane center). (in *m*) 113 | double pedal; //!< The actual pedal value [-1..1]. Negative values define a brake pedal 114 | double steering; //!< The actual steering value [-1..1]. Negative values define left turns 115 | Maneuver maneuver; //!< The general classification of the vehicle's path during the scenario 116 | double dsIntersection; //!< Distance along s to the intersection (if ego is approaching an intersection) 117 | 118 | }; 119 | 120 | /*!< A class to store horizon points. */ 121 | struct Horizon { 122 | double ds[NOH]; //!< Distance to the horizon point along s measured from the origin of the ego coordinate system. (in *m*) 123 | double x[NOH]; //!< x ordinate relative to ego unit (in *m*) 124 | double y[NOH]; //!< y ordinate relative to ego unit (in *m*) 125 | double psi[NOH]; //!< heading relative to vehicle x-axis (in *rad*) 126 | double kappa[NOH]; //!< curvature of the road (in *1/m*) 127 | double egoLaneWidth[NOH]; //!< Width of ego lane (in *m*) -1 if not set 128 | double rightLaneOffset[NOH]; //!< Offset to right centerlane (in *m*) 0 if not set 129 | double leftLaneOffset[NOH]; //!< Offset to left centerlane (in *m*) 0 if not set 130 | double destinationPoint; //!< s coordinate of destination point (in *m*) -1 if not set 131 | }; 132 | 133 | /*!< A class to store lane information. */ 134 | struct Lane { 135 | int id; //!< Unique ID of the signal. The id is not just an identifier but also specifies the position of the lane relative to the ego lane in OpenDRIVE manner! e.g. -1 = the next lane to the left, 1 = the next lane to the right. 136 | double width; //!< Width of the lane (in *m*) -1 if not set 137 | double route; //!< Distance on the lane until the lane splits from the current route. (in *m*) -1 if not set 138 | double closed; //!< Distance on the lane until the lane is closed. (in *m*) -1 if not set 139 | DrivingDirection dir; //!< The driving direction of the lane related to the ego direction. 140 | Accessibility access; //!< The accessibility of the lane from the ego lane. - true if type driving 141 | int lane_change; //!< Flag if lane change is 142 | // not defined (-1) 143 | // not necessary (0) - already on target lane 144 | // intended (1) - necessary, but also later possibility 145 | // required (2) - last chance 146 | }; 147 | 148 | /*!< A class to store control path information */ 149 | struct ControlPath { 150 | double offset; //!< The lateral offset from the reference line to be controlled to. (in *m*) 151 | double factor; //!< A factor to describe the influence of the point 152 | DynamicPosition refPoints[NORP]; //!< The reference points for the lateral control. 153 | }; 154 | 155 | /*!< A class to store the internal state for the conscious/follow component. */ 156 | struct FollowTarget { 157 | double factor; //!< The influence of the target 158 | double lane; //!< Lane ID of the actual lane of the target relative to driver's lane. 159 | double distance; //!< The distance to the target to be followed. (in *m*) 160 | double velocity; //!< The absolute velocity of the target to be followed. (in *m/s*) 161 | }; 162 | 163 | /*!< A class to store signal information. */ 164 | struct Signal { 165 | unsigned int id; //!< Unique ID of the signal 166 | double ds; //!< Distance to the sign from the current position along the reference line. (in *m*) 167 | SignalType type; //!< Type of the signal. 168 | double value; //!< Value of the signal. 169 | TrafficLightColor color; //!< Color of the light bulb. 170 | TrafficLightIcon icon; //!< Icon/Shape of the traffic light. 171 | bool subsignal; //!< if true sign is subsignal to TLS and only valid in certain situations 172 | bool sign_is_in_use; //!< indicates that subsign is in use (all paired TLS signals are out of service) 173 | }; 174 | 175 | 176 | /*!< A class to store target information. */ 177 | struct Target { 178 | unsigned int id; //!< Unique ID of the target. id=0 indicates that the target is not defined in the array position 179 | double ds; //!< Distance along s to the target center point from the ego driver position. (in *m*) 180 | Position xy; //!< Relative position of the target relative to the driver position and heading. 181 | double v; //!< Absolute velocity of the target. (in *m/s*) 182 | double a; //!< Absolute acceleration of the target. (in *m/s^2*) 183 | double d; //!< Lateral offset of the target in its corresponding lane. (in *m*) 184 | double psi; //!< Relative yaw angle of the target vehicle to the ego yaw angle. (in *rad*) 185 | int lane; //!< Lane ID of the actual lane of the target relative to driver's lane. 186 | Dimensions size; //!< Width and length of the target. 187 | double dsIntersection; //!< Distance along s to the intersection (if target is approaching an intersection) 188 | TargetPriority priority; //!< Priority of the target's lane. Used to determine right of way. 189 | TargetPosition position; //!< Area in junction of target. Used to determine right of way. 190 | }; 191 | 192 | /*!< A class to store the internal state for the decision/stopping component. */ 193 | struct DecisionStopping { 194 | unsigned int id; //!< The ID of the stop. 195 | double position; //!< The absolute longitudinal position of the stop. 196 | double standingTime; //!< The time, the driver shall stand at the stop. 197 | }; 198 | 199 | /*!< A class to store the internal state for the decisions components. */ 200 | struct Decisions { 201 | int laneChangeInt; //!< The intention to perform a lane change. The sign defines the direction. The value defines the number of lanes to be changed. 202 | int laneChangeDec; //!< The decision to perform a lane change. The sign defines the direction. The value defines the number of lanes to be changed. 203 | Point lateral; //!< The decision to move to a defined lateral offset within a defined distance or time (mode=0: distance, mode=1: time). 204 | DecisionStopping signal; //!< The decision information caused by a signal. 205 | DecisionStopping target; //!< The decision information caused by a target. 206 | DecisionStopping destination; //!< The decision information caused by a destination. 207 | DecisionStopping lane; //!< The decision information caused by a lane change. 208 | }; 209 | 210 | /*!< A class to store the internal state for the conscious/velocity component. */ 211 | struct ConsciousVelocity { 212 | double local; //!< The local velocity. (in *m/s*) 213 | double prediction; //!< The prediction mean velocity. (in *m/s*) 214 | }; 215 | 216 | /*!< A class to store the internal state for the conscious/stop component. */ 217 | struct ConsciousStop { 218 | double ds; //!< The actual distance to the stop point. (in *m*) 219 | double dsMax; //!< The reference distance at which the driver decides to stop (in *m*) 220 | bool standing; //!< A flag to define if the driver has stopped for the desired stop. 221 | bool priority; //!< A flag to define if the driver drives on a priority lane. 222 | bool give_way; //!< A flag to define if the driver drives on a give way lane. 223 | }; 224 | 225 | /*!< A class to store the internal state for the conscious/follow component. */ 226 | struct ConsciousFollow { 227 | FollowTarget targets[2]; 228 | bool standing; //!< A flag to define whether the driver wants to keep the vehicle in standstill. 229 | }; 230 | 231 | /*!< A class to store the internal state for the conscious/lateral component. */ 232 | struct ConsciousLateral { 233 | ControlPath paths[NOCP]; //!< An array of control paths 234 | }; 235 | 236 | /*!< A class to store the internal state for the conscious components. */ 237 | struct Conscious { 238 | ConsciousVelocity velocity; //!< A class to store the internal state for the conscious/velocity component. 239 | ConsciousStop stop; //!< A class to store the internal state for the conscious/stop component. 240 | ConsciousFollow follow; //!< A class to store the internal state for the conscious/follow component. 241 | ConsciousLateral lateral; //!< A class to store the internal state for the conscious/lateral component. 242 | }; 243 | 244 | /*!< A class to store the internal state for the subconscious components. */ 245 | struct Subconscious { 246 | double a; //!< Desired acceleration. (in *m/s^2*) 247 | double dPsi; //!< Desired yaw rate. (in *rad/s*) 248 | double kappa; //!< Desired curvature. (in *1/m*) 249 | double pedal; //!< Desired pedal value. 250 | double steering; //!< Desired steering angle. 251 | }; 252 | 253 | /*!< A class to store all memory vehicle states. */ 254 | struct MemoryVehicle { 255 | double s; //!< Absolute travelled distance since the last reset. (in *m*) 256 | }; 257 | 258 | /*!< A class to store all memory lateral control states. */ 259 | struct MemoryLateral { 260 | double time; //!< The time to reach the lateral offset. 261 | double startTime; //!< The start time of the lateral motion. 262 | double distance; //!< The distance to reach the lateral offset. 263 | double startDistance; //!< The start distance of the lateral motion. 264 | double offset; //!< The offset to be reached. 265 | }; 266 | 267 | /*!< A class to store all memory lane change states. */ 268 | struct MemoryLaneChange { 269 | int switchLane; //!< The lane to be switched to. 270 | int decision; //!< The decisions to which lane the driver wants to change to. 271 | double startTime; //!< The start time of the lane change. (in *s*) 272 | }; 273 | 274 | /*!< A class to store the parameters for velocity components. */ 275 | struct ParameterVelocityControl { 276 | double thwMax; //!< The maximum time headway the driver starts to react (in *s*) 277 | double delta; //!< The power for the local speed reaction (see delta in IDM: https://en.wikipedia.org/wiki/Intelligent_driver_model) 278 | double deltaPred; //!< The power for the predictive speed reaction 279 | double a; //!< The maximum acceleration (in *m/s^2*) 280 | double b; //!< The maximum deceleration (in *m/s^2*) 281 | double vScale; //!< A scale factor to scale up or down the speed limit 282 | double ayMax; //!< Maximum lateral acceleration (in *m/s^2*) 283 | double vComfort; //!< Maximum personal comfortable velocity (in *m/s*) 284 | }; 285 | 286 | /*!< A class to store the parameters for follow components. */ 287 | struct ParameterFollowing { 288 | double timeHeadway; //!< The time headway the driver tries to reach during following (in *s*) 289 | double dsStopped; //!< The distance to the controlled target when stopped 290 | double thwMax; //!< The time headway the driver shall earliest react to follow (in *s*) 291 | }; 292 | 293 | /*!< A class to store the parameters of the ego vehicle. */ 294 | struct ParameterVehicle { 295 | Dimensions size; //!< Size of the ego vehicle 296 | Position pos; //!< The driver's position referenced to the center of the vehicle box defined by *size* 297 | }; 298 | 299 | /*!< A class to store the parameters of the steering components. */ 300 | struct ParameterSteering { 301 | double thw[NORP]; //!< The time headway of the reference points 302 | double dsMin[NORP]; //!< The minimim distance of the reference points 303 | double P[NORP]; //!< The P parameter of the controller 304 | double D[NORP]; //!< The D parameter of the controller 305 | }; 306 | 307 | /*!< A class to store the parameters of the stop components. */ 308 | struct ParameterStopping { 309 | double dsGap; //!< The gap between vehicle front and stop sign during a stop. 310 | double TMax; //!< Maximum time headway to react for stopping 311 | double dsMax; //!< Maximum distance to react for stopping 312 | double T; //!< A time headway to parameterize the dynamics of the approaching 313 | double tSign; //!< The time the driver stop at a stop sign 314 | double vStopped; //!< The velocity at which the driver expects the vehicle to have stopped. 315 | double pedalDuringStanding; //!< The pedal value, the driver controls during standing. 316 | }; 317 | 318 | /*!< A class to store the parameters of the lane change components. */ 319 | struct ParameterLaneChange { 320 | double bSafe; //!< A safe deceleration 321 | double aThreshold; //!< Acceleration threshold 322 | double politenessFactor; //!< Politeness factor 323 | double time; //!< Time to perform a lane change 324 | }; 325 | 326 | /*!< A class to store the inputs. */ 327 | struct Input { 328 | VehicleState vehicle; //!< The vehicle state. 329 | Horizon horizon; //!< The horizon. 330 | Signal signals[NOS]; //!< The signals. 331 | Lane lanes[NOL]; //!< The lanes. 332 | Target targets[NOT]; //!< The targets. 333 | }; 334 | 335 | /*!< A class to store all internal states. */ 336 | struct State { 337 | double simulationTime; //!< The actual simulation time 338 | Decisions decisions; //!< Decision states. 339 | Conscious conscious; //!< Conscious states. 340 | Subconscious subconscious; //!< Subconscious states. 341 | double aux[NOA]; //!< Auxiliary states. 342 | }; 343 | 344 | /*!< A class to store all memory states. */ 345 | struct Memory { 346 | MemoryVehicle vehicle; //!< The memory for vehicle states. 347 | double velocity; //!< The local maximum velocity. (in *m/s*) 348 | MemoryLateral lateral; //!< The memory for lateral control components. 349 | MemoryLaneChange laneChange; //!< The memory for lane change components. 350 | }; 351 | 352 | /*!< A class to store all parameters. */ 353 | struct Parameters { 354 | ParameterVehicle vehicle; //!< Parameters for velocity components. 355 | ParameterLaneChange laneChange; //!< Parameters for lane change components. 356 | ParameterStopping stop; //!< Parameters for stop components. 357 | ParameterVelocityControl velocity; //!< Parameters for velocity components. 358 | ParameterFollowing follow; //!< Parameters for follow components. 359 | ParameterSteering steering; //!< Parameters for steering components. 360 | }; 361 | 362 | 363 | /** 364 | * @brief The agent model interface. 365 | * The class implements the data structure of the agent model, consisting of input, state, memory and parameters. 366 | */ 367 | class Interface { 368 | 369 | public: 370 | 371 | typedef agent_model::Input Input; 372 | typedef agent_model::State State; 373 | typedef agent_model::Memory Memory; 374 | typedef agent_model::Parameters Parameters; 375 | 376 | 377 | protected: 378 | 379 | Input _input{}; //!< The input of the agent model. 380 | State _state{}; //!< The state of the agent model. 381 | Memory _memory{}; //!< The memory of the agent model. 382 | Parameters _param{}; //!< The parameters of the agent model. 383 | 384 | public: 385 | 386 | /** Default constructor */ 387 | Interface() = default; 388 | 389 | /** Default destructor */ 390 | virtual ~Interface() = default; 391 | 392 | 393 | /** 394 | * Returns the pointer for the _input structure of the model 395 | * @return The _input point 396 | */ 397 | Input *getInput() { 398 | return &_input; 399 | } 400 | 401 | /** 402 | * Returns the const pointer for the _input structure of the model 403 | * @return The const _input point 404 | */ 405 | const Input *getInput() const { 406 | return &_input; 407 | } 408 | 409 | 410 | /** 411 | * Returns the pointer for the _state structure of the model 412 | * @return The _state point 413 | */ 414 | State *getState() { 415 | return &_state; 416 | } 417 | 418 | /** 419 | * Returns the const pointer for the _state structure of the model 420 | * @return The const _state point 421 | */ 422 | const State *getState() const { 423 | return &_state; 424 | } 425 | 426 | 427 | /** 428 | * Returns the pointer for the _memory structure of the model 429 | * @return The _memory point 430 | */ 431 | Memory *getMemory() { 432 | return &_memory; 433 | } 434 | 435 | /** 436 | * Returns the const pointer for the _memory structure of the model 437 | * @return The const _memory point 438 | */ 439 | const Memory *getMemory() const { 440 | return &_memory; 441 | } 442 | 443 | 444 | /** 445 | * Returns the pointer for the _param structure of the model 446 | * @return The _param point 447 | */ 448 | Parameters *getParameters() { 449 | return &_param; 450 | } 451 | 452 | /** 453 | * Returns the const pointer for the _param structure of the model 454 | * @return The const _param point 455 | */ 456 | const Parameters *getParameters() const { 457 | return &_param; 458 | } 459 | 460 | 461 | 462 | }; 463 | 464 | } // namespace 465 | 466 | #endif // AGENT_MODEL_INTERFACE_H 467 | -------------------------------------------------------------------------------- /resources/doc/img/horizonSketch.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 19 | 21 | 29 | 35 | 36 | 44 | 50 | 51 | 63 | 72 | 78 | 79 | 88 | 94 | 95 | 103 | 109 | 110 | 122 | 134 | 144 | 156 | 168 | 172 | 181 | 187 | 188 | 196 | 202 | 203 | 217 | 226 | 232 | 233 | 242 | 248 | 249 | 258 | 264 | 265 | 274 | 280 | 281 | 289 | 295 | 296 | 308 | 309 | 329 | 331 | 332 | 334 | image/svg+xml 335 | 337 | 338 | 339 | 340 | 341 | 347 | 350 | 352 | 354 | 361 | 367 | 368 | 371 | y 382 | x 393 | 394 | 395 | 396 | 402 | 403 | 409 | 415 | 421 | 427 | 433 | 437 | 439 | 444 | yGlobal 459 | xGlobal 474 | 475 | 476 | 482 | lane width 493 | 499 | lane width 510 | reference line 524 | 530 | 536 | 542 | 548 | 556 | 564 | 572 | 580 | s1 595 | s2 610 | s3 625 | si 640 | 646 | 652 | 658 | lane width 669 | 670 | 676 | 680 | 688 | 689 | 693 | 699 | 705 | 713 | φ 724 | 725 | κ 736 | x,y 747 | 748 | 749 | -------------------------------------------------------------------------------- /src/AgentModel.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Institute for Automotive Engineering (ika), RWTH Aachen University. All rights reserved. 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | // 21 | // Created by Jens Klimke on 2019-03-23. 22 | // Contributors: 23 | // 24 | // AgentModel.cpp 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "AgentModel.h" 32 | #include "model_collection.h" 33 | 34 | #ifndef NEG_INFINITY 35 | #define NEG_INFINITY (-1.0 * INFINITY) 36 | #endif 37 | 38 | #if WITH_INJECTION 39 | #include 40 | #define APPLY(PNTR) { InjectionInterface::applyAll(PNTR); InjectionInterface::resetAll(PNTR); } 41 | #else 42 | #define APPLY(PNTR) 43 | #endif 44 | 45 | static const int LOW_SPEED = 5.0; 46 | static const double AM_CLOSE_TO_ONE = 0.999999999; 47 | 48 | 49 | void AgentModel::init() { 50 | 51 | // unset distance counter 52 | _memory.vehicle.s = 0.0; 53 | 54 | // unset lane change 55 | _memory.laneChange.switchLane = 0; 56 | 57 | // unset lateral control memory 58 | _memory.lateral.startDistance = INFINITY; 59 | _memory.lateral.startTime = INFINITY; 60 | 61 | // unset velocity 62 | _memory.velocity = INFINITY; 63 | 64 | // set lane change 65 | _memory.laneChange.startTime = INFINITY; 66 | 67 | // init horizon 68 | _stop_horizon.init(_input.vehicle.s); 69 | _vel_horizon.init(_input.vehicle.s, 401); 70 | _filter.init(10); 71 | 72 | // init lateral control 73 | _lateral_offset_interval.reset(); 74 | _lateral_offset_interval.setScale(0.0); 75 | 76 | // init lane change process 77 | _lane_change_process_interval.reset(); 78 | _lane_change_process_interval.setDelta(0.3); 79 | _lane_change_process_interval.setScale(0.0); 80 | 81 | // init road priorities 82 | _state.conscious.stop.priority = false; 83 | _state.conscious.stop.give_way = false; 84 | 85 | } 86 | 87 | 88 | void AgentModel::step(double simulationTime) { 89 | 90 | // TODO: Think about using fixed horizon points. This would avoid changing curvature. 91 | 92 | // apply injection for parameters and inputs 93 | APPLY(&this->_param) 94 | APPLY(&this->_input) 95 | APPLY(&this->_memory) 96 | 97 | // update internal horizons 98 | _stop_horizon.update(_input.vehicle.s, simulationTime); 99 | _vel_horizon.update(_input.vehicle.s); 100 | _lateral_offset_interval.update(_input.vehicle.s, simulationTime); 101 | _lane_change_process_interval.update(_input.vehicle.s, simulationTime); 102 | 103 | // set time 104 | _state.simulationTime = simulationTime; 105 | 106 | // decisions 107 | decisionLaneChange(); // open 108 | decisionProcessStop(); // done: Test 10.1, 10.2 109 | decisionLateralOffset(); // done: no implementation yet 110 | 111 | // apply injection for decision 112 | APPLY(&this->_state.decisions) 113 | 114 | // conscious calculation 115 | consciousLaneChange(); // open 116 | consciousVelocity(); // done: Test 9.1, 9.2, 9.3 117 | consciousStop(); // done: Test 3.3b, 3.3c 118 | consciousFollow(); // done: Test 8.1, 8.2, 8.3 119 | consciousLateralOffset(); // done: Test 7.3 120 | consciousReferencePoints(); // done: Test 7.1, 7.2, 7.3, 7.4 121 | 122 | // apply injection for conscious states 123 | APPLY(&this->_state.conscious) 124 | 125 | // calculate speed reaction 126 | double rSpeed = subconsciousSpeed(); // done: Test 2.1, 2.2, 2.3, 3.1, 3.2, 3.6 127 | double rStop = subconsciousStop(); // done: Test 3.3, 3.6 128 | double rFollow = subconsciousFollow(); // done: Test 3.4, 3.5, 3.6 129 | double pedal = subconsciousStartStop(); // done: Test 1.1, 1.2 130 | double kappa = subconsciousLateralControl(); // done: Test 6.1, 6.2, 6.3, 6.4 131 | // calculate resulting acceleration 132 | double aRes = _param.velocity.a * (1.0 - rSpeed - rStop - rFollow); 133 | 134 | // set desired values 135 | _state.subconscious.a = std::min(std::max(-10.0, aRes), 10.0); // done: Test 1.3 136 | _state.subconscious.kappa = kappa; // done: Test 4.1, 4.2 137 | _state.subconscious.pedal = pedal; // done: Test 1.4 138 | _state.subconscious.steering = INFINITY; 139 | 140 | // apply injection for sub-conscious states 141 | APPLY(&this->_state.subconscious) 142 | 143 | // save values to memory 144 | _memory.vehicle.s = _input.vehicle.s; 145 | 146 | } 147 | 148 | 149 | void AgentModel::decisionProcessStop() { 150 | 151 | // unset decision 152 | _state.decisions.signal.id = std::numeric_limits::max(); 153 | _state.decisions.signal.position = INFINITY; 154 | _state.decisions.signal.standingTime = INFINITY; 155 | 156 | _state.decisions.target.id = std::numeric_limits::max(); 157 | _state.decisions.target.position = INFINITY; 158 | _state.decisions.target.standingTime = INFINITY; 159 | 160 | _state.decisions.destination.id = std::numeric_limits::max(); 161 | _state.decisions.destination.position = INFINITY; 162 | _state.decisions.destination.standingTime = INFINITY; 163 | 164 | _state.decisions.lane.id = std::numeric_limits::max(); 165 | _state.decisions.lane.position = INFINITY; 166 | _state.decisions.lane.standingTime = INFINITY; 167 | 168 | // add stop point because of destination point 169 | if (_input.horizon.destinationPoint > 0) 170 | { 171 | _state.decisions.destination.id = 3; 172 | _state.decisions.destination.position = _input.vehicle.s + _input.horizon.destinationPoint; 173 | _state.decisions.destination.standingTime = INFINITY; 174 | } 175 | 176 | // add stop point because of end of route 177 | agent_model::Lane* ego = nullptr; 178 | for (auto &lane : _input.lanes) { 179 | if (lane.id == 0) { 180 | ego = &lane; 181 | } 182 | } 183 | _state.decisions.lane.id = 4; 184 | _state.decisions.lane.position = _input.vehicle.s + ego->route; 185 | // only apply standing time when hard lane change required at end of route 186 | if (ego->lane_change == 2) { 187 | _state.decisions.lane.standingTime = INFINITY; 188 | } 189 | else { 190 | _state.decisions.lane.standingTime = 0; 191 | } 192 | 193 | // not yet decided about to stop or drive 194 | bool stop = false; 195 | bool drive = false; 196 | bool found_signal = false; 197 | 198 | // iterate over all signals and mark ds of the next relavant signal 199 | double ds_rel_tls = INFINITY; 200 | double ds_rel_sgn = INFINITY; 201 | agent_model::Signal* rel; 202 | agent_model::Signal* rel_tls; 203 | agent_model::Signal* rel_sgn; 204 | 205 | for(auto &e : _input.signals) 206 | { 207 | if (e.type == agent_model::SignalType::SIGNAL_TLS && 208 | e.ds >= 0 && e.ds < ds_rel_tls) 209 | { 210 | ds_rel_tls = e.ds; 211 | rel_tls = &e; 212 | found_signal = true; 213 | } 214 | if ((e.type == agent_model::SignalType::SIGNAL_YIELD || 215 | e.type == agent_model::SignalType::SIGNAL_PRIORITY || 216 | e.type == agent_model::SignalType::SIGNAL_STOP) && 217 | e.sign_is_in_use && 218 | !e.subsignal && 219 | e.ds >= 0 && e.ds < ds_rel_sgn) 220 | { 221 | ds_rel_sgn = e.ds; 222 | rel_sgn = &e; 223 | found_signal = true; 224 | } 225 | } 226 | 227 | if(found_signal) 228 | { 229 | // take closest signal 230 | // (take traffic light even if 10 meters behind potential sign) 231 | if (ds_rel_tls <= ds_rel_sgn + 10) { 232 | rel = rel_tls; 233 | } else { 234 | rel = rel_sgn; 235 | } 236 | // calculate net distance 237 | auto ds = rel->ds - _param.stop.dsGap + _param.vehicle.pos.x - _param.vehicle.size.length * 0.5; 238 | 239 | // trafficlight 240 | if (rel->type == agent_model::SignalType::SIGNAL_TLS) 241 | { 242 | // case red trafficlight 243 | if (rel->color == agent_model::TrafficLightColor::COLOR_RED) { 244 | 245 | _state.conscious.stop.give_way = true; 246 | 247 | // set stop point for INFINITY ( = until removed) 248 | _state.decisions.signal.id = 1; 249 | _state.decisions.signal.position = _input.vehicle.s + ds; 250 | _state.decisions.signal.standingTime = INFINITY; 251 | 252 | stop = true; 253 | return; 254 | } 255 | 256 | // case green trafficlight 257 | else if (rel->color == agent_model::TrafficLightColor::COLOR_GREEN) { 258 | _state.conscious.stop.priority = true; 259 | 260 | // "remove" stop point (by setting standing standingTime = 0) 261 | _state.decisions.signal.id = 1; 262 | _state.decisions.signal.position = _input.vehicle.s + ds; 263 | _state.decisions.signal.standingTime = 0; 264 | 265 | // drive if green and straight 266 | if (_input.vehicle.maneuver == agent_model::Maneuver::STRAIGHT) 267 | drive = true; 268 | 269 | // drive if green-left-arrow and left turn 270 | if (rel->icon == agent_model::TrafficLightIcon::ICON_ARROW_LEFT 271 | && _input.vehicle.maneuver == agent_model::Maneuver::TURN_LEFT) 272 | drive = true; 273 | } 274 | } 275 | 276 | // signal 277 | if (rel->type != agent_model::SignalType::SIGNAL_TLS) 278 | { 279 | // case stop signal 280 | if (rel->type == agent_model::SignalType::SIGNAL_STOP) { 281 | _state.conscious.stop.give_way = true; 282 | stop = true; 283 | } 284 | 285 | // case yield signal 286 | else if (rel->type == agent_model::SignalType::SIGNAL_YIELD) { 287 | _state.conscious.stop.give_way = true; 288 | } 289 | 290 | // case priority signal 291 | else if (rel->type == agent_model::SignalType::SIGNAL_PRIORITY) { 292 | _state.conscious.stop.priority = true; 293 | } 294 | } 295 | } 296 | 297 | // add stop point because of signal 298 | if (stop) 299 | { 300 | _state.decisions.signal.id = 1; 301 | _state.decisions.signal.position = _input.vehicle.s + rel->ds; 302 | _state.decisions.signal.standingTime = _param.stop.tSign; 303 | } 304 | 305 | // if not yet decided to drive or stop -> consider targets 306 | if (!drive && !stop) { 307 | 308 | // ignore if not approaching intersection 309 | if (_input.vehicle.dsIntersection == INFINITY) 310 | return; 311 | 312 | // ignore if on priority lane and driving straight (or right - for now) 313 | if (_state.conscious.stop.priority && 314 | (_input.vehicle.maneuver == agent_model::Maneuver::STRAIGHT || 315 | _input.vehicle.maneuver == agent_model::Maneuver::TURN_RIGHT)) 316 | return; 317 | 318 | // process all relevant targets 319 | for (auto &t : _input.targets) 320 | { 321 | // ignore unset targets 322 | if (t.id == 0) continue; 323 | 324 | // ignore targets not in junction area 325 | if (t.position == agent_model::TARGET_NOT_RELEVANT) 326 | continue; 327 | 328 | // ignore targets on path 329 | if (t.position == agent_model::TARGET_ON_PATH) 330 | continue; 331 | 332 | // ego is on priority 333 | if (_state.conscious.stop.priority) 334 | { 335 | // normaly do not stop when on priority lane 336 | if (_input.vehicle.maneuver == agent_model::Maneuver::STRAIGHT && _input.vehicle.maneuver == agent_model::Maneuver::TURN_RIGHT) 337 | { 338 | continue; 339 | } 340 | // special case for left turn 341 | else if (_input.vehicle.maneuver == agent_model::Maneuver::TURN_LEFT) 342 | { 343 | if (t.position == agent_model::TARGET_ON_OPPOSITE) 344 | { 345 | stop = true; 346 | } 347 | continue; 348 | } 349 | } 350 | 351 | // ego is on give way lane 352 | if (_state.conscious.stop.give_way) 353 | { 354 | // stop for target already on intersection 355 | if (t.position == agent_model::TargetPosition::TARGET_ON_INTERSECTION) 356 | { 357 | stop = true; 358 | continue; 359 | } 360 | 361 | // stop if target prority not set (passive behavior) 362 | if (t.priority == agent_model::TargetPriority::TARGET_PRIORITY_NOT_SET) 363 | { 364 | stop = true; 365 | continue; 366 | } 367 | 368 | // if target has to give way as well (first come, first drive) 369 | if (t.priority == agent_model::TargetPriority::TARGET_ON_GIVE_WAY_LANE) 370 | { 371 | // special cases 372 | 373 | // opposite target and left turn -> stop 374 | if (t.position == agent_model::TARGET_ON_OPPOSITE && _input.vehicle.maneuver == agent_model::Maneuver::TURN_LEFT) 375 | { 376 | if (_input.vehicle.maneuver == agent_model::Maneuver::TURN_LEFT) 377 | stop = true; 378 | continue; 379 | } 380 | 381 | // opposite target and not left turn -> continue 382 | if (t.position == agent_model::TARGET_ON_OPPOSITE && _input.vehicle.maneuver != agent_model::Maneuver::TURN_LEFT) 383 | { 384 | continue; 385 | } 386 | 387 | // right target and right turn -> continue 388 | if (t.position == agent_model::TARGET_ON_RIGHT && _input.vehicle.maneuver == agent_model::Maneuver::TURN_RIGHT) 389 | { 390 | continue; 391 | } 392 | 393 | // if no special case, check if ego reaches junction earlier 394 | if (_input.vehicle.dsIntersection / _input.vehicle.v < t.dsIntersection / t.v) 395 | { 396 | continue; 397 | } 398 | // if target vehicle approaches intersection earlier 399 | else 400 | { 401 | stop = true; 402 | continue; 403 | } 404 | } 405 | 406 | // stop for target on priority lane 407 | if (t.priority == agent_model::TargetPriority::TARGET_ON_PRIORITY_LANE) 408 | { 409 | stop = true; 410 | continue; 411 | } 412 | } 413 | 414 | // ego is not on priority and give way lane (right before left) 415 | if (!_state.conscious.stop.priority && !_state.conscious.stop.give_way) 416 | { 417 | if (t.position == agent_model::TARGET_ON_INTERSECTION) 418 | { 419 | stop = true; 420 | continue; 421 | } 422 | if (t.position == agent_model::TARGET_NOT_RELEVANT) 423 | { 424 | continue; 425 | } 426 | if (t.position == agent_model::TARGET_ON_OPPOSITE && 427 | _input.vehicle.maneuver == agent_model::Maneuver::TURN_LEFT) 428 | { 429 | stop= true; 430 | continue; 431 | } 432 | if (t.position == agent_model::TARGET_ON_RIGHT) 433 | { 434 | stop= true; 435 | continue; 436 | } 437 | if (t.position == agent_model::TARGET_ON_LEFT) 438 | { 439 | continue; 440 | } 441 | } 442 | } 443 | 444 | // add stop point because of target 445 | if (stop) 446 | { 447 | // try to stop 10m before intersection or take ds of the signal 448 | double ds_stop; 449 | if (std::isinf(ds_rel_sgn)) 450 | ds_stop = std::max(0.0, _input.vehicle.dsIntersection - 10); 451 | else 452 | ds_stop = std::max(0.0, rel->ds); 453 | _state.decisions.target.id = 2; 454 | _state.decisions.target.position = _input.vehicle.s + ds_stop; 455 | _state.decisions.target.standingTime = _param.stop.tSign; 456 | } 457 | } 458 | } 459 | 460 | void AgentModel::decisionLaneChange() { 461 | 462 | // check for route-based lane changes 463 | _state.decisions.laneChangeInt = 0; // intention 464 | _state.decisions.laneChangeDec = 0; // decision 465 | 466 | // determine velocity dependend required length (assumption: v is constant) 467 | double safety_factor = 1.0; 468 | double length = _param.laneChange.time * _input.vehicle.v * safety_factor; 469 | 470 | // get current lane pointers 471 | agent_model::Lane* ego = nullptr; 472 | agent_model::Lane* left = nullptr; 473 | agent_model::Lane* right = nullptr; 474 | for (auto &lane : _input.lanes) { 475 | 476 | if (lane.id == 0) { 477 | ego = &lane; 478 | } 479 | if (lane.id == 1) { 480 | left = &lane; 481 | } 482 | if (lane.id == -1) { 483 | right = &lane; 484 | } 485 | } 486 | 487 | // skip if ego lane not found 488 | if (!ego) return; 489 | 490 | int lane_change_status = ego->lane_change; 491 | 492 | // skip if lane_change not desired 493 | if (lane_change_status < 1) return; 494 | 495 | // if left lane accessible and route longer 496 | if (left && left->access == agent_model::ACC_ACCESSIBLE && left->route >= ego->route) { 497 | _state.decisions.laneChangeInt = 1; 498 | } 499 | // if right lane accessible and route longer 500 | else if (right && right->access == agent_model::ACC_ACCESSIBLE && right->route >= ego->route) { 501 | _state.decisions.laneChangeInt = -1; 502 | } 503 | 504 | // if lane_change intended 505 | if (_state.decisions.laneChangeInt != 0) { 506 | 507 | _state.decisions.laneChangeDec = _state.decisions.laneChangeInt; 508 | 509 | // allow later lane change if still enough route available 510 | if (lane_change_status == 2 && ego->route > length) { 511 | lane_change_status = 1; 512 | } 513 | 514 | // skip lane change if route to short and later possible (status 1) 515 | if (ego->route < length && lane_change_status == 1) { 516 | _state.decisions.laneChangeDec = 0; 517 | return; 518 | } 519 | 520 | // consider targets when status == 1 521 | if (lane_change_status == 1) { 522 | 523 | // only within critical thw 524 | double thw_crit = 1; 525 | double safety_boundary = 5; 526 | 527 | for (auto &target : _input.targets) { 528 | 529 | // get target 530 | auto tar = ⌖ 531 | 532 | // skip target if not on lane 533 | if (tar->lane != _state.decisions.laneChangeInt) continue; 534 | 535 | // caculate dv and s_crit 536 | double dv = _input.vehicle.v - tar->v; 537 | double s_crit = thw_crit * dv; 538 | 539 | // skip lane change if too close 540 | if (abs(tar->ds) < safety_boundary) { 541 | _state.decisions.laneChangeDec = 0; 542 | break; 543 | } 544 | 545 | // if ego vehicle is faster - target in front is critical 546 | if (dv > 0 && tar->ds > safety_boundary && tar->ds < s_crit) { 547 | _state.decisions.laneChangeDec = 0; 548 | break; 549 | } 550 | // if ego vehicle is slower - target in back is critical 551 | if (dv < 0 && tar->ds < -safety_boundary && tar->ds > s_crit) { 552 | _state.decisions.laneChangeDec = 0; 553 | break; 554 | } 555 | } 556 | } 557 | } 558 | 559 | return; 560 | 561 | // TODO: do not consider MOBIL model right now 562 | 563 | // check positions 564 | double dsLF = INFINITY; 565 | double vLF = 0.0; 566 | double dsLB = INFINITY; 567 | double vLB = 0.0; 568 | double dsRF = INFINITY; 569 | double vRF = 0.0; 570 | double dsRB = INFINITY; 571 | double vRB = 0.0; 572 | double dsEF = INFINITY; 573 | double vEF = 0.0; 574 | double dsEB = INFINITY; 575 | double vEB = 0.0; 576 | 577 | // iterate over targets 578 | for (auto &target : _input.targets) { 579 | 580 | // get target 581 | auto tar = ⌖ 582 | 583 | if (tar->lane == 0 && tar->ds >= 0.0 && tar->ds < dsEF) { 584 | dsEF = tar->ds; 585 | vEF = tar->v; 586 | } else if (tar->lane == 0 && tar->ds < 0.0 && tar->ds > dsEB) { 587 | dsEB = tar->ds; 588 | vEB = tar->v; 589 | } else if (tar->lane == 1 && tar->ds >= 0.0 && tar->ds < dsLF) { 590 | dsLF = tar->ds; 591 | vLF = tar->v; 592 | } else if (tar->lane == 1 && tar->ds < 0.0 && tar->ds > dsLB) { 593 | dsLB = tar->ds; 594 | vLB = tar->v; 595 | } else if (tar->lane == -1 && tar->ds >= 0.0 && tar->ds < dsRF) { 596 | dsRF = tar->ds; 597 | vRF = tar->v; 598 | } else if (tar->lane == -1 && tar->ds < 0.0 && tar->ds > dsRB) { 599 | dsRB = tar->ds; 600 | vRB = tar->v; 601 | } 602 | } 603 | 604 | // get values 605 | double v0 = _param.velocity.vComfort; 606 | double s0 = _param.follow.dsStopped; 607 | double T = _param.follow.timeHeadway; 608 | double v = _input.vehicle.v; 609 | double a = _param.velocity.a; 610 | double b = -_param.velocity.b; 611 | double bSafe = _param.laneChange.bSafe; 612 | double aThr = _param.laneChange.aThreshold; 613 | double p = _param.laneChange.politenessFactor; 614 | 615 | // instantiate result 616 | double sR, iR; 617 | double sL, iL; 618 | 619 | // calculate threshold dependent on following reaction TODO: 620 | // auto aThr = -std::min(0.0, agent_model::IDMOriginal(v, v0, v * _param.follow.thwMax, v - vEF, T, s0, a, b)); 621 | 622 | // calculate lane change 623 | agent_model::MOBILOriginal(sR, iR, v, v0, T, s0, a, b, dsEF, vEF, dsRF, vRF, -dsEB, vEB, -dsRB, vRB, bSafe, aThr, p); 624 | agent_model::MOBILOriginal(sL, iL, v, v0, T, s0, a, b, dsEF, vEF, dsLF, vLF, -dsEB, vEB, -dsLB, vLB, bSafe, aThr, p); 625 | 626 | // TODO: only decide lane change, when lane is available 627 | if(sL > 0.999 && iL >= -0.5) 628 | _state.decisions.laneChangeDec = 1; 629 | else if(sR > 0.999 && iR >= -0.5) 630 | _state.decisions.laneChangeDec = -1; 631 | else 632 | _state.decisions.laneChangeDec = 0; 633 | } 634 | 635 | 636 | void AgentModel::decisionLateralOffset() { 637 | 638 | _state.decisions.lateral.distance = INFINITY; 639 | _state.decisions.lateral.time = INFINITY; 640 | _state.decisions.lateral.value = 0.0; 641 | 642 | } 643 | 644 | 645 | void AgentModel::consciousVelocity() { 646 | 647 | using namespace std; 648 | 649 | // set max comfortable speed 650 | double vComf = _param.velocity.vComfort; 651 | _vel_horizon.setMaxVelocity(_param.velocity.vComfort); 652 | 653 | // some variables 654 | double dsLoc = -1.0 * INFINITY; 655 | double vLoc = INFINITY; 656 | 657 | // start for interval 658 | double s0 = _input.vehicle.s; 659 | double v0 = INFINITY; 660 | 661 | // unset the speed rules 662 | _vel_horizon.resetSpeedRule(); 663 | 664 | // find last rule 665 | for (const auto &e : _input.signals) { 666 | 667 | // get speed limits 668 | if (e.type != agent_model::SignalType::SIGNAL_SPEED_LIMIT) 669 | continue; 670 | 671 | // speed 672 | auto v = e.value < 0 ? INFINITY : (double) e.value / 3.6; 673 | 674 | // check if closest rule 675 | if (e.ds < 0.0 && dsLoc < e.ds) { 676 | vLoc = v; 677 | dsLoc = e.ds; 678 | } 679 | 680 | // calculate end of interval 681 | double s1 = _input.vehicle.s + e.ds; 682 | 683 | // add rule to horizon 684 | if(s1 > s0) 685 | _vel_horizon.updateSpeedRuleInInterval(s0, s1, v0); 686 | 687 | s0 = s1; 688 | v0 = v; 689 | 690 | } 691 | 692 | // add rule to horizon 693 | _vel_horizon.updateSpeedRuleInInterval(s0, INFINITY, v0); 694 | 695 | // save local speed limit to state 696 | _memory.velocity = isinf(vLoc) ? _memory.velocity : vLoc; 697 | double vRule = _memory.velocity; 698 | 699 | // calculate local curve speed 700 | double kappaCurrent = isinf(_input.horizon.ds[1]) 701 | ? 0.0 : agent_model::interpolate(0.0, _input.horizon.ds, _input.horizon.kappa, agent_model::NOH); 702 | double vCurve = max(0.0, sqrt(std::abs(_param.velocity.ayMax / kappaCurrent))); 703 | 704 | // iterate over horizon points 705 | for(unsigned int i = 0; i < agent_model::NOH; ++i) { 706 | 707 | // get position and speed 708 | auto s = _input.vehicle.s + _input.horizon.ds[i]; 709 | auto v = max(0.0, sqrt(std::abs(_param.velocity.ayMax / _input.horizon.kappa[i]))); 710 | 711 | // set speed 712 | _vel_horizon.updateContinuousPoint(s, v); 713 | 714 | } 715 | 716 | // sets the local speed 717 | _state.conscious.velocity.local = min(min(vComf, vRule), vCurve); 718 | 719 | // calculate interval 720 | double sI0 = _input.vehicle.s; 721 | double sI1 = sI0 + std::max(1.0, _input.vehicle.v * _param.velocity.thwMax); 722 | 723 | // calculate mean predictive velocity 724 | _state.conscious.velocity.prediction = _vel_horizon.mean(sI0, sI1, _param.velocity.deltaPred); 725 | 726 | } 727 | 728 | 729 | void AgentModel::consciousStop() { 730 | 731 | using namespace std; 732 | 733 | // add new signals 734 | agent_model::DecisionStopping signal = _state.decisions.signal; 735 | agent_model::DecisionStopping target = _state.decisions.target; 736 | agent_model::DecisionStopping destination = _state.decisions.destination; 737 | agent_model::DecisionStopping lane = _state.decisions.lane; 738 | 739 | // check position and add stop point 740 | if(!std::isinf(signal.position)) 741 | _stop_horizon.addStopPoint(signal.id, signal.position, signal.standingTime); 742 | 743 | if(!std::isinf(target.position)) 744 | _stop_horizon.addStopPoint(target.id, target.position, target.standingTime); 745 | 746 | if(!std::isinf(destination.position)) 747 | _stop_horizon.addStopPoint(destination.id, destination.position, destination.standingTime); 748 | 749 | if(!std::isinf(lane.position)) 750 | _stop_horizon.addStopPoint(lane.id, lane.position, lane.standingTime); 751 | 752 | // get stop 753 | auto stop = _stop_horizon.getNextStop(); 754 | auto standing = false; 755 | 756 | // check standing 757 | if(!isinf(stop.ds)) { 758 | 759 | // is standing? 760 | standing = _input.vehicle.v < _param.stop.vStopped && stop.ds <= 0.5; 761 | 762 | // mark as stopped, TODO: mark all of them 763 | if(standing) 764 | _stop_horizon.stopped(stop.id, _state.simulationTime); 765 | 766 | } 767 | 768 | // default values 769 | _state.conscious.stop.ds = stop.ds; 770 | _state.conscious.stop.dsMax = stop.interval; 771 | _state.conscious.stop.standing = standing; 772 | 773 | } 774 | 775 | 776 | void AgentModel::consciousFollow() { 777 | 778 | // get pointer to targets 779 | auto t = _input.targets; 780 | 781 | // calculate net distance for following targets 782 | unsigned long im = agent_model::NOT; // target on ego lane 783 | unsigned long im_loi = agent_model::NOT; // target on neigbouring lane 784 | 785 | int insert_idx; 786 | for (unsigned long i = 0; i < agent_model::NOT; ++i) { 787 | 788 | // ignore non-relevant targets (, ds < 0, other lane) 789 | if (t[i].id == 0 || std::isinf(t[i].ds) || t[i].ds < 0.0) continue; 790 | 791 | 792 | if (t[i].lane == 0) { 793 | 794 | // check if distance is smaller 795 | if (im == agent_model::NOT || t[im].ds > t[i].ds) 796 | im = i; 797 | } 798 | 799 | if (t[i].lane == _state.decisions.laneChangeInt) 800 | { 801 | // check if distance is smaller 802 | if (im_loi == agent_model::NOT || t[im_loi].ds > t[i].ds) 803 | im_loi = i; 804 | } 805 | } 806 | 807 | // instantiate distance, velocity, and factor 808 | double ds = INFINITY, v = 0.0; 809 | double factor = 1 - _lane_change_process_interval.getFactor(); 810 | 811 | // closest ego lane target 812 | if (im != agent_model::NOT) { 813 | ds = t[im].ds - t[im].size.length * 0.5 - _param.vehicle.size.length * 0.5 + _param.vehicle.pos.x; 814 | v = t[im].v; 815 | } 816 | 817 | // save distance and velocity 818 | _state.conscious.follow.targets[0].distance = ds; 819 | _state.conscious.follow.targets[0].velocity = v; 820 | _state.conscious.follow.targets[0].factor = factor; 821 | 822 | // calculate if vehicle stands behind target vehicle 823 | bool standing = _input.vehicle.v < 1e-3 && v < 0.5 && ds <= _param.follow.dsStopped + 1e-2; 824 | _state.conscious.follow.standing = standing; 825 | 826 | // reset distance, velocity, and factor for loi target 827 | ds = INFINITY, v = 0.0; 828 | factor = (_lane_change_process_interval.getFactor() > 0); 829 | 830 | // closest neigbouring lane target 831 | if (im_loi != agent_model::NOT) { 832 | ds = t[im_loi].ds - t[im_loi].size.length * 0.5 - _param.vehicle.size.length * 0.5 + _param.vehicle.pos.x; 833 | v = t[im_loi].v; 834 | } 835 | 836 | // save distance and velocity 837 | _state.conscious.follow.targets[1].distance = ds; 838 | _state.conscious.follow.targets[1].velocity = v; 839 | _state.conscious.follow.targets[1].factor = factor; 840 | } 841 | 842 | 843 | void AgentModel::consciousLaneChange() { 844 | 845 | if(_state.decisions.laneChangeDec != 0 && !_lane_change_process_interval.isSet()) { 846 | 847 | // start process 848 | _lane_change_process_interval.setTimeInterval(_param.laneChange.time); 849 | _lane_change_process_interval.setScale(1.0 * _state.decisions.laneChangeDec); 850 | 851 | } 852 | 853 | // calculate factor and limit 854 | _memory.laneChange.switchLane = 0; 855 | auto factor = _lane_change_process_interval.getScaledFactor(); 856 | factor = std::max(-1.0, std::min(1.0, factor)); 857 | 858 | // lane change has ended 859 | if(_lane_change_process_interval.getFactor() >= AM_CLOSE_TO_ONE) { 860 | 861 | // set lane change flag 862 | _memory.laneChange.switchLane = factor < 0.0 ? -1 : 1; 863 | 864 | // reset process 865 | _lane_change_process_interval.reset(); 866 | _lane_change_process_interval.setScale(0.0); 867 | } 868 | 869 | // set factor, TODO: multi-lane change 870 | _state.conscious.lateral.paths[0].factor = (1.0 - std::abs(factor)); 871 | _state.conscious.lateral.paths[1].factor = std::max(0.0, -factor); 872 | _state.conscious.lateral.paths[2].factor = std::max(0.0, factor); 873 | 874 | } 875 | 876 | 877 | void AgentModel::consciousLateralOffset() { 878 | 879 | using namespace std; 880 | 881 | 882 | // check decision and set 883 | if(!isinf(_state.decisions.lateral.distance) || !isinf(_state.decisions.lateral.time)) { 884 | 885 | // save current offset 886 | _memory.lateral.offset = _input.vehicle.d; 887 | 888 | // reset 889 | _lateral_offset_interval.reset(); 890 | 891 | // set intervals 892 | _lateral_offset_interval.setEndPosition(_input.vehicle.s + _state.decisions.lateral.distance); 893 | _lateral_offset_interval.setTimeInterval(_state.decisions.lateral.time); 894 | _lateral_offset_interval.setScale(_state.decisions.lateral.value); 895 | 896 | } 897 | 898 | // calculate factor and get scale 899 | auto factor = _lateral_offset_interval.getFactor(); 900 | auto offset = _lateral_offset_interval.getScale(); 901 | 902 | // set state 903 | _state.conscious.lateral.paths[0].offset = _memory.lateral.offset * (1.0 - factor) + offset * factor; 904 | 905 | } 906 | 907 | 908 | void AgentModel::consciousReferencePoints() { 909 | 910 | using namespace std; 911 | 912 | // get speed and offset 913 | auto v = _input.vehicle.v; 914 | 915 | // calculate reference points 916 | for (size_t i = 0; i < agent_model::NORP; ++i) { 917 | 918 | // get grid point 919 | double s = std::max(_param.steering.dsMin[i], v * _param.steering.thw[i]); 920 | 921 | // no interpolation possible (e.g. horizon ended) -> set horizon straight ahead 922 | if (isinf(_input.horizon.ds[1])) { 923 | 924 | // set all paths equal 925 | _state.conscious.lateral.paths[0].refPoints[i] = {s, 0.0, 0.0, 0.0}; // ego 926 | _state.conscious.lateral.paths[1].refPoints[i] = {s, 0.0, 0.0, 0.0}; // left 927 | _state.conscious.lateral.paths[2].refPoints[i] = {s, 0.0, 0.0, 0.0}; // right 928 | 929 | // next loop step 930 | continue; 931 | } 932 | 933 | // get lane offsets 934 | auto offR = -agent_model::interpolate(s, _input.horizon.ds, _input.horizon.rightLaneOffset, agent_model::NOH, 2); 935 | auto offL = agent_model::interpolate(s, _input.horizon.ds, _input.horizon.leftLaneOffset, agent_model::NOH, 2); 936 | 937 | // interpolate angle and do the rotation math 938 | auto psi = agent_model::interpolate(s, _input.horizon.ds, _input.horizon.psi, agent_model::NOH, 2); 939 | double sn = sin(psi), cn = cos(psi); 940 | 941 | // get offset 942 | auto off = _state.conscious.lateral.paths[0].offset; 943 | 944 | // interpolate x and y 945 | auto x = agent_model::interpolate(s, _input.horizon.ds, _input.horizon.x, agent_model::NOH, 2) + _param.vehicle.pos.x - sn * off; 946 | auto y = agent_model::interpolate(s, _input.horizon.ds, _input.horizon.y, agent_model::NOH, 2) + _param.vehicle.pos.y + cn * off; 947 | 948 | // calculate reference points 949 | // TODO: calculate dx, dy (independent on change in speed => dv/dt = 0 to avoid influence of speed change in reference points) 950 | auto re = agent_model::DynamicPosition{x, y, 0.0, 0.0}; 951 | auto rr = agent_model::DynamicPosition{x - sn * offR, y + cn * offR, 0.0, 0.0}; 952 | auto rl = agent_model::DynamicPosition{x - sn * offL, y + cn * offL, 0.0, 0.0}; 953 | 954 | // write data 955 | _state.conscious.lateral.paths[0].refPoints[i] = re; // ego 956 | _state.conscious.lateral.paths[1].refPoints[i] = rr; // right 957 | _state.conscious.lateral.paths[2].refPoints[i] = rl; // left 958 | } 959 | } 960 | 961 | 962 | double AgentModel::subconsciousLateralControl() { 963 | 964 | // initialize reaction 965 | double reaction = 0.0; 966 | 967 | // reset last aux entry 968 | _state.aux[31] = 0.0; 969 | 970 | // iterate over reference points 971 | for (unsigned int i = 0; i < agent_model::NORP; ++i) { 972 | 973 | // parameters 974 | auto P = _param.steering.P[i]; 975 | auto D = _param.steering.D[i]; 976 | 977 | // iterate over control paths 978 | for (size_t j = 0; j < agent_model::NOCP; ++j) { 979 | 980 | // get reference points with derivatives 981 | auto x = _state.conscious.lateral.paths[j].refPoints[i].x; 982 | auto y = _state.conscious.lateral.paths[j].refPoints[i].y; 983 | auto dx = _state.conscious.lateral.paths[j].refPoints[i].dx; 984 | auto dy = _state.conscious.lateral.paths[j].refPoints[i].dy; 985 | 986 | // generate index for aux vector (theta and dtheta/dt are stored in aux) 987 | auto idx = 2 * (i * agent_model::NOCP + j); 988 | 989 | // calculate salvucci and gray and apply factor 990 | reaction += _state.conscious.lateral.paths[j].factor 991 | * agent_model::SalvucciAndGray(x, y, dx, dy, P, D, _state.aux[idx + 0], _state.aux[idx + 1]); 992 | 993 | // save factored value 994 | _state.aux[31] += _state.conscious.lateral.paths[j].factor * _state.aux[idx]; 995 | 996 | } 997 | } 998 | 999 | // reset temporary theta and dTheta 1000 | if (_memory.laneChange.switchLane != 0) { 1001 | for (int i = 0; i < agent_model::NOCP * agent_model::NORP * 2; i++) 1002 | _state.aux[i] = 0; 1003 | } 1004 | 1005 | return reaction; 1006 | } 1007 | 1008 | double AgentModel::subconsciousFollow() { 1009 | 1010 | using namespace std; 1011 | 1012 | double res = 0; 1013 | // get values 1014 | for (auto &t : _state.conscious.follow.targets) { 1015 | 1016 | // ignore when distance is inf 1017 | if (std::isinf(t.distance)) 1018 | continue; 1019 | 1020 | double vT = t.velocity; 1021 | double ds = t.distance; 1022 | double v0 = _state.conscious.velocity.local; 1023 | double s0 = _param.follow.dsStopped; 1024 | double T = _param.follow.timeHeadway; 1025 | double TMax = _param.follow.thwMax; 1026 | double v = _input.vehicle.v; 1027 | 1028 | double v0T = std::max(10.0, v0); 1029 | double vTT = std::min(v0T, std::max(5.0, vT)); 1030 | 1031 | // calculate compensating time headway 1032 | double TT = (s0 + T * vTT - (T * vTT * sqrt(vTT * vTT + v0T * v0T) * sqrt(vTT + v0T) * sqrt(v0T - vTT)) / (v0T * v0T)) / vTT; 1033 | TT = max(0.0, min(T, TT)); 1034 | 1035 | // scale down factor 1036 | double f = agent_model::scaleInf(ds, v0 * TMax, vT * T); 1037 | double fT = agent_model::scale(vT, 5.0, 0.0); 1038 | 1039 | // calculate reaction and multiply with target factor 1040 | res += t.factor * agent_model::IDMFollowReaction(ds * f, vT, v, T - fT * TT, s0, _param.velocity.a, _param.velocity.b); 1041 | } 1042 | return res; 1043 | } 1044 | 1045 | 1046 | double AgentModel::subconsciousStop() { 1047 | 1048 | using namespace std; 1049 | 1050 | // get states 1051 | double v = _input.vehicle.v; 1052 | double ds = _state.conscious.stop.ds; 1053 | double dsMax = _state.conscious.stop.dsMax; 1054 | 1055 | // get parameters 1056 | double s0 = 2.0; // never set to 0.0 (this value is used to give IDM parameter s0 a value, its compensated though) 1057 | double T = 1.2; // time headway (this is only to have a degressive behavior) 1058 | double a = _param.velocity.a; // acceleration 1059 | double b = _param.velocity.b; // deceleration 1060 | 1061 | // abort, when out of range 1062 | if (ds > dsMax || isinf(dsMax)) 1063 | return 0.0; 1064 | 1065 | // apply s0 1066 | ds += s0; 1067 | dsMax += s0; 1068 | 1069 | // distance scaling (to have a smooth transition from uninfluenced to stopping) 1070 | ds *= agent_model::scaleInf(ds, dsMax, s0, 1.0); 1071 | 1072 | // calculate reaction 1073 | return agent_model::IDMFollowReaction(ds, 0.0, v, T, s0, a, b); 1074 | 1075 | } 1076 | 1077 | 1078 | double AgentModel::subconsciousSpeed() { 1079 | 1080 | // scale parameter 1081 | double deltaLoc = agent_model::scale(_state.conscious.velocity.local, 10.0, 2.0, 1.0) * 3.5 + 0.5; 1082 | double deltaPred = agent_model::scale(_state.conscious.velocity.prediction, 10.0, 2.0, 1.0) * 3.5 + 0.5; 1083 | 1084 | // calculate reaction 1085 | auto local = agent_model::IDMSpeedReaction(_input.vehicle.v, _state.conscious.velocity.local, deltaLoc); 1086 | auto pred = agent_model::IDMSpeedReaction(_input.vehicle.v, _state.conscious.velocity.prediction, deltaPred); 1087 | 1088 | // return 1089 | return _filter.value(std::max(local, pred)); 1090 | 1091 | } 1092 | 1093 | 1094 | double AgentModel::subconsciousStartStop() { 1095 | 1096 | // check for standing 1097 | return (_state.conscious.stop.standing || _state.conscious.follow.standing) 1098 | ? _param.stop.pedalDuringStanding : INFINITY; 1099 | 1100 | } 1101 | --------------------------------------------------------------------------------