├── .gitignore ├── content └── img │ ├── rosgraph_autoware.auto.png │ └── dotgraph_autoware.svg ├── reference_interfaces ├── msg │ ├── TransmissionStats.idl │ └── Message4kb.idl ├── package.xml └── CMakeLists.txt ├── reference_system ├── README.md ├── include │ └── reference_system │ │ ├── msg_types.hpp │ │ ├── system │ │ ├── systems.hpp │ │ └── type │ │ │ └── rclcpp_system.hpp │ │ ├── nodes │ │ ├── rclcpp │ │ │ ├── command.hpp │ │ │ ├── sensor.hpp │ │ │ ├── transformer.hpp │ │ │ ├── reactor.hpp │ │ │ └── fusion.hpp │ │ └── settings.hpp │ │ ├── number_cruncher.hpp │ │ └── sample_management.hpp ├── package.xml └── CMakeLists.txt ├── benchmark.sh ├── autoware_reference_system ├── test │ ├── generate_graph.sh │ ├── test_autoware_reference_system.cpp │ └── test_autoware_reference_system.py ├── package.xml ├── src │ └── ros2 │ │ └── executor │ │ ├── autoware_default_multithreaded.cpp │ │ ├── autoware_default_singlethreaded.cpp │ │ └── autoware_default_staticsinglethreaded.cpp ├── CMakeLists.txt ├── include │ └── autoware_reference_system │ │ ├── system │ │ └── timing │ │ │ ├── default.hpp │ │ │ └── benchmark.hpp │ │ └── autoware_system_builder.hpp └── README.md ├── CONTRIBUTING.md ├── .github └── workflows │ └── colcon-build.yml ├── README.md └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | **__pycache__ 2 | -------------------------------------------------------------------------------- /content/img/rosgraph_autoware.auto.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ApexAI/reference-system-autoware/HEAD/content/img/rosgraph_autoware.auto.png -------------------------------------------------------------------------------- /reference_interfaces/msg/TransmissionStats.idl: -------------------------------------------------------------------------------- 1 | module reference_interfaces { 2 | module msg { 3 | module TransmissionStats_Constants { 4 | const uint64 NODE_NAME_LENGTH = 56; 5 | }; 6 | struct TransmissionStats { // 64 bytes 7 | uint64 timestamp; 8 | char node_name[56]; 9 | }; 10 | }; 11 | }; 12 | -------------------------------------------------------------------------------- /reference_system/README.md: -------------------------------------------------------------------------------- 1 | # reference_system 2 | 3 | This package holds the core node definitions that can be used to build various reference systems that can end up being quite complex. 4 | 5 | These _reference systems_ can then be used to benchmark, test and evaluate various changes to the core middleware being used (e.g. executors, QoS settings, DDSs', etc.). 6 | 7 | See [autoware_reference_system](../autoware_reference_system/) as an example as to how one could use these nodes to build a simulated real-world system for benchmarking and testing purposes. -------------------------------------------------------------------------------- /reference_interfaces/msg/Message4kb.idl: -------------------------------------------------------------------------------- 1 | #include "reference_interfaces/msg/TransmissionStats.idl" 2 | 3 | module reference_interfaces { 4 | module msg { 5 | module Message4kb_Constants { 6 | const uint64 STATS_CAPACITY = 63; 7 | }; 8 | struct Message4kb { 9 | uint64 size; // 8 10 | reference_interfaces::msg::TransmissionStats stats[63]; // + 4032 = 63 * 64 11 | int64 data[7]; // + 56 = 7 * 8 12 | //----------------- 13 | // 4096 14 | }; 15 | }; 16 | }; 17 | -------------------------------------------------------------------------------- /benchmark.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | BENCHMARK_DURATION_IN_SECONDS=10 4 | 5 | if ! [ -x "$(command -v psrecord)" ]; then 6 | echo psrecord is not installed, please install it with pip install -U psrecord 7 | exit 1 8 | fi 9 | 10 | # colcon build --packages-up-to reference_system 11 | 12 | for FILE in $(find build/reference_system/ -maxdepth 1 -type f -executable); do 13 | BASE_FILE=$(basename ${FILE}) 14 | echo benchmarking: ${BASE_FILE} 15 | psrecord ./${FILE} --include-children --plot ${BASE_FILE}_benchmark.png --duration ${BENCHMARK_DURATION_IN_SECONDS} >/dev/null 2>/dev/null 16 | killall ${BASE_FILE} 17 | done 18 | -------------------------------------------------------------------------------- /autoware_reference_system/test/generate_graph.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if ! [ -x "$(command -v psrecord)" ] 4 | then 5 | echo Please install psrecord and make it available via the PATH variable. 6 | echo \# pip install psrecord 7 | echo \# export PATH=\${PATH}:\${HOME}/.local/bin/ 8 | exit -1 9 | fi 10 | 11 | psrecord "ros2 run reference_system_autoware $1" --log $2 --plot $3 --duration $4 -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | -------------------------------------------------------------------------------- /autoware_reference_system/test/test_autoware_reference_system.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "gtest/gtest.h" 16 | 17 | TEST(TestReferenceSystemAutoware, DummyTest) { 18 | EXPECT_EQ(1, 1); 19 | } 20 | -------------------------------------------------------------------------------- /reference_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | reference_interfaces 5 | 0.0.0 6 | Reference system for the ROScon workshop 7 | Christian Eltzschig 8 | Apache License 2.0 9 | 10 | ament_cmake_auto 11 | rosidl_default_generators 12 | 13 | rosidl_default_runtime 14 | 15 | 16 | rosidl_interface_packages 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/msg_types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__MSG_TYPES_HPP_ 15 | #define REFERENCE_SYSTEM__MSG_TYPES_HPP_ 16 | 17 | #include "reference_interfaces/msg/message4kb.hpp" 18 | 19 | using message_t = reference_interfaces::msg::Message4kb; 20 | 21 | #endif // REFERENCE_SYSTEM__MSG_TYPES_HPP_ 22 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/system/systems.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__SYSTEM__SYSTEMS_HPP_ 15 | #define REFERENCE_SYSTEM__SYSTEM__SYSTEMS_HPP_ 16 | 17 | // Add available systems here 18 | #include "reference_system/system/type/rclcpp_system.hpp" 19 | 20 | #endif // REFERENCE_SYSTEM__SYSTEM__SYSTEMS_HPP_ 21 | -------------------------------------------------------------------------------- /reference_system/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | reference_system 5 | 0.0.0 6 | Reference system for the ROScon workshop 7 | Evan Flynn 8 | Apache License 2.0 9 | 10 | Christian Eltzschig 11 | 12 | ament_cmake 13 | ament_cmake_auto 14 | 15 | rclcpp 16 | reference_interfaces 17 | rcl_interfaces 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | ros_testing 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /autoware_reference_system/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | autoware_reference_system 5 | 0.0.0 6 | Autoware Reference System for the ROScon workshop 7 | Evan Flynn 8 | Apache License 2.0 9 | 10 | Christian Eltzschig 11 | 12 | ament_cmake 13 | ament_cmake_auto 14 | 15 | reference_system 16 | 17 | rclcpp 18 | reference_interfaces 19 | rcl_interfaces 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | ros_testing 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /reference_system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(reference_system 3 | VERSION 0.0.1 4 | ) 5 | 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | endif() 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | find_package(ament_cmake_auto REQUIRED) 15 | ament_auto_find_build_dependencies() 16 | 17 | # Add header-only library 18 | add_library(${PROJECT_NAME} INTERFACE) 19 | 20 | target_include_directories(${PROJECT_NAME} INTERFACE 21 | $ 22 | $ 23 | ) 24 | 25 | if(${BUILD_TESTING}) 26 | find_package(ament_lint_auto REQUIRED) 27 | ament_lint_auto_find_test_dependencies() 28 | endif() 29 | 30 | # Install 31 | install(TARGETS ${PROJECT_NAME} 32 | EXPORT "export_${PROJECT_NAME}" 33 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 34 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 35 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 36 | INCLUDES DESTINATION include 37 | ) 38 | 39 | ament_auto_package() 40 | 41 | -------------------------------------------------------------------------------- /reference_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(reference_interfaces) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake_auto REQUIRED) 14 | ament_auto_find_build_dependencies() 15 | 16 | # add additional messages here 17 | set(msg_files 18 | "msg/TransmissionStats.idl" 19 | "msg/Message4kb.idl" 20 | ) 21 | 22 | # add additional message dependencies here 23 | #set(msg_dependencies 24 | # "std_msgs" 25 | #) 26 | 27 | rosidl_generate_interfaces(${PROJECT_NAME} 28 | ${msg_files} 29 | DEPENDENCIES 30 | ${msg_dependencies} 31 | ADD_LINTER_TESTS 32 | ) 33 | 34 | ament_auto_package() 35 | 36 | # fix rosidl_generator_py bug #143 37 | # https://github.com/ros2/rosidl_python/issues/143 38 | set(GENERATED_FILE "${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_py/${PROJECT_NAME}/msg/_transmission_stats.py") 39 | 40 | message(STATUS "checking generated file: ${GENERATED_FILE}") 41 | add_custom_command( 42 | TARGET ${PROJECT_NAME}__python 43 | POST_BUILD 44 | COMMAND sed -i "s/all(val >= 0 and val) < 256/all(ord(val) >= 0 and ord(val) < 256/" ${GENERATED_FILE} 45 | COMMENT "Check generated IDL files for extra parenthesis..." 46 | VERBATIM) -------------------------------------------------------------------------------- /reference_system/include/reference_system/system/type/rclcpp_system.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__SYSTEM__TYPE__RCLCPP_SYSTEM_HPP_ 15 | #define REFERENCE_SYSTEM__SYSTEM__TYPE__RCLCPP_SYSTEM_HPP_ 16 | #include "reference_system/nodes/rclcpp/command.hpp" 17 | #include "reference_system/nodes/rclcpp/fusion.hpp" 18 | #include "reference_system/nodes/rclcpp/transformer.hpp" 19 | #include "reference_system/nodes/rclcpp/reactor.hpp" 20 | #include "reference_system/nodes/rclcpp/sensor.hpp" 21 | 22 | struct RclcppSystem 23 | { 24 | using NodeBaseType = rclcpp::Node; 25 | using Sensor = nodes::rclcpp_system::Sensor; 26 | using Command = nodes::rclcpp_system::Command; 27 | using Fusion = nodes::rclcpp_system::Fusion; 28 | using Reactor = nodes::rclcpp_system::Reactor; 29 | using Transformer = nodes::rclcpp_system::Transformer; 30 | }; 31 | 32 | #endif // REFERENCE_SYSTEM__SYSTEM__TYPE__RCLCPP_SYSTEM_HPP_ 33 | -------------------------------------------------------------------------------- /autoware_reference_system/src/ros2/executor/autoware_default_multithreaded.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | 17 | #include "reference_system/system/systems.hpp" 18 | 19 | #include "autoware_reference_system/autoware_system_builder.hpp" 20 | #include "autoware_reference_system/system/timing/benchmark.hpp" 21 | #include "autoware_reference_system/system/timing/default.hpp" 22 | 23 | int main(int argc, char * argv[]) 24 | { 25 | rclcpp::init(argc, argv); 26 | 27 | using TimeConfig = nodes::timing::Default; 28 | // uncomment for benchmarking 29 | // using TimeConfig = nodes::timing::BenchmarkCPUUsage; 30 | // set_benchmark_mode(true); 31 | 32 | auto nodes = create_autoware_nodes(); 33 | 34 | rclcpp::executors::MultiThreadedExecutor executor; 35 | for (auto & node : nodes) { 36 | executor.add_node(node); 37 | } 38 | executor.spin(); 39 | 40 | nodes.clear(); 41 | rclcpp::shutdown(); 42 | 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /autoware_reference_system/src/ros2/executor/autoware_default_singlethreaded.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | 17 | #include "reference_system/system/systems.hpp" 18 | 19 | #include "autoware_reference_system/autoware_system_builder.hpp" 20 | #include "autoware_reference_system/system/timing/benchmark.hpp" 21 | #include "autoware_reference_system/system/timing/default.hpp" 22 | 23 | int main(int argc, char * argv[]) 24 | { 25 | rclcpp::init(argc, argv); 26 | 27 | using TimeConfig = nodes::timing::Default; 28 | // uncomment for benchmarking 29 | // using TimeConfig = nodes::timing::BenchmarkCPUUsage; 30 | // set_benchmark_mode(true); 31 | 32 | auto nodes = create_autoware_nodes(); 33 | 34 | rclcpp::executors::SingleThreadedExecutor executor; 35 | for (auto & node : nodes) { 36 | executor.add_node(node); 37 | } 38 | executor.spin(); 39 | 40 | nodes.clear(); 41 | rclcpp::shutdown(); 42 | 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /autoware_reference_system/src/ros2/executor/autoware_default_staticsinglethreaded.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | 17 | #include "reference_system/system/systems.hpp" 18 | 19 | #include "autoware_reference_system/autoware_system_builder.hpp" 20 | #include "autoware_reference_system/system/timing/benchmark.hpp" 21 | #include "autoware_reference_system/system/timing/default.hpp" 22 | 23 | int main(int argc, char * argv[]) 24 | { 25 | rclcpp::init(argc, argv); 26 | 27 | using TimeConfig = nodes::timing::Default; 28 | // uncomment for benchmarking 29 | // using TimeConfig = nodes::timing::BenchmarkCPUUsage; 30 | // set_benchmark_mode(true); 31 | 32 | auto nodes = create_autoware_nodes(); 33 | 34 | #if 0 35 | rclcpp::executors::StaticSingleThreadedExecutor executor; 36 | for (auto & node : nodes) { 37 | executor.add_node(node); 38 | } 39 | executor.spin(); 40 | #endif 41 | 42 | nodes.clear(); 43 | rclcpp::shutdown(); 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/nodes/rclcpp/command.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__NODES__RCLCPP__COMMAND_HPP_ 15 | #define REFERENCE_SYSTEM__NODES__RCLCPP__COMMAND_HPP_ 16 | 17 | #include 18 | #include 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "reference_system/nodes/settings.hpp" 22 | #include "reference_system/sample_management.hpp" 23 | #include "reference_system/msg_types.hpp" 24 | 25 | namespace nodes 26 | { 27 | namespace rclcpp_system 28 | { 29 | 30 | class Command : public rclcpp::Node 31 | { 32 | public: 33 | explicit Command(const CommandSettings & settings) 34 | : Node(settings.node_name) 35 | { 36 | subscription_ = this->create_subscription( 37 | settings.input_topic, 10, 38 | [this](const message_t::SharedPtr msg) {input_callback(msg);}); 39 | } 40 | 41 | private: 42 | void input_callback(const message_t::SharedPtr input_message) const 43 | { 44 | print_sample_path(this->get_name(), input_message); 45 | } 46 | 47 | private: 48 | rclcpp::Subscription::SharedPtr subscription_; 49 | }; 50 | } // namespace rclcpp_system 51 | } // namespace nodes 52 | #endif // REFERENCE_SYSTEM__NODES__RCLCPP__COMMAND_HPP_ 53 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/nodes/settings.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__NODES__SETTINGS_HPP_ 15 | #define REFERENCE_SYSTEM__NODES__SETTINGS_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | namespace nodes 22 | { 23 | 24 | struct CommandSettings 25 | { 26 | std::string node_name; 27 | std::string input_topic; 28 | }; 29 | 30 | struct FusionSettings 31 | { 32 | std::string node_name; 33 | std::string input_0; 34 | std::string input_1; 35 | std::string output_topic; 36 | std::chrono::nanoseconds number_crunch_time; 37 | std::chrono::nanoseconds max_input_time_difference; 38 | }; 39 | 40 | struct TransformerSettings 41 | { 42 | std::string node_name; 43 | std::string input_topic; 44 | std::string output_topic; 45 | std::chrono::nanoseconds number_crunch_time; 46 | }; 47 | 48 | struct ReactorSettings 49 | { 50 | std::string node_name; 51 | std::vector inputs; 52 | std::string output_topic; 53 | std::chrono::nanoseconds cycle_time; 54 | }; 55 | 56 | struct SensorSettings 57 | { 58 | std::string node_name; 59 | std::string topic_name; 60 | std::chrono::nanoseconds cycle_time; 61 | }; 62 | } // namespace nodes 63 | #endif // REFERENCE_SYSTEM__NODES__SETTINGS_HPP_ 64 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/number_cruncher.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__NUMBER_CRUNCHER_HPP_ 15 | #define REFERENCE_SYSTEM__NUMBER_CRUNCHER_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | std::vector number_cruncher(const std::chrono::nanoseconds & timeout) 22 | { 23 | // cause heavy CPU load by finding some primes 24 | std::vector primes{2}; 25 | // allocate memory beforehand so that we put only load on the CPU 26 | primes.reserve(1024 * 1024); 27 | bool has_timeout_occurred = false; 28 | auto current_time = [] { 29 | return std::chrono::duration_cast( 30 | std::chrono::system_clock::now().time_since_epoch()) 31 | .count(); 32 | }; 33 | int64_t start_time = current_time(); 34 | for (uint64_t i = 3; !has_timeout_occurred; ++i) { 35 | uint64_t rootOfI = static_cast(std::sqrt(i)); 36 | for (auto p : primes) { 37 | if (current_time() - start_time > timeout.count()) { 38 | has_timeout_occurred = true; 39 | } 40 | if (p > rootOfI) { 41 | primes.emplace_back(i); 42 | break; 43 | } else if (i % p == 0) { 44 | break; 45 | } 46 | } 47 | } 48 | return primes; 49 | } 50 | 51 | #endif // REFERENCE_SYSTEM__NUMBER_CRUNCHER_HPP_ 52 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/nodes/rclcpp/sensor.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__NODES__RCLCPP__SENSOR_HPP_ 15 | #define REFERENCE_SYSTEM__NODES__RCLCPP__SENSOR_HPP_ 16 | #include 17 | #include 18 | #include 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "reference_system/nodes/settings.hpp" 22 | #include "reference_system/sample_management.hpp" 23 | #include "reference_system/msg_types.hpp" 24 | 25 | namespace nodes 26 | { 27 | namespace rclcpp_system 28 | { 29 | 30 | class Sensor : public rclcpp::Node 31 | { 32 | public: 33 | explicit Sensor(const SensorSettings & settings) 34 | : Node(settings.node_name) 35 | { 36 | publisher_ = this->create_publisher(settings.topic_name, 10); 37 | timer_ = this->create_wall_timer( 38 | settings.cycle_time, 39 | [this] {timer_callback();}); 40 | } 41 | 42 | private: 43 | void timer_callback() 44 | { 45 | auto message = publisher_->borrow_loaned_message(); 46 | message.get().size = 0; 47 | 48 | set_sample(this->get_name(), message.get()); 49 | 50 | publisher_->publish(std::move(message)); 51 | } 52 | 53 | private: 54 | rclcpp::Publisher::SharedPtr publisher_; 55 | rclcpp::TimerBase::SharedPtr timer_; 56 | }; 57 | } // namespace rclcpp_system 58 | } // namespace nodes 59 | #endif // REFERENCE_SYSTEM__NODES__RCLCPP__SENSOR_HPP_ 60 | -------------------------------------------------------------------------------- /.github/workflows/colcon-build.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: CI 4 | 5 | # Controls when the workflow will run 6 | on: 7 | # Triggers the workflow on push or pull request events but only for the main branch 8 | push: 9 | branches: [ main ] 10 | pull_request: 11 | branches: [ main ] 12 | 13 | # Allows you to run this workflow manually from the Actions tab 14 | workflow_dispatch: 15 | 16 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel 17 | jobs: 18 | # This workflow contains a single job called "build" 19 | build: 20 | # The type of runner that the job will run on 21 | runs-on: [ubuntu-latest] 22 | strategy: 23 | fail-fast: false 24 | matrix: 25 | ros_distribution: 26 | - foxy 27 | - galactic 28 | - rolling 29 | include: 30 | # Foxy Fitzroy (June 2020 - May 2023) 31 | - ros_distribution: foxy 32 | docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-foxy-ros-base-latest 33 | # Galactic Geochelone (May 2021 - November 2022) 34 | - ros_distribution: galactic 35 | docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-ros-base-latest 36 | # Rolling Ridley (June 2020 - Present) 37 | - ros_distribution: rolling 38 | docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-rolling-ros-base-latest 39 | container: 40 | image: ${{ matrix.docker_image }} 41 | # Steps represent a sequence of tasks that will be executed as part of the job 42 | steps: 43 | - name: setup workspace 44 | run: mkdir -p ros2_ws/src 45 | - name: checkout 46 | uses: actions/checkout@v2 47 | with: 48 | path: ros2_ws/src 49 | - name: build and test 50 | uses: ros-tooling/action-ros-ci@v0.2 51 | with: 52 | package-name: autoware_reference_system 53 | target-ros2-distro: ${{ matrix.ros_distribution }} 54 | vcs-repo-file-url: "" 55 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/nodes/rclcpp/transformer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__NODES__RCLCPP__PROCESSING_HPP_ 15 | #define REFERENCE_SYSTEM__NODES__RCLCPP__PROCESSING_HPP_ 16 | #include 17 | #include 18 | #include 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "reference_system/nodes/settings.hpp" 22 | #include "reference_system/number_cruncher.hpp" 23 | #include "reference_system/sample_management.hpp" 24 | #include "reference_system/msg_types.hpp" 25 | 26 | namespace nodes 27 | { 28 | namespace rclcpp_system 29 | { 30 | 31 | class Transformer : public rclcpp::Node 32 | { 33 | public: 34 | explicit Transformer(const TransformerSettings & settings) 35 | : Node(settings.node_name), 36 | number_crunch_time_(settings.number_crunch_time) 37 | { 38 | subscription_ = this->create_subscription( 39 | settings.input_topic, 10, 40 | [this](const message_t::SharedPtr msg) {input_callback(msg);}); 41 | publisher_ = this->create_publisher(settings.output_topic, 10); 42 | } 43 | 44 | private: 45 | void input_callback(const message_t::SharedPtr input_message) const 46 | { 47 | auto number_cruncher_result = number_cruncher(number_crunch_time_); 48 | 49 | auto output_message = publisher_->borrow_loaned_message(); 50 | 51 | fuse_samples(this->get_name(), output_message.get(), input_message); 52 | 53 | // use result so that it is not optimizied away by some clever compiler 54 | output_message.get().data[0] = number_cruncher_result.empty(); 55 | publisher_->publish(std::move(output_message)); 56 | } 57 | 58 | private: 59 | rclcpp::Publisher::SharedPtr publisher_; 60 | rclcpp::Subscription::SharedPtr subscription_; 61 | std::chrono::nanoseconds number_crunch_time_; 62 | }; 63 | } // namespace rclcpp_system 64 | } // namespace nodes 65 | #endif // REFERENCE_SYSTEM__NODES__RCLCPP__PROCESSING_HPP_ 66 | -------------------------------------------------------------------------------- /autoware_reference_system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(autoware_reference_system) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake_auto REQUIRED) 13 | ament_auto_find_build_dependencies() 14 | 15 | # find_package(reference_system REQUIRED) 16 | 17 | # Single Threaded Executor 18 | ament_auto_add_executable(autoware_default_singlethreaded 19 | src/ros2/executor/autoware_default_singlethreaded.cpp 20 | ) 21 | 22 | # Static Single Threaded Executor 23 | ament_auto_add_executable(autoware_default_staticsinglethreaded 24 | src/ros2/executor/autoware_default_staticsinglethreaded.cpp 25 | ) 26 | 27 | # Multi Threaded Executor 28 | ament_auto_add_executable(autoware_default_multithreaded 29 | src/ros2/executor/autoware_default_multithreaded.cpp 30 | ) 31 | 32 | # Add new executors to test here 33 | #ament_auto_add_executable(autoware_default_custom 34 | # src/ros2/executor/autoware_default_custom.cpp 35 | #) 36 | 37 | if(${BUILD_TESTING}) 38 | find_package(ament_lint_auto REQUIRED) 39 | ament_lint_auto_find_test_dependencies() 40 | 41 | find_package(ros_testing REQUIRED) 42 | 43 | # run target for n seconds 44 | function(test_reference_system_autoware target timeout) 45 | set(TEST_EXECUTABLE ${CMAKE_CURRENT_BINARY_DIR}/${target}) 46 | set(TEST_EXECUTABLE_NAME ${target}_${rmw_implementation}) 47 | set(TIMEOUT ${timeout}) 48 | set(RMW_IMPLEMENTATION ${rmw_implementation}) 49 | 50 | # replaces all @var@ and ${var} within input file 51 | configure_file( 52 | test/test_${PROJECT_NAME}.py 53 | test_${target}_${rmw_implementation}.py 54 | @ONLY 55 | ) 56 | 57 | add_ros_test( 58 | ${CMAKE_CURRENT_BINARY_DIR}/test_${target}_${rmw_implementation}.py 59 | TIMEOUT 10 # seconds 60 | ) 61 | 62 | if(TARGET ${target}) 63 | ament_target_dependencies(${target} 64 | "rclcpp" "reference_interfaces" "reference_system") 65 | endif() 66 | endfunction() 67 | 68 | # get available rmw implementations 69 | find_package(rmw_implementation_cmake REQUIRED) 70 | get_available_rmw_implementations(rmws_available) 71 | 72 | # loop over each rmw implmentation 73 | foreach(rmw_implementation ${rmws_available}) 74 | find_package("${rmw_implementation}" REQUIRED) 75 | 76 | # add each exe to test 77 | test_reference_system_autoware(autoware_default_singlethreaded 10) 78 | test_reference_system_autoware(autoware_default_staticsinglethreaded 10) 79 | test_reference_system_autoware(autoware_default_multithreaded 10) 80 | # Add new exe's here to test 81 | #test_reference_system_autoware(autoware_default_custom) 82 | 83 | endforeach() 84 | endif() 85 | 86 | ament_auto_package( 87 | INSTALL_TO_SHARE test 88 | ) 89 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/nodes/rclcpp/reactor.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__NODES__RCLCPP__REACTOR_HPP_ 15 | #define REFERENCE_SYSTEM__NODES__RCLCPP__REACTOR_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "reference_system/nodes/settings.hpp" 23 | #include "reference_system/sample_management.hpp" 24 | #include "reference_system/msg_types.hpp" 25 | 26 | namespace nodes 27 | { 28 | namespace rclcpp_system 29 | { 30 | 31 | class Reactor : public rclcpp::Node 32 | { 33 | public: 34 | explicit Reactor(const ReactorSettings & settings) 35 | : Node(settings.node_name) 36 | { 37 | uint64_t input_number = 0U; 38 | for (const auto & input_topic : settings.inputs) { 39 | subscriptions_.emplace_back( 40 | this->create_subscription( 41 | input_topic, 10, 42 | [this, input_number](const message_t::SharedPtr msg) { 43 | input_callback(input_number, msg); 44 | })); 45 | ++input_number; 46 | } 47 | message_cache_.resize(subscriptions_.size()); 48 | publisher_ = this->create_publisher(settings.output_topic, 10); 49 | timer_ = this->create_wall_timer( 50 | settings.cycle_time, 51 | [this]{timer_callback();}); 52 | } 53 | 54 | private: 55 | void input_callback( 56 | const uint64_t input_number, 57 | const message_t::SharedPtr input_message) 58 | { 59 | message_cache_[input_number] = input_message; 60 | } 61 | 62 | void timer_callback() { 63 | uint64_t sent_samples = 0; 64 | for(auto & m : message_cache_) { 65 | if ( !m ) continue; 66 | 67 | auto output_message = publisher_->borrow_loaned_message(); 68 | 69 | fuse_samples(this->get_name(), output_message.get(), m); 70 | 71 | publisher_->publish(std::move(output_message)); 72 | m.reset(); 73 | ++sent_samples; 74 | } 75 | 76 | if ( sent_samples == 0 ) { 77 | auto message = publisher_->borrow_loaned_message(); 78 | message.get().size = 0; 79 | 80 | set_sample(this->get_name(), message.get()); 81 | 82 | publisher_->publish(std::move(message)); 83 | } 84 | } 85 | 86 | private: 87 | rclcpp::Publisher::SharedPtr publisher_; 88 | rclcpp::TimerBase::SharedPtr timer_; 89 | std::vector::SharedPtr> subscriptions_; 90 | std::vector message_cache_; 91 | }; 92 | } // namespace rclcpp_system 93 | } // namespace nodes 94 | #endif // REFERENCE_SYSTEM__NODES__RCLCPP__REACTOR_HPP_ 95 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/nodes/rclcpp/fusion.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__NODES__RCLCPP__FUSION_HPP_ 15 | #define REFERENCE_SYSTEM__NODES__RCLCPP__FUSION_HPP_ 16 | #include 17 | #include 18 | #include 19 | 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "reference_system/nodes/settings.hpp" 22 | #include "reference_system/number_cruncher.hpp" 23 | #include "reference_system/sample_management.hpp" 24 | #include "reference_system/msg_types.hpp" 25 | 26 | namespace nodes 27 | { 28 | namespace rclcpp_system 29 | { 30 | 31 | class Fusion : public rclcpp::Node 32 | { 33 | public: 34 | explicit Fusion(const FusionSettings & settings) 35 | : Node(settings.node_name), 36 | number_crunch_time_(settings.number_crunch_time), 37 | max_input_time_difference_(settings.max_input_time_difference) 38 | { 39 | subscription_[0] = this->create_subscription( 40 | settings.input_0, 10, 41 | [this](const message_t::SharedPtr msg) {input_callback(0U, msg);}); 42 | 43 | subscription_[1] = this->create_subscription( 44 | settings.input_1, 10, 45 | [this](const message_t::SharedPtr msg) {input_callback(1U, msg);}); 46 | publisher_ = this->create_publisher(settings.output_topic, 10); 47 | } 48 | 49 | private: 50 | void input_callback( 51 | const uint64_t input_number, 52 | const message_t::SharedPtr input_message) 53 | { 54 | message_cache_[input_number] = input_message; 55 | 56 | // only process and publish when we can perform an actual fusion, this means 57 | // we have received a sample from each subscription 58 | if (!message_cache_[0] || !message_cache_[1]) { 59 | return; 60 | } 61 | 62 | uint64_t timestamp_input0 = get_sample_timestamp(message_cache_[0]); 63 | uint64_t timestamp_input1 = get_sample_timestamp(message_cache_[1]); 64 | int64_t time_diff = (timestamp_input0 < timestamp_input1) 65 | ? timestamp_input1 - timestamp_input0 66 | : timestamp_input0 - timestamp_input1; 67 | 68 | if ( time_diff >= max_input_time_difference_.count()) { 69 | std::cerr << "[ Warning ] " << this->get_name() << " : input latency exceeded from " 70 | << "{" << subscription_[0]->get_topic_name() << ", " 71 | << subscription_[1]->get_topic_name() << "} " << std::endl; 72 | exit(0); 73 | } 74 | 75 | auto number_cruncher_result = number_cruncher(number_crunch_time_); 76 | 77 | auto output_message = publisher_->borrow_loaned_message(); 78 | fuse_samples( 79 | this->get_name(), output_message.get(), message_cache_[0], 80 | message_cache_[1]); 81 | output_message.get().data[0] = number_cruncher_result.empty(); 82 | publisher_->publish(std::move(output_message)); 83 | 84 | message_cache_[0].reset(); 85 | message_cache_[1].reset(); 86 | } 87 | 88 | private: 89 | message_t::SharedPtr message_cache_[2]; 90 | rclcpp::Publisher::SharedPtr publisher_; 91 | rclcpp::Subscription::SharedPtr subscription_[2]; 92 | 93 | std::chrono::nanoseconds number_crunch_time_; 94 | std::chrono::nanoseconds max_input_time_difference_; 95 | }; 96 | } // namespace rclcpp_system 97 | } // namespace nodes 98 | #endif // REFERENCE_SYSTEM__NODES__RCLCPP__FUSION_HPP_ 99 | -------------------------------------------------------------------------------- /autoware_reference_system/include/autoware_reference_system/system/timing/default.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef AUTOWARE_REFERENCE_SYSTEM__SYSTEM__TIMING__DEFAULT_HPP_ 15 | #define AUTOWARE_REFERENCE_SYSTEM__SYSTEM__TIMING__DEFAULT_HPP_ 16 | #include 17 | 18 | namespace nodes 19 | { 20 | namespace timing 21 | { 22 | 23 | struct Default 24 | { 25 | using time_t = std::chrono::nanoseconds; 26 | using milliseconds = std::chrono::milliseconds; 27 | 28 | // sensors 29 | static constexpr time_t FRONT_LIDAR_DRIVER = milliseconds(100); 30 | static constexpr time_t REAR_LIDAR_DRIVER = milliseconds(100); 31 | static constexpr time_t POINT_CLOUD_MAP = milliseconds(100); 32 | static constexpr time_t RVIZ2 = milliseconds(100); 33 | static constexpr time_t LANELET2MAP = milliseconds(100); 34 | 35 | // processing 36 | static constexpr time_t POINTS_TRANSFORMER_FRONT = milliseconds(50); 37 | static constexpr time_t POINTS_TRANSFORMER_REAR = milliseconds(50); 38 | static constexpr time_t VOXEL_GRID_DOWNSAMPLER = milliseconds(50); 39 | static constexpr time_t POINT_CLOUD_MAP_LOADER = milliseconds(50); 40 | static constexpr time_t RAY_GROUND_FILTER = milliseconds(50); 41 | static constexpr time_t EUCLIDEAN_CLUSTER_DETECTOR = milliseconds(50); 42 | static constexpr time_t OBJECT_COLLISION_ESTIMATOR = milliseconds(50); 43 | static constexpr time_t MPC_CONTROLLER = milliseconds(50); 44 | static constexpr time_t PARKING_PLANNER = milliseconds(50); 45 | static constexpr time_t LANE_PLANNER = milliseconds(50); 46 | 47 | // fusion 48 | static constexpr time_t POINT_CLOUD_FUSION = milliseconds(25); 49 | static constexpr time_t NDT_LOCALIZER = milliseconds(25); 50 | static constexpr time_t VEHICLE_INTERFACE = milliseconds(25); 51 | static constexpr time_t LANELET_2_GLOBAL_PLANNER = milliseconds(25); 52 | static constexpr time_t LANELET_2_MAP_LOADER = milliseconds(25); 53 | 54 | static constexpr time_t POINT_CLOUD_FUSION_MAX_INPUT_TIME_DIFF = milliseconds(125); 55 | static constexpr time_t NDT_LOCALIZER_MAX_INPUT_TIME_DIFF = milliseconds(125); 56 | static constexpr time_t VEHICLE_INTERFACE_MAX_INPUT_TIME_DIFF = milliseconds(125); 57 | static constexpr time_t LANELET_2_GLOBAL_PLANNER_MAX_INPUT_TIME_DIFF = milliseconds(125); 58 | static constexpr time_t LANELET_2_MAP_LOADER_MAX_INPUT_TIME_DIFF = milliseconds(125); 59 | 60 | // reactor 61 | static constexpr time_t BEHAVIOR_PLANNER = milliseconds(50); 62 | }; 63 | 64 | constexpr Default::time_t Default::FRONT_LIDAR_DRIVER; 65 | constexpr Default::time_t Default::REAR_LIDAR_DRIVER; 66 | constexpr Default::time_t Default::POINT_CLOUD_MAP; 67 | constexpr Default::time_t Default::RVIZ2; 68 | constexpr Default::time_t Default::LANELET2MAP; 69 | constexpr Default::time_t Default::POINTS_TRANSFORMER_FRONT; 70 | constexpr Default::time_t Default::POINTS_TRANSFORMER_REAR; 71 | constexpr Default::time_t Default::VOXEL_GRID_DOWNSAMPLER; 72 | constexpr Default::time_t Default::POINT_CLOUD_MAP_LOADER; 73 | constexpr Default::time_t Default::RAY_GROUND_FILTER; 74 | constexpr Default::time_t Default::EUCLIDEAN_CLUSTER_DETECTOR; 75 | constexpr Default::time_t Default::OBJECT_COLLISION_ESTIMATOR; 76 | constexpr Default::time_t Default::MPC_CONTROLLER; 77 | constexpr Default::time_t Default::PARKING_PLANNER; 78 | constexpr Default::time_t Default::LANE_PLANNER; 79 | constexpr Default::time_t Default::POINT_CLOUD_FUSION; 80 | constexpr Default::time_t Default::NDT_LOCALIZER; 81 | constexpr Default::time_t Default::VEHICLE_INTERFACE; 82 | constexpr Default::time_t Default::LANELET_2_GLOBAL_PLANNER; 83 | constexpr Default::time_t Default::LANELET_2_MAP_LOADER; 84 | constexpr Default::time_t Default::POINT_CLOUD_FUSION_MAX_INPUT_TIME_DIFF; 85 | constexpr Default::time_t Default::NDT_LOCALIZER_MAX_INPUT_TIME_DIFF; 86 | constexpr Default::time_t Default::VEHICLE_INTERFACE_MAX_INPUT_TIME_DIFF; 87 | constexpr Default::time_t Default::LANELET_2_GLOBAL_PLANNER_MAX_INPUT_TIME_DIFF; 88 | constexpr Default::time_t Default::LANELET_2_MAP_LOADER_MAX_INPUT_TIME_DIFF; 89 | constexpr Default::time_t Default::BEHAVIOR_PLANNER; 90 | 91 | } // namespace timing 92 | } // namespace nodes 93 | #endif // AUTOWARE_REFERENCE_SYSTEM__SYSTEM__TIMING__DEFAULT_HPP_ 94 | -------------------------------------------------------------------------------- /reference_system/include/reference_system/sample_management.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REFERENCE_SYSTEM__SAMPLE_MANAGEMENT_HPP_ 15 | #define REFERENCE_SYSTEM__SAMPLE_MANAGEMENT_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "reference_system/msg_types.hpp" 22 | 23 | bool set_benchmark_mode(const bool has_benchmark_mode, const bool set_value = true) 24 | { 25 | static bool value{false}; 26 | if (set_value) {value = has_benchmark_mode;} 27 | return value; 28 | } 29 | 30 | bool is_in_benchmark_mode() 31 | { 32 | return set_benchmark_mode(false, false); 33 | } 34 | 35 | template 36 | void set_sample(const std::string & node_name, SampleTypePointer & sample) 37 | { 38 | if (is_in_benchmark_mode() ) {return;} 39 | 40 | if (sample.size >= message_t::STATS_CAPACITY) { 41 | return; 42 | } 43 | 44 | uint64_t idx = sample.size; 45 | ++sample.size; 46 | 47 | memcpy( 48 | sample.stats[idx].node_name.data(), node_name.data(), 49 | std::min( 50 | node_name.size(), 51 | reference_interfaces::msg::TransmissionStats::NODE_NAME_LENGTH)); 52 | 53 | sample.stats[idx].timestamp = static_cast( 54 | std::chrono::duration_cast( 55 | std::chrono::system_clock::now().time_since_epoch()) 56 | .count()); 57 | } 58 | 59 | template 60 | uint64_t get_sample_timestamp(SampleTypePointer & sample) 61 | { 62 | if ( is_in_benchmark_mode() || sample->size == 0 ) 63 | return 0; 64 | else 65 | return sample->stats[sample->size - 1].timestamp; 66 | } 67 | 68 | template 69 | void fuse_samples( 70 | const std::string & node_name, SampleTypePointer & destination, 71 | const SourceType & source) 72 | { 73 | if (is_in_benchmark_mode() ) {return;} 74 | 75 | destination.size = source->size; 76 | destination.stats = source->stats; 77 | 78 | set_sample(node_name, destination); 79 | } 80 | 81 | template 82 | void fuse_samples( 83 | const std::string & node_name, SampleTypePointer & destination, 84 | const SourceType & source1, const SourceType & source2) 85 | { 86 | if (is_in_benchmark_mode() ) {return;} 87 | 88 | uint64_t elements_to_copy = 89 | std::min(message_t::STATS_CAPACITY, source1->size + source2->size); 90 | 91 | destination.size = elements_to_copy; 92 | 93 | destination.stats = source1->stats; 94 | memcpy( 95 | destination.stats.data() + source1->size, source2->stats.data(), 96 | sizeof(reference_interfaces::msg::TransmissionStats) * 97 | (elements_to_copy - source1->size)); 98 | 99 | set_sample(node_name, destination); 100 | } 101 | 102 | template 103 | void print_sample_path( 104 | const std::string & node_name, 105 | const SampleTypePointer & sample) 106 | { 107 | if (is_in_benchmark_mode() ) {return;} 108 | 109 | const uint64_t timestamp_in_ns = static_cast( 110 | std::chrono::duration_cast( 111 | std::chrono::system_clock::now().time_since_epoch()) 112 | .count()); 113 | 114 | std::cout << "----------------------------------------------------------" << 115 | std::endl; 116 | std::cout << "sample path: " << std::endl; 117 | std::cout << " timepoint node name" << std::endl; 118 | 119 | std::map timestamp2Order; 120 | for (uint64_t i = 0; i < sample->size; ++i) { 121 | timestamp2Order[sample->stats[i].timestamp] = 0; 122 | } 123 | uint64_t i = 0; 124 | for (auto & e : timestamp2Order) { 125 | e.second = i++; 126 | } 127 | 128 | for (uint64_t i = 0; i < sample->size; ++i) { 129 | std::cout << " ["; 130 | std::cout.width(2); 131 | std::cout << timestamp2Order[sample->stats[i].timestamp]; 132 | std::cout << "] " << sample->stats[i].timestamp << " : " << 133 | sample->stats[i].node_name.data() << std::endl; 134 | } 135 | 136 | uint64_t latency_in_ns = 0; 137 | if (sample->size > 0) { 138 | latency_in_ns = timestamp_in_ns - sample->stats[sample->size - 1].timestamp; 139 | } 140 | 141 | std::cout << std::endl; 142 | std::cout << " destination: " << node_name << std::endl; 143 | std::cout << " current time: " << timestamp_in_ns << std::endl; 144 | std::cout << " latency: " << latency_in_ns << std::endl; 145 | std::cout << "----------------------------------------------------------" << 146 | std::endl; 147 | std::cout << std::endl; 148 | } 149 | 150 | #endif // REFERENCE_SYSTEM__SAMPLE_MANAGEMENT_HPP_ 151 | -------------------------------------------------------------------------------- /autoware_reference_system/README.md: -------------------------------------------------------------------------------- 1 | # reference_system_autoware 2 | 3 | This file is meant to define the Autoware Reference System and all of its nodes, topics and message types. 4 | 5 | ![Node graph of reference-system-autoware](../content/img/dotgraph_autoware.svg) 6 | 7 | ## Message Types 8 | 9 | A **single message type** is used for the entire _reference system_ when generating results in order to simplify the setup as well as make it more repeatible and extensible. 10 | 11 | This means **only one _message type_** from the list below is used during any given experimental run for every node in the reference system. 12 | 13 | 1. [**Message4kB**](../reference_interfaces/msg/Message4kB.idl) 14 | - reference message with a fixed size of 4 kilobytes (kB) 15 | 16 | Other messages with different fixed sizes could be added here in the future. 17 | 18 | When reporting results it will be important to include the _message type_ used duing the experiement so that comparisons can be done "apples to apples" and not "apples to pears". 19 | 20 | ## Reference System Autoware 21 | 22 | Built from [a handful of building-block node types](../README.md#concept-overview), each one of these nodes are meant to simulate a real-world node from the Autoware.Auto project lidar data pipeline. 23 | 24 | Under each node type are the requirements used for _this specific reference system_, `reference_system_autoware`. Future reference systems could have slightly different requirements and still use the same building-block node types. 25 | 26 | For simplicity's sake, every node except for the _command nodes_ only ever publishes one topic and this topic has the same name as the node that publishes it. However, each topic can be subscribed to by multiple different nodes. 27 | 28 | 1. [**Message Type**](#message-types) 29 | - all nodes use the same message type during any single test run 30 | - default _message type_: 31 | - [Message4kB](include/reference_system/msg_types.hpp#L21) 32 | - to be implemented: 33 | - Message64kB 34 | - Message256kB 35 | - Message512kB 36 | - Message1024kB 37 | - Message5120kB 38 | 2. [**Sensor Nodes**](include/reference_system/nodes/rclcpp/sensor.hpp) 39 | - all _sensor nodes_ have a publishing rate (cycle time) of [**100 milliseconds**](include/reference_system/system/timing/default.hpp#L26) 40 | - all _sensor_nodes_ publish the same _message type_ 41 | - total of **5 _sensor nodes_**: 42 | - [Front Lidar Driver](include/reference_system/autoware_system_builder.hpp#L38) 43 | - [Rear Lidar Driver](include/reference_system/autoware_system_builder.hpp#L44) 44 | - [Point Cloud Map](include/reference_system/autoware_system_builder.hpp#L50) 45 | - [rviz2](include/reference_system/autoware_system_builder.hpp#L56) 46 | - [Lanelet2Map](include/reference_system/autoware_system_builder.hpp#62) 47 | 3. [**Transformer Nodes**](include/reference_system/nodes/rclcpp/processing.hpp) 48 | - all _processing nodes_ have one subscriber and one publisher 49 | - all _proccing nodes_ start processing for [**50 milliseconds**](include/reference_system/system/timing/default.hpp#L28) after a message is received 50 | - publishes message after processing is complete 51 | - total of **10 _processing nodes_:** 52 | - [Front Points Transformer](include/reference_system/autoware_system_builder.hpp#L69) 53 | - [Rear Points Transformer](include/reference_system/autoware_system_builder.hpp#L78) 54 | - [Voxel Grid Downsampler](include/reference_system/autoware_system_builder.hpp#L87) 55 | - [Point Cloud Map Loader](include/reference_system/autoware_system_builder.hpp#L96) 56 | - [Ray Ground Filter](include/reference_system/autoware_system_builder.hpp#L105) 57 | - [Euclidean Cluster Detector](include/reference_system/autoware_system_builder.hpp#L114) 58 | - [Object Collision Estimator](include/reference_system/autoware_system_builder.hpp#L123) 59 | - [MPC Controller](include/reference_system/autoware_system_builder.hpp#L132) 60 | - [Parking Planner](include/reference_system/autoware_system_builder.hpp#L141) 61 | - [Lane Planner](include/reference_system/autoware_system_builder.hpp#L150) 62 | 4. [**Fusion Nodes**](include/reference_system/nodes/rclcpp/fusion.hpp) 63 | - all _fusion nodes_ have **two subscribers** and one publisher for this _reference system_ 64 | - all _fusion nodes_ start processing for [**25 milliseconds**](include/reference_system/system/timing/default.hpp#L30) after a message is received **from all** subscriptions 65 | - publishes message after processing is complete 66 | - total of **5 _fusion nodes_:** 67 | - [Point Cloud Fusion](include/reference_system/autoware_system_builder.hpp#L160) 68 | - [NDT Localizer](include/reference_system/autoware_system_builder.hpp#L169) 69 | - [Vehicle Interface](include/reference_system/autoware_system_builder.hpp#L178) 70 | - [Lanelet2 Global Planner](include/reference_system/autoware_system_builder.hpp#L187) 71 | - [Lanelet 2 Map Loader](include/reference_system/autoware_system_builder.hpp#L196) 72 | 5. [**Reactor Nodes**](include/reference_system/nodes/rclcpp/reactor.hpp) 73 | - for this _reference system_ there is onle [**1 _reactor node_**](include/reference_system/autoware_system_builder.hpp#L206) 74 | - this _reactor node_ has **6 subscribers**and one publisher 75 | - this _reactor node_ starts processing for [**1 millisecond**](include/reference_system/system/timing/default.hpp#L32) after a message is received **from any** single subscription 76 | - publishes message after processing is complete 77 | 6. [**Command Nodes**](include/reference_system/nodes/rclcpp/command.hpp) 78 | - for this _reference system_ there is onle [**1 _command node_**](include/reference_system/autoware_system_builder.hpp#L217) 79 | - this _command node_ has **1 subscriber** and zero publishers 80 | - this _command node_ prints out the final latency statistics after a message is received on the specified topic 81 | -------------------------------------------------------------------------------- /autoware_reference_system/test/test_autoware_reference_system.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Apex.AI, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # import multiprocessing 16 | import os 17 | # import platform 18 | import time 19 | import unittest 20 | 21 | from launch import LaunchDescription 22 | from launch.actions import ExecuteProcess 23 | 24 | import launch_testing 25 | from launch_testing import post_shutdown_test 26 | import launch_testing.actions 27 | from launch_testing.asserts import assertExitCodes 28 | 29 | import rclpy 30 | from rclpy.context import Context 31 | import rclpy.executors 32 | from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy 33 | 34 | import reference_interfaces.msg 35 | 36 | # this file has @variables@ that are meant to be automatically replaced 37 | # by values using the `configure_file` CMake function during the build 38 | 39 | platforms = {} 40 | 41 | # TODO(flynneva): move this to its own file for portability 42 | # can add more supported platforms here 43 | platforms['rpi4-linux-rt'] = { 44 | 'common-name': 'raspberrypi4', 45 | 'machine': 'aarch64', 46 | 'processor': 'aarch64', 47 | 'system': 'Linux', 48 | 'flavor': 'ubuntu', 49 | 'cores': 4, 50 | 'realtime': True 51 | } 52 | 53 | 54 | def generate_test_description(): 55 | env = os.environ.copy() 56 | env['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '[{severity}] [{name}]: {message}' 57 | # specify rmw to use 58 | env['RCL_ASSERT_RMW_ID_MATCHES'] = '@RMW_IMPLEMENTATION@' 59 | env['RMW_IMPLEMENTATION'] = '@RMW_IMPLEMENTATION@' 60 | 61 | launch_description = LaunchDescription() 62 | proc_under_test = ExecuteProcess( 63 | cmd=['@TEST_EXECUTABLE@'], 64 | name='@TEST_EXECUTABLE_NAME@', 65 | sigterm_timeout='@TIMEOUT@', 66 | output='screen', 67 | env=env, 68 | ) 69 | launch_description.add_action(proc_under_test) 70 | launch_description.add_action( 71 | launch_testing.actions.ReadyToTest() 72 | ) 73 | return launch_description, locals() 74 | 75 | 76 | class TestReferenceSystemAutoware(unittest.TestCase): 77 | 78 | @classmethod 79 | def setUpClass(cls): 80 | cls.context = Context() 81 | rclpy.init(context=cls.context) 82 | cls.node = rclpy.create_node('test_node', context=cls.context) 83 | cls.msgs = [] 84 | 85 | # TODO(flynneva): sweep over different QoS settings during testing? 86 | qos_profile = QoSProfile( 87 | reliability=ReliabilityPolicy.BEST_EFFORT, 88 | history=HistoryPolicy.KEEP_LAST, 89 | depth=1 90 | ) 91 | 92 | cls.sub = cls.node.create_subscription( 93 | msg_type=reference_interfaces.msg.Message4kb, 94 | topic='/FrontLidarDriver', 95 | callback=cls._msg_received, 96 | qos_profile=qos_profile 97 | ) 98 | 99 | @classmethod 100 | def tearDownClass(cls): 101 | cls.node.destroy_subscription(cls.sub) 102 | rclpy.shutdown(context=cls.context) 103 | 104 | @classmethod 105 | def _msg_received(self, msg): 106 | # Callback for ROS subscriber used in the test 107 | self.msgs.append(msg) 108 | 109 | def get_message(self): 110 | # Try up to 60sec to receive messages 111 | startlen = len(self.msgs) 112 | 113 | executor = rclpy.executors.SingleThreadedExecutor(context=self.context) 114 | executor.add_node(self.node) 115 | 116 | try: 117 | end_time = time.time() + 60.0 118 | while time.time() < end_time: 119 | executor.spin_once(timeout_sec=0.1) 120 | if startlen != len(self.msgs): 121 | break 122 | 123 | self.assertNotEqual(startlen, len(self.msgs)) 124 | return self.msgs[-1] 125 | finally: 126 | executor.remove_node(self.node) 127 | 128 | def test_msg_rate(self): 129 | # Receive messages for 5 seconds - make sure we don't get too many or too few 130 | RUNTIME = 5.0 131 | start_time = time.time() 132 | end_time = start_time + RUNTIME 133 | 134 | while time.time() < end_time: 135 | self.get_message() 136 | 137 | self.assertGreater(len(self.msgs), 0) 138 | # self.assertGreater(len(self.msgs), RUNTIME) # At least 1 message per second 139 | # self.assertLess(len(self.msgs), RUNTIME * 9) # Fewer than 15 messages per second 140 | 141 | # def test_cpu_info(self): 142 | # # get current system information 143 | # system, node, release, version, machine, processor = platform.uname() 144 | # platform_supported = False 145 | # for pform in platforms: 146 | # if(platforms[pform]['system'] == system): 147 | # if(platforms[pform]['processor'] == processor): 148 | # platform_supported = True 149 | # self.assertEqual(multiprocessing.cpu_count(), platforms[pform]['cores']) 150 | # if(platforms[pform]['realtime']): 151 | # self.assertNotEqual(version.find('PREEMPT_RT'), -1) 152 | # 153 | # self.assertEqual(platform_supported, True) 154 | 155 | 156 | @post_shutdown_test() 157 | class TestReferenceSystemAutowareAfterShutdown(unittest.TestCase): 158 | 159 | def test_process_exit_codes(self): 160 | # Checks that all processes exited cleanly 161 | assertExitCodes(self.proc_info) 162 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # reference-system-autoware 2 | 3 | With the distributed development of ROS across many different organizations it is sometimes hard to benchmark and concretely show how a certain change to a certain system improves or reduces the performance of that system. For example did a change from one executor to another actually reduce the CPU or was it something else entirely? 4 | 5 | In order to try and address this problem we at [Apex.AI](https://apex.ai) would like to propose a definition of a [_reference system_](#reference-system) that simulates a real world scenario - in this case Autoware.Auto and its lidar data pipeline - that can be repeated no matter the underlying change of any piece of the full stack (i.e. executor, DDS or even RMW). 6 | 7 | ![Node graph of reference-system-autoware](content/img/dotgraph_autoware.svg) 8 | 9 | Future reference systems could be proposed that are more complex using the same basic node building blocks developed for this first scenario. 10 | 11 | ## Reference System 12 | 13 | A _reference system_ is defined by: 14 | - A [platform](#supported-platforms) is defined by: 15 | - Hardware (e.g. an off-the-shelf single-board computer, embedded ECU, etc.) 16 | - if there are multiple configurations available for such hardware, ensure it is specified 17 | - Operating System (OS) like RT linux, QNX, etc. along with any special configurations made 18 | - for simplicity and ease of benchmarking, **all nodes must run on a single process** 19 | - a fixed number of nodes 20 | - each node with: 21 | - a fixed number of publishers and subscribers 22 | - a fixed _processing time_ or a fixed _publishing rate_ 23 | - a fixed _message type_ of fixed size to be used for every _node_. 24 | 25 | With these defined attributes the _reference system_ can be replicated across many different possible configurations to be used to benchmark each configuration against the other in a reliable and fair manner. 26 | 27 | With this approach [unit tests](#unit-testing) can also be defined to reliably confirm if a given _reference system_ meets the requirements. 28 | 29 | ## Supported Platforms 30 | 31 | To enable as many people as possible to replicate this reference system, the platform(s) were chosen to be easily accessible (inexpensive, high volume), have lots of documentation, large community use and will be supported well into the future. 32 | 33 | Platforms were not chosen for performance of the reference system - we know we could run “faster” with a more powerful CPU or GPU but then it would be harder for others to validate findings and test their own configurations. Accessibility is the key here and will be considered if more platforms want to be added to this benchmark list. 34 | 35 | **Platforms:** 36 | - [Raspberry Pi 4B](https://www.raspberrypi.org/products/raspberry-pi-4-model-b/): 37 | - 4 GB RAM version is the assumed default 38 | - other versions could also be tested / added by the community 39 | - [real-time linux kernel](https://github.com/ros-realtime/rt-kernel-docker-builder) 40 | 41 | *Note: create an [issue](https://github.com/ros-realtime/reference-system-autoware/issues/) to add more platforms to the list, keeping in mind the above criteria* 42 | 43 | ## Concept Overview 44 | 45 | Rather than trying to write code to cover all potential variations of executors, APIs, and future features we cannot even imagine today we have chosen instead to define what we call a “reference system” based on part of a real-world system, [Autoware.Auto](https://www.autoware.org/autoware-auto). 46 | 47 | The above node graph can be boiled down to only a handful of node "types" that are replicated to make this complex system: 48 | 49 | **Node Types:** 50 | 51 | 1. [**Sensor Node**](reference_system/include/reference_system/nodes/rclcpp/sensor.hpp) 52 | - input node to system 53 | - publishes message cyclically at some fixed frequency 54 | 2. [**Transformer Node**](reference_system/include/reference_system/nodes/rclcpp/transformer.hpp) 55 | - one subscriber, one publisher 56 | - starts processing for N milliseconds after a message is received 57 | - publishes message after processing is complete 58 | 3. [**Fusion Node**](reference_system/include/reference_system/nodes/rclcpp/fusion.hpp) 59 | - two subscribers, one publisher 60 | - starts processing for N milliseconds after a message is received **from all** subscriptions 61 | - publishes message after processing is complete 62 | 4. [**Reactor Node**](reference_system/include/reference_system/nodes/rclcpp/reactor.hpp) 63 | - 0..N subscribers, one publisher 64 | - cyclically publishes at least one message or one for every received message 65 | 5. [**Command Node**](reference_system/include/reference_system/nodes/rclcpp/command.hpp) 66 | - prints output stats everytime a message is received 67 | 68 | These basic building-block nodes can be mixed-and-matched to create quite complex systems that replicate real-world scenarios to benchmark different configurations against each other. 69 | 70 | ## Reference Systems Overview 71 | 72 | The first reference system benchmark proposed is based on the *Autoware.Auto* lidar data pipeline as stated above and shown in the node graph image above as well. 73 | 74 | 1. [**Reference System Autoware.Auto**](reference_system/reference_system_autoware.md) 75 | - ROS2: 76 | - Executors: 77 | - Default: 78 | - [Single Threaded](reference_system/src/ros2/executor/autoware_default_singlethreaded.cpp) 79 | - [Sttaic Singe Threaded](reference_system/src/ros2/executor/autoware_default_staticsinglethreaded.cpp) 80 | - [Multithreaded](reference_system/src/ros2/executor/autoware_default_multithreaded.cpp) 81 | 82 | Results below show various characteristics of the same simulated system (Autoware.Auto). 83 | 84 | To add your own executor / middleware / configuration to the list above follow the *Contributing* section below. 85 | 86 | ## Benchmark Results 87 | 88 | Results will be added to different tagged releases along with the specific configurations ran during the tests. 89 | 90 | ## Unit Testing 91 | 92 | Unit tests can be written for the _reference system_ to check if all nodes, topics and other requirements are met before accepting test results. 93 | 94 | 95 | Tests are provided to automatically generate results for you by running `colcon test` on a supported platform above. 96 | 97 | To run the test, simply run the following command from your workspace: 98 | 99 | ``` 100 | colcon test --packages-up-to reference_system_autoware 101 | ``` 102 | 103 | Alternatively if for some reason you do not or cannot use `colcon` the tests are simple `gtests` that can be ported and ran on any configuration. 104 | 105 | 106 | ## Contributing 107 | 108 | If you see a missing configuration on the list above that you would like to see benchmarked against please follow the steps below to request it to be added. 109 | 110 | - look over the open / closed [issues](https://github.com/ros-realtime/reference-system-autoware/issues/) to make sure there isn't already an open ticket for the configuration you are looking for 111 | create `include/reference_system/MY_EXECUTOR_NAME_nodes` 112 | ## Howto Implement Your Custom Executor 113 | 114 | 1. Read over [the above documentation](#concept-overview) on the base node types 115 | 2. Review the base [`rclcpp nodes`](include/reference_system/nodes/rclcpp) that are provided and determine if your executor can use them 116 | 3. If you cannot, implment your own version of each base node type and place the source in [`include/reference_system/nodes`](include/reference_system/nodes) 117 | 4. Add your new nodes as a seperate `node system` in [`include/reference_system/system/systems.hpp`](include/reference_system/system/systems.hpp) 118 | 5. Copy one of the provided example `.cpp` files from the [`src/ros2/executor`](src/ros2/executor) directory and replace the `create_autoware_nodes` template type with your new `node system` which should be in the `system/systems.hpp` file already included 119 | 6. Add new `.cpp` source file as a new executable in the `CMakelist.txt` 120 | 7. Add new executable to test wtihin the `CMakelist.txt` 121 | 8. Build and run tests! 122 | 123 | 124 | -------------------------------------------------------------------------------- /autoware_reference_system/include/autoware_reference_system/autoware_system_builder.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef AUTOWARE_REFERENCE_SYSTEM__AUTOWARE_SYSTEM_BUILDER_HPP_ 15 | #define AUTOWARE_REFERENCE_SYSTEM__AUTOWARE_SYSTEM_BUILDER_HPP_ 16 | #include 17 | #include 18 | #include 19 | 20 | #include "reference_system/nodes/settings.hpp" 21 | #include "reference_system/system/systems.hpp" 22 | 23 | using namespace std::chrono_literals; // NOLINT 24 | 25 | template 26 | auto create_autoware_nodes() 27 | ->std::vector> 28 | { 29 | std::vector> nodes; 30 | 31 | // ignore the warning about designated initializers - they make the code much 32 | // more readable 33 | #pragma GCC diagnostic push 34 | #pragma GCC diagnostic ignored "-Wpedantic" 35 | 36 | // setup communication graph 37 | // sensor nodes 38 | nodes.emplace_back( 39 | std::make_shared( 40 | nodes::SensorSettings{.node_name = "FrontLidarDriver", 41 | .topic_name = "FrontLidarDriver", 42 | .cycle_time = TimingConfig::FRONT_LIDAR_DRIVER})); 43 | 44 | nodes.emplace_back( 45 | std::make_shared( 46 | nodes::SensorSettings{.node_name = "RearLidarDriver", 47 | .topic_name = "RearLidarDriver", 48 | .cycle_time = TimingConfig::REAR_LIDAR_DRIVER})); 49 | 50 | nodes.emplace_back( 51 | std::make_shared( 52 | nodes::SensorSettings{.node_name = "PointCloudMap", 53 | .topic_name = "PointCloudMap", 54 | .cycle_time = TimingConfig::POINT_CLOUD_MAP})); 55 | 56 | nodes.emplace_back( 57 | std::make_shared( 58 | nodes::SensorSettings{.node_name = "rviz2", 59 | .topic_name = "rviz2", 60 | .cycle_time = TimingConfig::RVIZ2})); 61 | 62 | nodes.emplace_back( 63 | std::make_shared( 64 | nodes::SensorSettings{.node_name = "Lanelet2Map", 65 | .topic_name = "Lanelet2Map", 66 | .cycle_time = TimingConfig::LANELET2MAP})); 67 | 68 | // processing nodes 69 | nodes.emplace_back( 70 | std::make_shared( 71 | nodes::TransformerSettings{ 72 | .node_name = "PointsTransformerFront", 73 | .input_topic = "FrontLidarDriver", 74 | .output_topic = "PointsTransformerFront", 75 | .number_crunch_time = TimingConfig::POINTS_TRANSFORMER_FRONT})); 76 | 77 | nodes.emplace_back( 78 | std::make_shared( 79 | nodes::TransformerSettings{ 80 | .node_name = "PointsTransformerRear", 81 | .input_topic = "RearLidarDriver", 82 | .output_topic = "PointsTransformerRear", 83 | .number_crunch_time = TimingConfig::POINTS_TRANSFORMER_REAR})); 84 | 85 | nodes.emplace_back( 86 | std::make_shared( 87 | nodes::TransformerSettings{ 88 | .node_name = "VoxelGridDownsampler", 89 | .input_topic = "PointCloudFusion", 90 | .output_topic = "VoxelGridDownsampler", 91 | .number_crunch_time = TimingConfig::VOXEL_GRID_DOWNSAMPLER})); 92 | 93 | nodes.emplace_back( 94 | std::make_shared( 95 | nodes::TransformerSettings{ 96 | .node_name = "PointCloudMapLoader", 97 | .input_topic = "PointCloudMap", 98 | .output_topic = "PointCloudMapLoader", 99 | .number_crunch_time = TimingConfig::POINT_CLOUD_MAP_LOADER})); 100 | 101 | nodes.emplace_back( 102 | std::make_shared( 103 | nodes::TransformerSettings{ 104 | .node_name = "RayGroundFilter", 105 | .input_topic = "PointCloudFusion", 106 | .output_topic = "RayGroundFilter", 107 | .number_crunch_time = TimingConfig::RAY_GROUND_FILTER})); 108 | 109 | nodes.emplace_back( 110 | std::make_shared( 111 | nodes::TransformerSettings{ 112 | .node_name = "EuclideanClusterDetector", 113 | .input_topic = "RayGroundFilter", 114 | .output_topic = "EuclideanClusterDetector", 115 | .number_crunch_time = TimingConfig::EUCLIDEAN_CLUSTER_DETECTOR})); 116 | 117 | nodes.emplace_back( 118 | std::make_shared( 119 | nodes::TransformerSettings{ 120 | .node_name = "ObjectCollisionEstimator", 121 | .input_topic = "EuclideanClusterDetector", 122 | .output_topic = "ObjectCollisionEstimator", 123 | .number_crunch_time = TimingConfig::OBJECT_COLLISION_ESTIMATOR})); 124 | 125 | nodes.emplace_back( 126 | std::make_shared( 127 | nodes::TransformerSettings{ 128 | .node_name = "MPCController", 129 | .input_topic = "BehaviorPlanner", 130 | .output_topic = "MPCController", 131 | .number_crunch_time = TimingConfig::MPC_CONTROLLER})); 132 | 133 | nodes.emplace_back( 134 | std::make_shared( 135 | nodes::TransformerSettings{ 136 | .node_name = "ParkingPlanner", 137 | .input_topic = "Lanelet2MapLoader", 138 | .output_topic = "ParkingPlanner", 139 | .number_crunch_time = TimingConfig::PARKING_PLANNER})); 140 | 141 | nodes.emplace_back( 142 | std::make_shared( 143 | nodes::TransformerSettings{ 144 | .node_name = "LanePlanner", 145 | .input_topic = "Lanelet2MapLoader", 146 | .output_topic = "LanePlanner", 147 | .number_crunch_time = TimingConfig::LANE_PLANNER})); 148 | 149 | // fusion nodes 150 | nodes.emplace_back( 151 | std::make_shared( 152 | nodes::FusionSettings{ 153 | .node_name = "PointCloudFusion", 154 | .input_0 = "PointsTransformerFront", 155 | .input_1 = "PointsTransformerRear", 156 | .output_topic = "PointCloudFusion", 157 | .number_crunch_time = TimingConfig::POINT_CLOUD_FUSION, 158 | .max_input_time_difference = TimingConfig::POINT_CLOUD_FUSION_MAX_INPUT_TIME_DIFF})); 159 | 160 | nodes.emplace_back( 161 | std::make_shared( 162 | nodes::FusionSettings{ 163 | .node_name = "NDTLocalizer", 164 | .input_0 = "VoxelGridDownsampler", 165 | .input_1 = "PointCloudMapLoader", 166 | .output_topic = "NDTLocalizer", 167 | .number_crunch_time = TimingConfig::NDT_LOCALIZER, 168 | .max_input_time_difference = TimingConfig::NDT_LOCALIZER_MAX_INPUT_TIME_DIFF})); 169 | 170 | nodes.emplace_back( 171 | std::make_shared( 172 | nodes::FusionSettings{ 173 | .node_name = "VehicleInterface", 174 | .input_0 = "MPCController", 175 | .input_1 = "BehaviorPlanner", 176 | .output_topic = "VehicleInterface", 177 | .number_crunch_time = TimingConfig::VEHICLE_INTERFACE, 178 | .max_input_time_difference = TimingConfig::VEHICLE_INTERFACE_MAX_INPUT_TIME_DIFF })); 179 | 180 | nodes.emplace_back( 181 | std::make_shared( 182 | nodes::FusionSettings{ 183 | .node_name = "Lanelet2GlobalPlanner", 184 | .input_0 = "rviz2", 185 | .input_1 = "NDTLocalizer", 186 | .output_topic = "Lanelet2GlobalPlanner", 187 | .number_crunch_time = TimingConfig::LANELET_2_GLOBAL_PLANNER, 188 | .max_input_time_difference = TimingConfig::LANELET_2_GLOBAL_PLANNER_MAX_INPUT_TIME_DIFF})); 189 | 190 | nodes.emplace_back( 191 | std::make_shared( 192 | nodes::FusionSettings{ 193 | .node_name = "Lanelet2MapLoader", 194 | .input_0 = "Lanelet2Map", 195 | .input_1 = "Lanelet2GlobalPlanner", 196 | .output_topic = "Lanelet2MapLoader", 197 | .number_crunch_time = TimingConfig::LANELET_2_MAP_LOADER, 198 | .max_input_time_difference = TimingConfig::LANELET_2_MAP_LOADER_MAX_INPUT_TIME_DIFF})); 199 | 200 | // reactor node 201 | nodes.emplace_back( 202 | std::make_shared( 203 | nodes::ReactorSettings{ 204 | .node_name = "BehaviorPlanner", 205 | .inputs = {"ObjectCollisionEstimator", "NDTLocalizer", 206 | "Lanelet2GlobalPlanner", "Lanelet2MapLoader", 207 | "ParkingPlanner", "LanePlanner"}, 208 | .output_topic = "BehaviorPlanner", 209 | .cycle_time = TimingConfig::BEHAVIOR_PLANNER})); 210 | 211 | // command node 212 | nodes.emplace_back( 213 | std::make_shared( 214 | nodes::CommandSettings{ 215 | .node_name = "VehicleDBWSystem", .input_topic = "VehicleInterface"})); 216 | #pragma GCC diagnostic pop 217 | 218 | return nodes; 219 | } 220 | 221 | #endif // AUTOWARE_REFERENCE_SYSTEM__AUTOWARE_SYSTEM_BUILDER_HPP_ 222 | -------------------------------------------------------------------------------- /autoware_reference_system/include/autoware_reference_system/system/timing/benchmark.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Apex.AI, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef AUTOWARE_REFERENCE_SYSTEM__SYSTEM__TIMING__BENCHMARK_HPP_ 15 | #define AUTOWARE_REFERENCE_SYSTEM__SYSTEM__TIMING__BENCHMARK_HPP_ 16 | #include 17 | 18 | namespace nodes 19 | { 20 | namespace timing 21 | { 22 | 23 | struct BenchmarkThroughput 24 | { 25 | using time_t = std::chrono::nanoseconds; 26 | using milliseconds = std::chrono::milliseconds; 27 | using seconds = std::chrono::seconds; 28 | 29 | // sensors 30 | static constexpr time_t FRONT_LIDAR_DRIVER = milliseconds(0); 31 | static constexpr time_t REAR_LIDAR_DRIVER = milliseconds(0); 32 | static constexpr time_t POINT_CLOUD_MAP = milliseconds(0); 33 | static constexpr time_t RVIZ2 = milliseconds(0); 34 | static constexpr time_t LANELET2MAP = milliseconds(0); 35 | 36 | // processing 37 | static constexpr time_t POINTS_TRANSFORMER_FRONT = milliseconds(0); 38 | static constexpr time_t POINTS_TRANSFORMER_REAR = milliseconds(0); 39 | static constexpr time_t VOXEL_GRID_DOWNSAMPLER = milliseconds(0); 40 | static constexpr time_t POINT_CLOUD_MAP_LOADER = milliseconds(0); 41 | static constexpr time_t RAY_GROUND_FILTER = milliseconds(0); 42 | static constexpr time_t EUCLIDEAN_CLUSTER_DETECTOR = milliseconds(0); 43 | static constexpr time_t OBJECT_COLLISION_ESTIMATOR = milliseconds(0); 44 | static constexpr time_t MPC_CONTROLLER = milliseconds(0); 45 | static constexpr time_t PARKING_PLANNER = milliseconds(0); 46 | static constexpr time_t LANE_PLANNER = milliseconds(0); 47 | 48 | // fusion 49 | static constexpr time_t POINT_CLOUD_FUSION = milliseconds(0); 50 | static constexpr time_t NDT_LOCALIZER = milliseconds(0); 51 | static constexpr time_t VEHICLE_INTERFACE = milliseconds(0); 52 | static constexpr time_t LANELET_2_GLOBAL_PLANNER = milliseconds(0); 53 | static constexpr time_t LANELET_2_MAP_LOADER = milliseconds(0); 54 | 55 | static constexpr time_t POINT_CLOUD_FUSION_MAX_INPUT_TIME_DIFF = seconds(9999); 56 | static constexpr time_t NDT_LOCALIZER_MAX_INPUT_TIME_DIFF = seconds(9999); 57 | static constexpr time_t VEHICLE_INTERFACE_MAX_INPUT_TIME_DIFF = seconds(9999); 58 | static constexpr time_t LANELET_2_GLOBAL_PLANNER_MAX_INPUT_TIME_DIFF = seconds(9999); 59 | static constexpr time_t LANELET_2_MAP_LOADER_MAX_INPUT_TIME_DIFF = seconds(9999); 60 | 61 | // reactor 62 | static constexpr time_t BEHAVIOR_PLANNER = milliseconds(0); 63 | }; 64 | 65 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::FRONT_LIDAR_DRIVER; 66 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::REAR_LIDAR_DRIVER; 67 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::POINT_CLOUD_MAP; 68 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::RVIZ2; 69 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::LANELET2MAP; 70 | constexpr BenchmarkThroughput::time_t 71 | BenchmarkThroughput::POINTS_TRANSFORMER_FRONT; 72 | constexpr BenchmarkThroughput::time_t 73 | BenchmarkThroughput::POINTS_TRANSFORMER_REAR; 74 | constexpr BenchmarkThroughput::time_t 75 | BenchmarkThroughput::VOXEL_GRID_DOWNSAMPLER; 76 | constexpr BenchmarkThroughput::time_t 77 | BenchmarkThroughput::POINT_CLOUD_MAP_LOADER; 78 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::RAY_GROUND_FILTER; 79 | constexpr BenchmarkThroughput::time_t 80 | BenchmarkThroughput::EUCLIDEAN_CLUSTER_DETECTOR; 81 | constexpr BenchmarkThroughput::time_t 82 | BenchmarkThroughput::OBJECT_COLLISION_ESTIMATOR; 83 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::MPC_CONTROLLER; 84 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::PARKING_PLANNER; 85 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::LANE_PLANNER; 86 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::POINT_CLOUD_FUSION; 87 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::NDT_LOCALIZER; 88 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::VEHICLE_INTERFACE; 89 | constexpr BenchmarkThroughput::time_t 90 | BenchmarkThroughput::LANELET_2_GLOBAL_PLANNER; 91 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::LANELET_2_MAP_LOADER; 92 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::POINT_CLOUD_FUSION_MAX_INPUT_TIME_DIFF; 93 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::NDT_LOCALIZER_MAX_INPUT_TIME_DIFF; 94 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::VEHICLE_INTERFACE_MAX_INPUT_TIME_DIFF; 95 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::LANELET_2_GLOBAL_PLANNER_MAX_INPUT_TIME_DIFF; 96 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::LANELET_2_MAP_LOADER_MAX_INPUT_TIME_DIFF; 97 | constexpr BenchmarkThroughput::time_t BenchmarkThroughput::BEHAVIOR_PLANNER; 98 | 99 | struct BenchmarkCPUUsage 100 | { 101 | using time_t = std::chrono::nanoseconds; 102 | using milliseconds = std::chrono::milliseconds; 103 | using seconds = std::chrono::seconds; 104 | 105 | // sensors 106 | static constexpr time_t FRONT_LIDAR_DRIVER = milliseconds(50); 107 | static constexpr time_t REAR_LIDAR_DRIVER = milliseconds(50); 108 | static constexpr time_t POINT_CLOUD_MAP = milliseconds(50); 109 | static constexpr time_t RVIZ2 = milliseconds(50); 110 | static constexpr time_t LANELET2MAP = milliseconds(50); 111 | 112 | // processing 113 | static constexpr time_t POINTS_TRANSFORMER_FRONT = milliseconds(0); 114 | static constexpr time_t POINTS_TRANSFORMER_REAR = milliseconds(0); 115 | static constexpr time_t VOXEL_GRID_DOWNSAMPLER = milliseconds(0); 116 | static constexpr time_t POINT_CLOUD_MAP_LOADER = milliseconds(0); 117 | static constexpr time_t RAY_GROUND_FILTER = milliseconds(0); 118 | static constexpr time_t EUCLIDEAN_CLUSTER_DETECTOR = milliseconds(0); 119 | static constexpr time_t OBJECT_COLLISION_ESTIMATOR = milliseconds(0); 120 | static constexpr time_t MPC_CONTROLLER = milliseconds(0); 121 | static constexpr time_t PARKING_PLANNER = milliseconds(0); 122 | static constexpr time_t LANE_PLANNER = milliseconds(0); 123 | 124 | // fusion 125 | static constexpr time_t POINT_CLOUD_FUSION = milliseconds(0); 126 | static constexpr time_t NDT_LOCALIZER = milliseconds(0); 127 | static constexpr time_t VEHICLE_INTERFACE = milliseconds(0); 128 | static constexpr time_t LANELET_2_GLOBAL_PLANNER = milliseconds(0); 129 | static constexpr time_t LANELET_2_MAP_LOADER = milliseconds(0); 130 | 131 | static constexpr time_t POINT_CLOUD_FUSION_MAX_INPUT_TIME_DIFF = seconds(9999); 132 | static constexpr time_t NDT_LOCALIZER_MAX_INPUT_TIME_DIFF = seconds(9999); 133 | static constexpr time_t VEHICLE_INTERFACE_MAX_INPUT_TIME_DIFF = seconds(9999); 134 | static constexpr time_t LANELET_2_GLOBAL_PLANNER_MAX_INPUT_TIME_DIFF = seconds(9999); 135 | static constexpr time_t LANELET_2_MAP_LOADER_MAX_INPUT_TIME_DIFF = seconds(9999); 136 | 137 | // reactor 138 | static constexpr time_t BEHAVIOR_PLANNER = milliseconds(0); 139 | }; 140 | 141 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::FRONT_LIDAR_DRIVER; 142 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::REAR_LIDAR_DRIVER; 143 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::POINT_CLOUD_MAP; 144 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::RVIZ2; 145 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::LANELET2MAP; 146 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::POINTS_TRANSFORMER_FRONT; 147 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::POINTS_TRANSFORMER_REAR; 148 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::VOXEL_GRID_DOWNSAMPLER; 149 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::POINT_CLOUD_MAP_LOADER; 150 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::RAY_GROUND_FILTER; 151 | constexpr BenchmarkCPUUsage::time_t 152 | BenchmarkCPUUsage::EUCLIDEAN_CLUSTER_DETECTOR; 153 | constexpr BenchmarkCPUUsage::time_t 154 | BenchmarkCPUUsage::OBJECT_COLLISION_ESTIMATOR; 155 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::MPC_CONTROLLER; 156 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::PARKING_PLANNER; 157 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::LANE_PLANNER; 158 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::POINT_CLOUD_FUSION; 159 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::NDT_LOCALIZER; 160 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::VEHICLE_INTERFACE; 161 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::LANELET_2_GLOBAL_PLANNER; 162 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::LANELET_2_MAP_LOADER; 163 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::POINT_CLOUD_FUSION_MAX_INPUT_TIME_DIFF; 164 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::NDT_LOCALIZER_MAX_INPUT_TIME_DIFF; 165 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::VEHICLE_INTERFACE_MAX_INPUT_TIME_DIFF; 166 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::LANELET_2_GLOBAL_PLANNER_MAX_INPUT_TIME_DIFF; 167 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::LANELET_2_MAP_LOADER_MAX_INPUT_TIME_DIFF; 168 | constexpr BenchmarkCPUUsage::time_t BenchmarkCPUUsage::BEHAVIOR_PLANNER; 169 | 170 | } // namespace timing 171 | } // namespace nodes 172 | #endif // AUTOWARE_REFERENCE_SYSTEM__SYSTEM__TIMING__BENCHMARK_HPP_ 173 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /content/img/dotgraph_autoware.svg: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 9 | 10 | G 11 | 12 | reference_system_autoware 13 | 14 | 15 | Front Lidar Driver 16 | 17 | Front Lidar Driver 18 | 19 | 20 | 21 | Front Points Transformer 22 | 23 | Front Points Transformer 24 | 25 | 26 | 27 | Front Lidar Driver->Front Points Transformer 28 | 29 | 30 | 31 | 32 | 33 | Rear Lidar Driver 34 | 35 | Rear Lidar Driver 36 | 37 | 38 | 39 | Rear Points Transformer 40 | 41 | Rear Points Transformer 42 | 43 | 44 | 45 | Rear Lidar Driver->Rear Points Transformer 46 | 47 | 48 | 49 | 50 | 51 | Point Cloud Fusion 52 | 53 | Point Cloud Fusion 54 | 55 | 56 | 57 | Front Points Transformer->Point Cloud Fusion 58 | 59 | 60 | 61 | 62 | 63 | Rear Points Transformer->Point Cloud Fusion 64 | 65 | 66 | 67 | 68 | 69 | Ray Ground Filter 70 | 71 | Ray Ground Filter 72 | 73 | 74 | 75 | Point Cloud Fusion->Ray Ground Filter 76 | 77 | 78 | 79 | 80 | 81 | Voxel Grid Downsampler 82 | 83 | Voxel Grid Downsampler 84 | 85 | 86 | 87 | Point Cloud Fusion->Voxel Grid Downsampler 88 | 89 | 90 | 91 | 92 | 93 | Euclidean Cluster Detector 94 | 95 | Euclidean Cluster Detector 96 | 97 | 98 | 99 | Ray Ground Filter->Euclidean Cluster Detector 100 | 101 | 102 | 103 | 104 | 105 | Object Collision Estimator 106 | 107 | Object Collision Estimator 108 | 109 | 110 | 111 | Euclidean Cluster Detector->Object Collision Estimator 112 | 113 | 114 | 115 | 116 | 117 | Lanelet2 Map 118 | 119 | 120 | Lanelet2 Map 121 | 122 | 123 | 124 | Lanelet2 Map Loader 125 | 126 | Lanelet2 Map Loader 127 | 128 | 129 | 130 | Lanelet2 Map->Lanelet2 Map Loader 131 | 132 | 133 | 134 | 135 | 136 | Behavior Planner 137 | 138 | Behavior Planner 139 | 140 | 141 | 142 | Object Collision Estimator->Behavior Planner 143 | 144 | 145 | 146 | 147 | 148 | MPC Controller 149 | 150 | MPC Controller 151 | 152 | 153 | 154 | Behavior Planner->MPC Controller 155 | 156 | 157 | 158 | 159 | 160 | Vehicle Interface 161 | 162 | Vehicle Interface 163 | 164 | 165 | 166 | Behavior Planner->Vehicle Interface 167 | 168 | 169 | 170 | 171 | 172 | Lane Planner 173 | 174 | Lane Planner 175 | 176 | 177 | 178 | Lanelet2 Map Loader->Lane Planner 179 | 180 | 181 | 182 | 183 | 184 | Parking Planner 185 | 186 | Parking Planner 187 | 188 | 189 | 190 | Lanelet2 Map Loader->Parking Planner 191 | 192 | 193 | 194 | 195 | 196 | Lanelet2 Global Planner 197 | 198 | Lanelet2 Global Planner 199 | 200 | 201 | 202 | Lanelet2 Global Planner->Behavior Planner 203 | 204 | 205 | 206 | 207 | 208 | Lanelet2 Global Planner->Lanelet2 Map Loader 209 | 210 | 211 | 212 | 213 | 214 | Lane Planner->Behavior Planner 215 | 216 | 217 | 218 | 219 | 220 | Parking Planner->Behavior Planner 221 | 222 | 223 | 224 | 225 | 226 | MPC Controller->Vehicle Interface 227 | 228 | 229 | 230 | 231 | 232 | Vehicle DBW System 233 | 234 | Vehicle DBW System 235 | 236 | 237 | 238 | Vehicle Interface->Vehicle DBW System 239 | 240 | 241 | 242 | 243 | 244 | Point Cloud Map 245 | 246 | 247 | Point Cloud Map 248 | 249 | 250 | 251 | Point Cloud Map Loader 252 | 253 | Point Cloud Map Loader 254 | 255 | 256 | 257 | Point Cloud Map->Point Cloud Map Loader 258 | 259 | 260 | 261 | 262 | 263 | NDT Localizer 264 | 265 | NDT Localizer 266 | 267 | 268 | 269 | Point Cloud Map Loader->NDT Localizer 270 | 271 | 272 | 273 | 274 | 275 | Voxel Grid Downsampler->NDT Localizer 276 | 277 | 278 | 279 | 280 | 281 | NDT Localizer->Behavior Planner 282 | 283 | 284 | 285 | 286 | 287 | NDT Localizer->Lanelet2 Global Planner 288 | 289 | 290 | 291 | 292 | 293 | rviz2 or web interface 294 | 295 | 296 | 297 | 298 | rviz2 or web interface 299 | 300 | 301 | 302 | rviz2 or web interface->Lanelet2 Global Planner 303 | 304 | 305 | 306 | 307 | 308 | --------------------------------------------------------------------------------