├── .gitignore ├── CMakeLists.txt.ros1 ├── CMakeLists.txt.ros2 ├── LICENSE ├── README.md ├── cmake ├── common_robotics_utilities-dependencies.cmake └── common_robotics_utilities-extras.cmake ├── example ├── clustering_example.cpp └── dtw_example.cpp ├── include └── common_robotics_utilities │ ├── base64_helpers.hpp │ ├── color_builder.hpp │ ├── conversions.hpp │ ├── cru_namespace.hpp │ ├── dynamic_spatial_hashed_voxel_grid.hpp │ ├── gaussian_distributions.hpp │ ├── math.hpp │ ├── maybe.hpp │ ├── openmp_helpers.hpp │ ├── parallelism.hpp │ ├── path_processing.hpp │ ├── print.hpp │ ├── random_rotation_generator.hpp │ ├── ros_conversions.hpp │ ├── ros_helpers.hpp │ ├── serialization.hpp │ ├── simple_astar_search.hpp │ ├── simple_dtw.hpp │ ├── simple_graph.hpp │ ├── simple_graph_search.hpp │ ├── simple_hausdorff_distance.hpp │ ├── simple_hierarchical_clustering.hpp │ ├── simple_kmeans_clustering.hpp │ ├── simple_knearest_neighbors.hpp │ ├── simple_prm_planner.hpp │ ├── simple_prngs.hpp │ ├── simple_robot_model_interface.hpp │ ├── simple_rrt_planner.hpp │ ├── simple_task_planner.hpp │ ├── time_optimal_trajectory_parametrization.hpp │ ├── utility.hpp │ ├── voxel_grid.hpp │ └── zlib_helpers.hpp ├── package.xml.ros1 ├── package.xml.ros2 ├── src └── common_robotics_utilities │ ├── base64_helpers.cpp │ ├── conversions.cpp │ ├── math.cpp │ ├── ros_conversions.cpp │ ├── serialization.cpp │ ├── time_optimal_trajectory_parametrization.cpp │ └── zlib_helpers.cpp └── test ├── hausdorff_distance_test.cpp ├── math_test.cpp ├── maybe_test.cpp ├── parallelism_test.cpp ├── planning_test.cpp ├── print_test.cpp ├── ros_helpers_test.cpp ├── task_planning_test.cpp ├── utility_test.cpp └── voxel_grid_test.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt 2 | package.xml 3 | -------------------------------------------------------------------------------- /CMakeLists.txt.ros1: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(common_robotics_utilities) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs visualization_msgs) 8 | find_package(Eigen3 REQUIRED) 9 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 10 | find_package(OpenMP) 11 | 12 | ## We don't depend on Drake, but we do use different build flags if present. 13 | find_package(drake QUIET) 14 | 15 | ################################### 16 | ## catkin specific configuration ## 17 | ################################### 18 | ## The catkin_package macro generates cmake config files for your package 19 | ## Declare things to be passed to dependent projects 20 | ## LIBRARIES: libraries you create in this project 21 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 22 | ## DEPENDS: system dependencies of this project 23 | catkin_package(INCLUDE_DIRS 24 | include 25 | LIBRARIES 26 | ${PROJECT_NAME} 27 | CATKIN_DEPENDS 28 | roscpp 29 | geometry_msgs 30 | visualization_msgs 31 | DEPENDS 32 | Eigen3 33 | CFG_EXTRAS ${PROJECT_NAME}-extras.cmake) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | ## Specify additional locations of header files 40 | ## Your package locations should be listed before other locations 41 | include_directories(include SYSTEM ${catkin_INCLUDE_DIRS} 42 | ${Eigen3_INCLUDE_DIRS}) 43 | 44 | ## Build options 45 | set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) 46 | cmake_policy(SET CMP0069 NEW) 47 | 48 | add_compile_options(-std=c++11) 49 | add_compile_options(-Wall) 50 | add_compile_options(-Wextra) 51 | add_compile_options(-Werror) 52 | add_compile_options(-Wconversion) 53 | add_compile_options(-Wshadow) 54 | add_compile_options(-O3) 55 | add_compile_options(-g) 56 | add_compile_options(-Werror=non-virtual-dtor) 57 | add_compile_options(-Wold-style-cast) 58 | add_compile_options(-Wpessimizing-move) 59 | add_compile_options(-Wuninitialized) 60 | add_compile_options(-Wmissing-declarations) 61 | 62 | if(drake_FOUND) 63 | message(STATUS "Drake found, disabling -march=native") 64 | else() 65 | message(STATUS "Drake NOT found, enabling -march=native") 66 | add_compile_options(-march=native) 67 | endif() 68 | 69 | add_definitions(-DCOMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION=1) 70 | 71 | ## It's not clear if add_compile_options does the right things for flags that 72 | ## may differ between languages and target type. 73 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 74 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 75 | set(CMAKE_EXE_LINKER_FLAGS 76 | "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 77 | set(CMAKE_SHARED_LINKER_FLAGS 78 | "${CMAKE_SHARED_LINKER_FLAGS} ${OpenMP_SHARED_LINKER_FLAGS}") 79 | 80 | # Utility library 81 | add_library(${PROJECT_NAME} 82 | include/${PROJECT_NAME}/base64_helpers.hpp 83 | include/${PROJECT_NAME}/color_builder.hpp 84 | include/${PROJECT_NAME}/conversions.hpp 85 | include/${PROJECT_NAME}/cru_namespace.hpp 86 | include/${PROJECT_NAME}/dynamic_spatial_hashed_voxel_grid.hpp 87 | include/${PROJECT_NAME}/gaussian_distributions.hpp 88 | include/${PROJECT_NAME}/math.hpp 89 | include/${PROJECT_NAME}/maybe.hpp 90 | include/${PROJECT_NAME}/openmp_helpers.hpp 91 | include/${PROJECT_NAME}/parallelism.hpp 92 | include/${PROJECT_NAME}/path_processing.hpp 93 | include/${PROJECT_NAME}/print.hpp 94 | include/${PROJECT_NAME}/random_rotation_generator.hpp 95 | include/${PROJECT_NAME}/ros_conversions.hpp 96 | include/${PROJECT_NAME}/ros_helpers.hpp 97 | include/${PROJECT_NAME}/serialization.hpp 98 | include/${PROJECT_NAME}/simple_astar_search.hpp 99 | include/${PROJECT_NAME}/simple_dtw.hpp 100 | include/${PROJECT_NAME}/simple_graph.hpp 101 | include/${PROJECT_NAME}/simple_graph_search.hpp 102 | include/${PROJECT_NAME}/simple_hausdorff_distance.hpp 103 | include/${PROJECT_NAME}/simple_hierarchical_clustering.hpp 104 | include/${PROJECT_NAME}/simple_kmeans_clustering.hpp 105 | include/${PROJECT_NAME}/simple_knearest_neighbors.hpp 106 | include/${PROJECT_NAME}/simple_prm_planner.hpp 107 | include/${PROJECT_NAME}/simple_prngs.hpp 108 | include/${PROJECT_NAME}/simple_robot_model_interface.hpp 109 | include/${PROJECT_NAME}/simple_rrt_planner.hpp 110 | include/${PROJECT_NAME}/simple_task_planner.hpp 111 | include/${PROJECT_NAME}/time_optimal_trajectory_parametrization.hpp 112 | include/${PROJECT_NAME}/utility.hpp 113 | include/${PROJECT_NAME}/voxel_grid.hpp 114 | include/${PROJECT_NAME}/zlib_helpers.hpp 115 | src/${PROJECT_NAME}/base64_helpers.cpp 116 | src/${PROJECT_NAME}/conversions.cpp 117 | src/${PROJECT_NAME}/math.cpp 118 | src/${PROJECT_NAME}/ros_conversions.cpp 119 | src/${PROJECT_NAME}/serialization.cpp 120 | src/${PROJECT_NAME}/time_optimal_trajectory_parametrization.cpp 121 | src/${PROJECT_NAME}/zlib_helpers.cpp) 122 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 123 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} z) 124 | 125 | # Examples 126 | add_executable(clustering_example example/clustering_example.cpp) 127 | add_dependencies(clustering_example ${PROJECT_NAME}) 128 | target_link_libraries(clustering_example ${PROJECT_NAME}) 129 | 130 | add_executable(dtw_example example/dtw_example.cpp) 131 | add_dependencies(dtw_example ${PROJECT_NAME}) 132 | target_link_libraries(dtw_example ${PROJECT_NAME}) 133 | 134 | if(CATKIN_ENABLE_TESTING) 135 | # Tests 136 | catkin_add_gtest(hausdorff_distance_test test/hausdorff_distance_test.cpp) 137 | add_dependencies(hausdorff_distance_test ${PROJECT_NAME}) 138 | target_link_libraries(hausdorff_distance_test ${PROJECT_NAME}) 139 | 140 | catkin_add_gtest(math_test test/math_test.cpp) 141 | add_dependencies(math_test ${PROJECT_NAME}) 142 | target_link_libraries(math_test ${PROJECT_NAME}) 143 | 144 | catkin_add_gtest(maybe_test test/maybe_test.cpp) 145 | add_dependencies(maybe_test ${PROJECT_NAME}) 146 | target_link_libraries(maybe_test ${PROJECT_NAME}) 147 | 148 | catkin_add_gtest(parallelism_test test/parallelism_test.cpp) 149 | add_dependencies(parallelism_test ${PROJECT_NAME}) 150 | target_link_libraries(parallelism_test ${PROJECT_NAME}) 151 | 152 | catkin_add_gtest(planning_test test/planning_test.cpp) 153 | add_dependencies(planning_test ${PROJECT_NAME}) 154 | target_link_libraries(planning_test ${PROJECT_NAME}) 155 | 156 | catkin_add_gtest(task_planning_test test/task_planning_test.cpp) 157 | add_dependencies(task_planning_test ${PROJECT_NAME}) 158 | target_link_libraries(task_planning_test ${PROJECT_NAME}) 159 | 160 | catkin_add_gtest(ros_helpers_test test/ros_helpers_test.cpp) 161 | add_dependencies(ros_helpers_test ${PROJECT_NAME}) 162 | target_link_libraries(ros_helpers_test ${PROJECT_NAME}) 163 | 164 | catkin_add_gtest(utility_test test/utility_test.cpp) 165 | add_dependencies(utility_test ${PROJECT_NAME}) 166 | target_link_libraries(utility_test ${PROJECT_NAME}) 167 | 168 | catkin_add_gtest(voxel_grid_test test/voxel_grid_test.cpp) 169 | add_dependencies(voxel_grid_test ${PROJECT_NAME}) 170 | target_link_libraries(voxel_grid_test ${PROJECT_NAME}) 171 | 172 | catkin_add_gtest(print_test test/print_test.cpp) 173 | add_dependencies(print_test ${PROJECT_NAME}) 174 | target_link_libraries(print_test ${PROJECT_NAME}) 175 | endif() 176 | 177 | ############# 178 | ## Install ## 179 | ############# 180 | 181 | ## Mark library for installation 182 | install(TARGETS ${PROJECT_NAME} 183 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 184 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 185 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 186 | ) 187 | ## Mark cpp header files for installation 188 | install(DIRECTORY include/${PROJECT_NAME}/ 189 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 190 | FILES_MATCHING PATTERN "*.hpp" 191 | PATTERN ".svn" EXCLUDE 192 | ) 193 | -------------------------------------------------------------------------------- /CMakeLists.txt.ros2: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(common_robotics_utilities) 3 | 4 | find_package(ament_cmake_ros REQUIRED) 5 | 6 | find_package(rclcpp REQUIRED) 7 | find_package(geometry_msgs REQUIRED) 8 | find_package(visualization_msgs REQUIRED) 9 | 10 | find_package(Eigen3 REQUIRED) 11 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 12 | find_package(OpenMP) 13 | find_package(ZLIB REQUIRED) 14 | 15 | ## We don't depend on Drake, but we do use different build flags if present. 16 | find_package(drake QUIET) 17 | 18 | ########### 19 | ## Build ## 20 | ########### 21 | 22 | ## Specify additional locations of header files 23 | ## Your package locations should be listed before other locations 24 | include_directories(include SYSTEM ${Eigen3_INCLUDE_DIRS} ${ZLIB_INCLUDE_DIRS}) 25 | 26 | ## Build options 27 | set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) 28 | cmake_policy(SET CMP0069 NEW) 29 | 30 | add_compile_options(-std=c++17) 31 | add_compile_options(-Wall) 32 | add_compile_options(-Wextra) 33 | add_compile_options(-Werror) 34 | add_compile_options(-Wconversion) 35 | add_compile_options(-Wshadow) 36 | add_compile_options(-O3) 37 | add_compile_options(-g) 38 | add_compile_options(-Werror=non-virtual-dtor) 39 | add_compile_options(-Wold-style-cast) 40 | add_compile_options(-Wpessimizing-move) 41 | add_compile_options(-Wuninitialized) 42 | add_compile_options(-Wmissing-declarations) 43 | 44 | if(drake_FOUND) 45 | message(STATUS "Drake found, disabling -march=native") 46 | else() 47 | message(STATUS "Drake NOT found, enabling -march=native") 48 | add_compile_options(-march=native) 49 | endif() 50 | 51 | add_definitions(-DCOMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION=2) 52 | 53 | ## It's not clear if add_compile_options does the right things for flags that 54 | ## may differ between languages and target type. 55 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 56 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 57 | set(CMAKE_EXE_LINKER_FLAGS 58 | "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 59 | set(CMAKE_SHARED_LINKER_FLAGS 60 | "${CMAKE_SHARED_LINKER_FLAGS} ${OpenMP_SHARED_LINKER_FLAGS}") 61 | 62 | # Utility library 63 | add_library(${PROJECT_NAME} 64 | include/${PROJECT_NAME}/base64_helpers.hpp 65 | include/${PROJECT_NAME}/color_builder.hpp 66 | include/${PROJECT_NAME}/conversions.hpp 67 | include/${PROJECT_NAME}/cru_namespace.hpp 68 | include/${PROJECT_NAME}/dynamic_spatial_hashed_voxel_grid.hpp 69 | include/${PROJECT_NAME}/gaussian_distributions.hpp 70 | include/${PROJECT_NAME}/math.hpp 71 | include/${PROJECT_NAME}/maybe.hpp 72 | include/${PROJECT_NAME}/openmp_helpers.hpp 73 | include/${PROJECT_NAME}/parallelism.hpp 74 | include/${PROJECT_NAME}/path_processing.hpp 75 | include/${PROJECT_NAME}/print.hpp 76 | include/${PROJECT_NAME}/random_rotation_generator.hpp 77 | include/${PROJECT_NAME}/ros_conversions.hpp 78 | include/${PROJECT_NAME}/ros_helpers.hpp 79 | include/${PROJECT_NAME}/serialization.hpp 80 | include/${PROJECT_NAME}/simple_astar_search.hpp 81 | include/${PROJECT_NAME}/simple_dtw.hpp 82 | include/${PROJECT_NAME}/simple_graph.hpp 83 | include/${PROJECT_NAME}/simple_graph_search.hpp 84 | include/${PROJECT_NAME}/simple_hausdorff_distance.hpp 85 | include/${PROJECT_NAME}/simple_hierarchical_clustering.hpp 86 | include/${PROJECT_NAME}/simple_kmeans_clustering.hpp 87 | include/${PROJECT_NAME}/simple_knearest_neighbors.hpp 88 | include/${PROJECT_NAME}/simple_prm_planner.hpp 89 | include/${PROJECT_NAME}/simple_prngs.hpp 90 | include/${PROJECT_NAME}/simple_robot_model_interface.hpp 91 | include/${PROJECT_NAME}/simple_rrt_planner.hpp 92 | include/${PROJECT_NAME}/simple_task_planner.hpp 93 | include/${PROJECT_NAME}/time_optimal_trajectory_parametrization.hpp 94 | include/${PROJECT_NAME}/utility.hpp 95 | include/${PROJECT_NAME}/voxel_grid.hpp 96 | include/${PROJECT_NAME}/zlib_helpers.hpp 97 | src/${PROJECT_NAME}/base64_helpers.cpp 98 | src/${PROJECT_NAME}/conversions.cpp 99 | src/${PROJECT_NAME}/math.cpp 100 | src/${PROJECT_NAME}/ros_conversions.cpp 101 | src/${PROJECT_NAME}/serialization.cpp 102 | src/${PROJECT_NAME}/time_optimal_trajectory_parametrization.cpp 103 | src/${PROJECT_NAME}/zlib_helpers.cpp) 104 | ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs visualization_msgs) 105 | target_link_libraries(${PROJECT_NAME} ${ZLIB_LIBRARIES}) 106 | 107 | # Examples 108 | add_executable(clustering_example example/clustering_example.cpp) 109 | target_link_libraries(clustering_example ${PROJECT_NAME}) 110 | 111 | add_executable(dtw_example example/dtw_example.cpp) 112 | target_link_libraries(dtw_example ${PROJECT_NAME}) 113 | 114 | if(BUILD_TESTING) 115 | # Tests 116 | find_package(ament_cmake_gtest REQUIRED) 117 | 118 | ament_add_gtest(hausdorff_distance_test test/hausdorff_distance_test.cpp) 119 | target_link_libraries(hausdorff_distance_test ${PROJECT_NAME}) 120 | 121 | ament_add_gtest(math_test test/math_test.cpp) 122 | target_link_libraries(math_test ${PROJECT_NAME}) 123 | 124 | ament_add_gtest(maybe_test test/maybe_test.cpp) 125 | target_link_libraries(maybe_test ${PROJECT_NAME}) 126 | 127 | ament_add_gtest(parallelism_test test/parallelism_test.cpp) 128 | target_link_libraries(parallelism_test ${PROJECT_NAME}) 129 | 130 | ament_add_gtest(planning_test test/planning_test.cpp) 131 | target_link_libraries(planning_test ${PROJECT_NAME}) 132 | 133 | ament_add_gtest(task_planning_test test/task_planning_test.cpp) 134 | target_link_libraries(task_planning_test ${PROJECT_NAME}) 135 | 136 | ament_add_gtest(ros_helpers_test test/ros_helpers_test.cpp) 137 | add_dependencies(ros_helpers_test ${PROJECT_NAME}) 138 | target_link_libraries(ros_helpers_test ${PROJECT_NAME}) 139 | 140 | ament_add_gtest(utility_test test/utility_test.cpp) 141 | add_dependencies(utility_test ${PROJECT_NAME}) 142 | target_link_libraries(utility_test ${PROJECT_NAME}) 143 | 144 | ament_add_gtest(voxel_grid_test test/voxel_grid_test.cpp) 145 | target_link_libraries(voxel_grid_test ${PROJECT_NAME}) 146 | 147 | ament_add_gtest(print_test test/print_test.cpp) 148 | target_link_libraries(print_test ${PROJECT_NAME}) 149 | endif() 150 | 151 | ############# 152 | ## Install ## 153 | ############# 154 | 155 | install(TARGETS ${PROJECT_NAME} 156 | ARCHIVE DESTINATION lib 157 | LIBRARY DESTINATION lib 158 | RUNTIME DESTINATION bin 159 | ) 160 | 161 | install( 162 | DIRECTORY include/ 163 | DESTINATION include 164 | ) 165 | 166 | ament_export_definitions(-DCOMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION=2) 167 | ament_export_include_directories(include) 168 | ament_export_libraries(${PROJECT_NAME}) 169 | 170 | ament_export_dependencies(rclcpp) 171 | ament_export_dependencies(geometry_msgs) 172 | ament_export_dependencies(visualization_msgs) 173 | 174 | ament_package(CONFIG_EXTRAS cmake/common_robotics_utilities-dependencies.cmake) 175 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, 4 | Portions Calder Phillips-Grafflin, Worcester Polytechnic Institute, 5 | The University of Michigan, and Toyota Research Institute. Some specific files 6 | ae licensed with other permissive licenses, see license headers in those files. 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | 12 | * Redistributions of source code must retain the above copyright notice, this 13 | list of conditions and the following disclaimer. 14 | 15 | * Redistributions in binary form must reproduce the above copyright notice, 16 | this list of conditions and the following disclaimer in the documentation 17 | and/or other materials provided with the distribution. 18 | 19 | * Neither the name of the copyright holder nor the names of its 20 | contributors may be used to endorse or promote products derived from 21 | this software without specific prior written permission. 22 | 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 27 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 32 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # common_robotics_utilities 2 | Common utility functions and algorithms for robotics work used by ARC & ARM labs and TRI. 3 | 4 | ## Setup 5 | 6 | `common_robotics_utilities` is a ROS package. 7 | 8 | Thus, it is best to build it within a ROS workspace: 9 | 10 | ```sh 11 | mkdir -p ~/ws/src 12 | cd ~/ws/src 13 | git clone https://github.com/calderpg/common_robotics_utilities.git 14 | ``` 15 | 16 | This package supports [ROS 1 Kinetic+](http://wiki.ros.org/ROS/Installation) 17 | and [ROS 2 Galactic+](https://index.ros.org/doc/ros2/Installation/) distributions. 18 | Prior distributions of ROS 2 can be supported by removing ROS 2 message printing 19 | support from `include/common_robotics_utilities/print.hpp`. 20 | Make sure to symlink the corresponding `CMakeLists.txt` and `package.xml` files 21 | for the ROS distribution of choice: 22 | 23 | *For ROS 1 Kinetic+* 24 | ```sh 25 | cd ~/ws/src/common_robotics_utilities 26 | ln -sT CMakeLists.txt.ros1 CMakeLists.txt 27 | ln -sT package.xml.ros1 package.xml 28 | ``` 29 | 30 | *For ROS 2 Galactic+* 31 | ```sh 32 | cd ~/ws/src/common_robotics_utilities 33 | ln -sT CMakeLists.txt.ros2 CMakeLists.txt 34 | ln -sT package.xml.ros2 package.xml 35 | ``` 36 | 37 | Finally, use [`rosdep`](https://docs.ros.org/independent/api/rosdep/html/) 38 | to ensure all dependencies in the `package.xml` are satisfied: 39 | 40 | ```sh 41 | cd ~/ws 42 | rosdep install -i -y --from-path src 43 | ``` 44 | 45 | ## Building 46 | 47 | Use [`catkin_make`](http://wiki.ros.org/catkin/commands/catkin_make) or 48 | [`colcon`](https://colcon.readthedocs.io/en/released/) accordingly. 49 | 50 | *For ROS 1 Kinetic+* 51 | ```sh 52 | cd ~/ws 53 | catkin_make # the entire workspace 54 | catkin_make --pkg common_robotics_utilities # the package only 55 | ``` 56 | 57 | *For ROS 2 Galactic+* 58 | ```sh 59 | cd ~/ws 60 | colcon build # the entire workspace 61 | colcon build --packages-select common_robotics_utilities # the package only 62 | ``` 63 | 64 | ## Testing 65 | 66 | Use [`catkin_make`](http://wiki.ros.org/catkin/commands/catkin_make) or 67 | [`colcon`](https://colcon.readthedocs.io/en/released/) accordingly. 68 | 69 | *For ROS 1 Kinetic+* 70 | ```sh 71 | cd ~/ws 72 | catkin_make run_tests # the entire workspace 73 | catkin_make run_tests_common_robotics_utilities # the package only 74 | ``` 75 | 76 | *For ROS 2 Galactic+* 77 | ```sh 78 | cd ~/ws 79 | colcon test --event-handlers=console_direct+ # the entire workspace 80 | colcon test --event-handlers=console_direct+ --packages-select common_robotics_utilities # the package only 81 | ``` 82 | -------------------------------------------------------------------------------- /cmake/common_robotics_utilities-dependencies.cmake: -------------------------------------------------------------------------------- 1 | find_package(Eigen3 REQUIRED) 2 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 3 | include_directories(SYSTEM ${Eigen3_INCLUDE_DIRS}) 4 | -------------------------------------------------------------------------------- /cmake/common_robotics_utilities-extras.cmake: -------------------------------------------------------------------------------- 1 | # force automatic escaping of preprocessor definitions 2 | cmake_policy(PUSH) 3 | cmake_policy(SET CMP0005 NEW) 4 | 5 | add_definitions(-DCOMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION=1) 6 | 7 | cmake_policy(POP) 8 | -------------------------------------------------------------------------------- /example/clustering_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using common_robotics_utilities::parallelism::DegreeOfParallelism; 14 | using PointVector = common_robotics_utilities::math::VectorVector4d; 15 | using IndexClusteringResult 16 | = common_robotics_utilities::simple_hierarchical_clustering 17 | ::IndexClusteringResult; 18 | using PointClusteringResult 19 | = common_robotics_utilities::simple_hierarchical_clustering 20 | ::ClusteringResult; 21 | 22 | namespace 23 | { 24 | void SaveClustering( 25 | const PointClusteringResult& clusters, 26 | const std::string& filename) 27 | { 28 | // Use the cluster index to set z-coordinates for easy visualization 29 | std::ofstream clustering_output_file(filename, std::ios_base::out); 30 | if (!clustering_output_file.is_open()) 31 | { 32 | throw std::invalid_argument( 33 | "Log file " + filename + " must be write-openable"); 34 | } 35 | std::cout << "Saving clustering to " << filename << std::endl; 36 | for (size_t idx = 0; idx < clusters.Clusters().size(); idx++) 37 | { 38 | const PointVector& cluster = clusters.Clusters().at(idx); 39 | const double cluster_num = 1.0 + static_cast(idx); 40 | for (const Eigen::Vector4d& point : cluster) 41 | { 42 | clustering_output_file << point(0) << "," << point(1) << "," 43 | << cluster_num << std::endl; 44 | } 45 | } 46 | clustering_output_file.close(); 47 | } 48 | 49 | void SaveIndexClustering( 50 | const PointVector& raw_points, 51 | const IndexClusteringResult& index_clusters, 52 | const std::string& filename) 53 | { 54 | SaveClustering( 55 | common_robotics_utilities::simple_hierarchical_clustering 56 | ::MakeElementClusteringFromIndexClustering< 57 | Eigen::Vector4d, PointVector>(raw_points, index_clusters), 58 | filename); 59 | } 60 | } // namespace 61 | 62 | int main(int argc, char** argv) 63 | { 64 | const size_t num_points 65 | = (argc >= 2) ? static_cast(atoi(argv[1])) : 1000; 66 | const double cluster_threshold 67 | = (argc >= 3) ? atof(argv[2]) : 0.1; 68 | if (cluster_threshold < 0.0) 69 | { 70 | throw std::invalid_argument("cluster_threshold < 0.0"); 71 | } 72 | std::cout << "Generating " << num_points << " points..." << std::endl; 73 | const int64_t seed = 42; 74 | std::mt19937_64 prng(seed); 75 | std::uniform_real_distribution dist(0.0, 10.0); 76 | PointVector random_points(num_points); 77 | for (size_t idx = 0; idx < num_points; idx++) 78 | { 79 | const double x = dist(prng); 80 | const double y = dist(prng); 81 | random_points.at(idx) = Eigen::Vector4d(x, y, 0.0, 0.0); 82 | } 83 | std::function 84 | distance_fn = [] (const Eigen::Vector4d& v1, const Eigen::Vector4d& v2) 85 | { 86 | return (v1 - v2).norm(); 87 | }; 88 | const std::vector use_parallel_options = {false, true}; 89 | for (const bool use_parallel : use_parallel_options) 90 | { 91 | const DegreeOfParallelism parallelism = (use_parallel) 92 | ? DegreeOfParallelism::FromOmp() 93 | : DegreeOfParallelism::None(); 94 | // Cluster using index-cluster methods 95 | // Single-link clustering 96 | std::cout << "Single-link hierarchical index clustering " << num_points 97 | << " points..." << std::endl; 98 | const auto slhic_start = std::chrono::steady_clock::now(); 99 | const auto single_link_index_clusters 100 | = common_robotics_utilities::simple_hierarchical_clustering 101 | ::IndexCluster( 102 | random_points, distance_fn, 1.0, 103 | common_robotics_utilities::simple_hierarchical_clustering 104 | ::ClusterStrategy::SINGLE_LINK, parallelism); 105 | const auto slhic_end = std::chrono::steady_clock::now(); 106 | const double slhic_elapsed = 107 | std::chrono::duration(slhic_end - slhic_start).count(); 108 | std::cout << "...took " << slhic_elapsed << " seconds" << std::endl; 109 | // Save 110 | SaveIndexClustering( 111 | random_points, single_link_index_clusters, (use_parallel) 112 | ? "/tmp/test_parallel_single_link_index_clustering.csv" 113 | : "/tmp/test_serial_single_link_index_clustering.csv"); 114 | // Complete-link clustering 115 | std::cout << "Complete-link hierarchical index clustering " << num_points 116 | << " points..." << std::endl; 117 | const auto clhic_start = std::chrono::steady_clock::now(); 118 | const auto complete_link_index_clusters 119 | = common_robotics_utilities::simple_hierarchical_clustering 120 | ::IndexCluster( 121 | random_points, distance_fn, 1.0, 122 | common_robotics_utilities::simple_hierarchical_clustering 123 | ::ClusterStrategy::COMPLETE_LINK, parallelism); 124 | const auto clhic_end = std::chrono::steady_clock::now(); 125 | const double clhic_elapsed = 126 | std::chrono::duration(clhic_end - clhic_start).count(); 127 | std::cout << "...took " << clhic_elapsed << " seconds" << std::endl; 128 | // Save 129 | SaveIndexClustering( 130 | random_points, complete_link_index_clusters, (use_parallel) 131 | ? "/tmp/test_parallel_complete_link_index_clustering.csv" 132 | : "/tmp/test_serial_complete_link_index_clustering.csv"); 133 | // Cluster using value-cluster methods 134 | // Single-link clustering 135 | std::cout << "Single-link hierarchical clustering " << num_points 136 | << " points..." << std::endl; 137 | const auto slhc_start = std::chrono::steady_clock::now(); 138 | const auto single_link_clusters 139 | = common_robotics_utilities::simple_hierarchical_clustering::Cluster( 140 | random_points, distance_fn, 1.0, 141 | common_robotics_utilities::simple_hierarchical_clustering 142 | ::ClusterStrategy::SINGLE_LINK, parallelism); 143 | const auto slhc_end = std::chrono::steady_clock::now(); 144 | const double slhc_elapsed = 145 | std::chrono::duration(slhc_end - slhc_start).count(); 146 | std::cout << "...took " << slhc_elapsed << " seconds" << std::endl; 147 | // Save 148 | SaveClustering(single_link_clusters, (use_parallel) 149 | ? "/tmp/test_parallel_single_link_clustering.csv" 150 | : "/tmp/test_serial_single_link_clustering.csv"); 151 | // Complete-link clustering 152 | std::cout << "Complete-link hierarchical clustering " << num_points 153 | << " points..." << std::endl; 154 | const auto clhc_start = std::chrono::steady_clock::now(); 155 | const auto complete_link_clusters 156 | = common_robotics_utilities::simple_hierarchical_clustering::Cluster( 157 | random_points, distance_fn, 1.0, 158 | common_robotics_utilities::simple_hierarchical_clustering 159 | ::ClusterStrategy::COMPLETE_LINK, parallelism); 160 | const auto clhc_end = std::chrono::steady_clock::now(); 161 | const double clhc_elapsed = 162 | std::chrono::duration(clhc_end - clhc_start).count(); 163 | std::cout << "...took " << clhc_elapsed << " seconds" << std::endl; 164 | // Save 165 | SaveClustering(complete_link_clusters, (use_parallel) 166 | ? "/tmp/test_parallel_complete_link_clustering.csv" 167 | : "/tmp/test_serial_complete_link_clustering.csv"); 168 | // Cluster using K-means 169 | const int32_t num_clusters = 50; 170 | std::cout << "K-means clustering " << num_points << " points into " 171 | << num_clusters << " clusters..." << std::endl; 172 | std::function 173 | average_fn = [] (const PointVector& cluster) 174 | { 175 | return common_robotics_utilities::math::AverageEigenVector4d(cluster); 176 | }; 177 | const auto logging_fn = [] (const std::string& message) 178 | { 179 | std::cout << message << std::endl; 180 | }; 181 | const auto kmeans_start = std::chrono::steady_clock::now(); 182 | const std::vector kmeans_labels 183 | = common_robotics_utilities::simple_kmeans_clustering::Cluster( 184 | random_points, distance_fn, average_fn, num_clusters, 42, true, 185 | parallelism, logging_fn); 186 | const auto kmeans_end = std::chrono::steady_clock::now(); 187 | const double kmeans_elapsed = 188 | std::chrono::duration(kmeans_end - kmeans_start).count(); 189 | std::cout << "...took " << kmeans_elapsed << " seconds" << std::endl; 190 | // Get value clusters from the K-means labels 191 | std::vector kmeans_clusters(num_clusters); 192 | for (size_t idx = 0; idx < kmeans_labels.size(); idx++) 193 | { 194 | const size_t cluster_num = static_cast(kmeans_labels.at(idx)); 195 | kmeans_clusters.at(cluster_num).push_back(random_points.at(idx)); 196 | } 197 | // Save 198 | SaveClustering(complete_link_clusters, (use_parallel) 199 | ? "/tmp/test_parallel_kmeans_clustering.csv" 200 | : "/tmp/test_serial_kmeans_clustering.csv"); 201 | } 202 | std::cout << "Done saving, you can plot as a 3d scatterplot to see clustering" 203 | << std::endl; 204 | return 0; 205 | } 206 | -------------------------------------------------------------------------------- /example/dtw_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | // We're using Vector4d to make sure alignment is handled properly for builds 13 | // using -march=native and fixed-size-vectorizable Eigen types. 14 | using PointVector = common_robotics_utilities::math::VectorVector4d; 15 | using PointVectorDTW = common_robotics_utilities::simple_dtw::SimpleDTW< 16 | Eigen::Vector4d, Eigen::Vector4d, PointVector, PointVector>; 17 | 18 | namespace 19 | { 20 | PointVector SampleRandomPointVector( 21 | const size_t num_points, std::mt19937_64& prng, 22 | std::uniform_real_distribution& distribution) 23 | { 24 | std::cout << "Generating " << num_points << " random points" << std::endl; 25 | PointVector random_points(num_points); 26 | for (size_t idx = 0; idx < num_points; idx++) 27 | { 28 | const double x = distribution(prng); 29 | const double y = distribution(prng); 30 | const double z = distribution(prng); 31 | random_points.at(idx) = Eigen::Vector4d(x, y, z, 1.0); 32 | } 33 | return random_points; 34 | } 35 | } // namespace 36 | 37 | int main(int, char**) 38 | { 39 | // Set up PRNG and sampling distribution. 40 | const int64_t seed = 42; 41 | std::mt19937_64 prng(seed); 42 | std::uniform_real_distribution dist(0.0, 10.0); 43 | 44 | // Set up test vectors. 45 | const PointVector test_vector1 = SampleRandomPointVector(12500, prng, dist); 46 | const PointVector test_vector2 = SampleRandomPointVector(15000, prng, dist); 47 | const PointVector test_vector3 = SampleRandomPointVector(17500, prng, dist); 48 | const PointVector test_vector4 = SampleRandomPointVector(20000, prng, dist); 49 | 50 | // Set up the distance function. 51 | const std::function 52 | distance_fn = [] (const Eigen::Vector4d& v1, const Eigen::Vector4d& v2) 53 | { 54 | return (v1 - v2).norm(); 55 | }; 56 | 57 | // Run a number of DTW queries without reusing the DTW matrix memory. 58 | const auto uncached_start_time = std::chrono::steady_clock::now(); 59 | 60 | { 61 | const auto point_vector_dtw_distance_fn 62 | = [&] (const PointVector& vec1, const PointVector& vec2) 63 | { 64 | return common_robotics_utilities::simple_dtw::EvaluateWarpingCost< 65 | Eigen::Vector4d, Eigen::Vector4d, PointVector, PointVector>( 66 | vec1, vec2, distance_fn); 67 | }; 68 | 69 | const auto vector1_self_distance = 70 | point_vector_dtw_distance_fn(test_vector1, test_vector1); 71 | std::cout << "vector1 self-distance = " << vector1_self_distance 72 | << std::endl; 73 | 74 | const auto vector2_self_distance = 75 | point_vector_dtw_distance_fn(test_vector2, test_vector2); 76 | std::cout << "vector2 self-distance = " << vector2_self_distance 77 | << std::endl; 78 | 79 | const auto vector3_self_distance = 80 | point_vector_dtw_distance_fn(test_vector3, test_vector3); 81 | std::cout << "vector3 self-distance = " << vector3_self_distance 82 | << std::endl; 83 | 84 | const auto vector4_self_distance = 85 | point_vector_dtw_distance_fn(test_vector4, test_vector4); 86 | std::cout << "vector4 self-distance = " << vector4_self_distance 87 | << std::endl; 88 | 89 | const auto vector1_vector2_distance = 90 | point_vector_dtw_distance_fn(test_vector1, test_vector2); 91 | std::cout << "vector1-vector2 distance = " << vector1_vector2_distance 92 | << std::endl; 93 | 94 | const auto vector2_vector3_distance = 95 | point_vector_dtw_distance_fn(test_vector2, test_vector3); 96 | std::cout << "vector2-vector3 distance = " << vector2_vector3_distance 97 | << std::endl; 98 | 99 | const auto vector3_vector4_distance = 100 | point_vector_dtw_distance_fn(test_vector3, test_vector4); 101 | std::cout << "vector3-vector4 distance = " << vector3_vector4_distance 102 | << std::endl; 103 | 104 | const auto vector4_vector1_distance = 105 | point_vector_dtw_distance_fn(test_vector4, test_vector1); 106 | std::cout << "vector4-vector1 distance = " << vector4_vector1_distance 107 | << std::endl; 108 | } 109 | 110 | const auto uncached_end_time = std::chrono::steady_clock::now(); 111 | const double uncached_elapsed = std::chrono::duration( 112 | uncached_end_time - uncached_start_time).count(); 113 | 114 | std::cout << "DTW queries runtime: " << uncached_elapsed 115 | << " seconds (without memory reuse)" << std::endl; 116 | 117 | // Run the same set of DTW queries reusing the DTW matrix memory. 118 | const auto cached_start_time = std::chrono::steady_clock::now(); 119 | 120 | { 121 | // Create a DTW evaluator with the known maximum sequence size. 122 | PointVectorDTW dtw_evaluator(2000, 2000); 123 | 124 | const auto vector1_self_distance = dtw_evaluator.EvaluateWarpingCost( 125 | test_vector1, test_vector1, distance_fn); 126 | std::cout << "vector1 self-distance = " << vector1_self_distance 127 | << std::endl; 128 | 129 | const auto vector2_self_distance = dtw_evaluator.EvaluateWarpingCost( 130 | test_vector2, test_vector2, distance_fn); 131 | std::cout << "vector2 self-distance = " << vector2_self_distance 132 | << std::endl; 133 | 134 | const auto vector3_self_distance = dtw_evaluator.EvaluateWarpingCost( 135 | test_vector3, test_vector3, distance_fn); 136 | std::cout << "vector3 self-distance = " << vector3_self_distance 137 | << std::endl; 138 | 139 | const auto vector4_self_distance = dtw_evaluator.EvaluateWarpingCost( 140 | test_vector4, test_vector4, distance_fn); 141 | std::cout << "vector4 self-distance = " << vector4_self_distance 142 | << std::endl; 143 | 144 | const auto vector1_vector2_distance = dtw_evaluator.EvaluateWarpingCost( 145 | test_vector1, test_vector2, distance_fn); 146 | std::cout << "vector1-vector2 distance = " << vector1_vector2_distance 147 | << std::endl; 148 | 149 | const auto vector2_vector3_distance = dtw_evaluator.EvaluateWarpingCost( 150 | test_vector2, test_vector3, distance_fn); 151 | std::cout << "vector2-vector3 distance = " << vector2_vector3_distance 152 | << std::endl; 153 | 154 | const auto vector3_vector4_distance = dtw_evaluator.EvaluateWarpingCost( 155 | test_vector3, test_vector4, distance_fn); 156 | std::cout << "vector3-vector4 distance = " << vector3_vector4_distance 157 | << std::endl; 158 | 159 | const auto vector4_vector1_distance = dtw_evaluator.EvaluateWarpingCost( 160 | test_vector4, test_vector1, distance_fn); 161 | std::cout << "vector4-vector1 distance = " << vector4_vector1_distance 162 | << std::endl; 163 | } 164 | 165 | const auto cached_end_time = std::chrono::steady_clock::now(); 166 | const double cached_elapsed = std::chrono::duration( 167 | cached_end_time - cached_start_time).count(); 168 | 169 | std::cout << "DTW queries runtime: " << cached_elapsed 170 | << " seconds (with memory reuse)" << std::endl; 171 | 172 | return 0; 173 | } 174 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/base64_helpers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace common_robotics_utilities 10 | { 11 | CRU_NAMESPACE_BEGIN 12 | namespace base64_helpers 13 | { 14 | std::vector Decode(const std::string& encoded); 15 | 16 | std::string Encode(const std::vector& binary); 17 | } // namespace base64_helpers 18 | CRU_NAMESPACE_END 19 | } // namespace common_robotics_utilities 20 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/color_builder.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace common_robotics_utilities 10 | { 11 | CRU_NAMESPACE_BEGIN 12 | namespace color_builder 13 | { 14 | /// Convert @param hexval color channel to float. 15 | inline constexpr float ColorChannelFromHex(uint8_t hexval) 16 | { 17 | return static_cast(hexval) / 255.0f; 18 | } 19 | 20 | /// Clamp @param val color channel to [0, 1]. 21 | inline float TrimColorValue(const float val) 22 | { 23 | return utility::ClampValue(val, 0.0f, 1.0f); 24 | } 25 | 26 | /// Convert @param colorval to hex value. 27 | inline uint8_t ColorChannelToHex(float colorval) 28 | { 29 | return static_cast(std::round(TrimColorValue(colorval) * 255.0f)); 30 | } 31 | 32 | /// Simple RGBA color storage type, meant to be API-compatible with other color 33 | /// types like ROS's std_msgs::ColorRGBA. 34 | class RGBAColor 35 | { 36 | public: 37 | 38 | float r = 0.0f; 39 | float g = 0.0f; 40 | float b = 0.0f; 41 | float a = 0.0f; 42 | 43 | RGBAColor(const float in_r, const float in_g, const float in_b, 44 | const float in_a) 45 | : r(TrimColorValue(in_r)), g(TrimColorValue(in_g)), 46 | b(TrimColorValue(in_b)), a(TrimColorValue(in_a)) {} 47 | 48 | RGBAColor(const float in_r, const float in_g, const float in_b) 49 | : r(TrimColorValue(in_r)), g(TrimColorValue(in_g)), 50 | b(TrimColorValue(in_b)), a(1.0f) {} 51 | 52 | RGBAColor(const uint8_t in_r, const uint8_t in_g, const uint8_t in_b, 53 | const uint8_t in_a) 54 | : r(ColorChannelFromHex(in_r)), g(ColorChannelFromHex(in_g)), 55 | b(ColorChannelFromHex(in_b)), a(ColorChannelFromHex(in_a)) {} 56 | 57 | RGBAColor(const uint8_t in_r, const uint8_t in_g, const uint8_t in_b, 58 | const float in_a) 59 | : r(ColorChannelFromHex(in_r)), g(ColorChannelFromHex(in_g)), 60 | b(ColorChannelFromHex(in_b)), a(TrimColorValue(in_a)) {} 61 | 62 | RGBAColor(const uint8_t in_r, const uint8_t in_g, const uint8_t in_b) 63 | : r(ColorChannelFromHex(in_r)), g(ColorChannelFromHex(in_g)), 64 | b(ColorChannelFromHex(in_b)), a(1.0f) {} 65 | 66 | RGBAColor() : r(0.0f), g(0.0f), b(0.0f), a(0.0f) {} 67 | 68 | float R() const { return r; } 69 | 70 | float G() const { return g; } 71 | 72 | float B() const { return b; } 73 | 74 | float A() const { return a; } 75 | 76 | void SetR(const float new_r) { r = TrimColorValue(new_r); } 77 | 78 | void SetG(const float new_g) { g = TrimColorValue(new_g); } 79 | 80 | void SetB(const float new_b) { b = TrimColorValue(new_b); } 81 | 82 | void SetA(const float new_a) { a = TrimColorValue(new_a); } 83 | 84 | uint8_t GetRHex() const { return ColorChannelToHex(r); } 85 | 86 | uint8_t GetGHex() const { return ColorChannelToHex(g); } 87 | 88 | uint8_t GetBHex() const { return ColorChannelToHex(b); } 89 | 90 | uint8_t GetAHex() const { return ColorChannelToHex(a); } 91 | 92 | void SetRHex(const uint8_t hex_r) { r = ColorChannelFromHex(hex_r); } 93 | 94 | void SetGHex(const uint8_t hex_g) { g = ColorChannelFromHex(hex_g); } 95 | 96 | void SetBHex(const uint8_t hex_b) { b = ColorChannelFromHex(hex_b); } 97 | 98 | void SetAHex(const uint8_t hex_a) { a = ColorChannelFromHex(hex_a); } 99 | }; 100 | 101 | /// Make a color value of ColorType from the provided float color chanel values. 102 | template 103 | inline ColorType MakeFromFloatColors( 104 | const float r, const float g, const float b, const float a=1.0f) 105 | { 106 | ColorType color; 107 | color.r = TrimColorValue(r); 108 | color.g = TrimColorValue(g); 109 | color.b = TrimColorValue(b); 110 | color.a = TrimColorValue(a); 111 | return color; 112 | } 113 | 114 | /// Make a color value of ColorType from the provided hex color chanel values. 115 | template 116 | inline ColorType MakeFromHexColors( 117 | const uint8_t r, const uint8_t g, const uint8_t b, const uint8_t a=0xff) 118 | { 119 | return MakeFromFloatColors( 120 | ColorChannelFromHex(r), ColorChannelFromHex(g), ColorChannelFromHex(b), 121 | ColorChannelFromHex(a)); 122 | } 123 | 124 | /// Make a color value of ColorType from the provided color chanel values. This 125 | /// covers the common case of colors specified in RGB hex but with a variable 126 | /// alpha computed separately. 127 | template 128 | inline ColorType MakeFromMixedColors( 129 | const uint8_t r, const uint8_t g, const uint8_t b, const float a=1.0f) 130 | { 131 | return MakeFromFloatColors( 132 | ColorChannelFromHex(r), ColorChannelFromHex(g), ColorChannelFromHex(b), 133 | TrimColorValue(a)); 134 | } 135 | 136 | /// Interpolates a color in the "hot-to-cold" pattern for @param value given 137 | /// lower value @param min_value and upper value @param max_value. 138 | template 139 | inline ColorType InterpolateHotToCold( 140 | const double value, const double min_value=0.0, const double max_value=1.0) 141 | { 142 | const double real_value = utility::ClampValue(value, min_value, max_value); 143 | const double range = max_value - min_value; 144 | // Start with white 145 | double r = 1.0; 146 | double g = 1.0; 147 | double b = 1.0; 148 | // Interpolate 149 | if (real_value < (min_value + (0.25 * range))) 150 | { 151 | r = 0.0; 152 | g = 4.0 * (real_value - min_value) / range; 153 | } 154 | else if (real_value < (min_value + (0.5 * range))) 155 | { 156 | r = 0.0; 157 | b = 1.0 + 4.0 * (min_value + 0.25 * range - real_value) / range; 158 | } 159 | else if (real_value < (min_value + (0.75 * range))) 160 | { 161 | r = 4.0 * (real_value - min_value - 0.5 * range) / range; 162 | b = 0.0; 163 | } 164 | else 165 | { 166 | g = 1.0 + 4.0 * (min_value + 0.75 * range - real_value) / range; 167 | b = 0.0; 168 | } 169 | return MakeFromFloatColors( 170 | static_cast(r), static_cast(g), static_cast(b), 171 | 1.0f); 172 | } 173 | 174 | /// Converts between two color types wth float (or float-like) R,G,B,A members. 175 | template 176 | inline ColorTypeB ConvertColor(const ColorTypeA& color) 177 | { 178 | ColorTypeB cvt_color; 179 | cvt_color.r = TrimColorValue(color.r); 180 | cvt_color.g = TrimColorValue(color.g); 181 | cvt_color.b = TrimColorValue(color.b); 182 | cvt_color.a = TrimColorValue(color.a); 183 | return cvt_color; 184 | } 185 | 186 | /// Checks if two color types wth float (or float-like) R,G,B,A members are 187 | /// equal to the provided tolerance. 188 | template 189 | inline bool ColorsEqual(const ColorTypeA& color1, const ColorTypeB& color2, 190 | const float tolerance=0.0f) 191 | { 192 | if (std::abs(color1.r - color2.r) > tolerance) 193 | { 194 | return false; 195 | } 196 | if (std::abs(color1.g - color2.g) > tolerance) 197 | { 198 | return false; 199 | } 200 | if (std::abs(color1.b - color2.b) > tolerance) 201 | { 202 | return false; 203 | } 204 | if (std::abs(color1.a - color2.a) > tolerance) 205 | { 206 | return false; 207 | } 208 | return true; 209 | } 210 | 211 | /// Finds the "unique color" for the provided color code @param color_code, for 212 | /// all color codes <= 20, the color is derived from a list of 213 | /// percepturally-distinct colors. Beyond 20, black is returned. @param alpha 214 | /// specifies the alpha channel of the color. 215 | template 216 | inline ColorType LookupUniqueColor(const uint32_t color_code, 217 | const float alpha=1.0f) 218 | { 219 | if (color_code == 0) 220 | { 221 | return MakeFromFloatColors(1.0f, 1.0f, 1.0f, 0.0f); 222 | } 223 | if (color_code == 1) 224 | { 225 | return MakeFromMixedColors(0xff, 0x00, 0xb3, alpha); 226 | } 227 | else if (color_code == 2) 228 | { 229 | return MakeFromMixedColors(0x80, 0x75, 0x3e, alpha); 230 | } 231 | else if (color_code == 3) 232 | { 233 | return MakeFromMixedColors(0xff, 0x00, 0x68, alpha); 234 | } 235 | else if (color_code == 4) 236 | { 237 | return MakeFromMixedColors(0xa6, 0xd7, 0xbd, alpha); 238 | } 239 | else if (color_code == 5) 240 | { 241 | return MakeFromMixedColors(0xc1, 0x20, 0x00, alpha); 242 | } 243 | else if (color_code == 6) 244 | { 245 | return MakeFromMixedColors(0xce, 0x62, 0xa2, alpha); 246 | } 247 | else if (color_code == 7) 248 | { 249 | return MakeFromMixedColors(0x81, 0x66, 0x70, alpha); 250 | } 251 | else if (color_code == 8) 252 | { 253 | return MakeFromMixedColors(0x00, 0x34, 0x7d, alpha); 254 | } 255 | else if (color_code == 9) 256 | { 257 | return MakeFromMixedColors(0xf6, 0x8e, 0x76, alpha); 258 | } 259 | else if (color_code == 10) 260 | { 261 | return MakeFromMixedColors(0x00, 0x8a, 0x53, alpha); 262 | } 263 | else if (color_code == 11) 264 | { 265 | return MakeFromMixedColors(0xff, 0x5c, 0x7a, alpha); 266 | } 267 | else if (color_code == 12) 268 | { 269 | return MakeFromMixedColors(0x53, 0x7a, 0x37, alpha); 270 | } 271 | else if (color_code == 13) 272 | { 273 | return MakeFromMixedColors(0xff, 0x00, 0x8e, alpha); 274 | } 275 | else if (color_code == 14) 276 | { 277 | return MakeFromMixedColors(0xb3, 0x51, 0x28, alpha); 278 | } 279 | else if (color_code == 15) 280 | { 281 | return MakeFromMixedColors(0xf4, 0x00, 0xc8, alpha); 282 | } 283 | else if (color_code == 16) 284 | { 285 | return MakeFromMixedColors(0x7f, 0x0d, 0x18, alpha); 286 | } 287 | else if (color_code == 17) 288 | { 289 | return MakeFromMixedColors(0x93, 0x00, 0xaa, alpha); 290 | } 291 | else if (color_code == 18) 292 | { 293 | return MakeFromMixedColors(0x59, 0x15, 0x33, alpha); 294 | } 295 | else if (color_code == 19) 296 | { 297 | return MakeFromMixedColors(0xf1, 0x13, 0x3a, alpha); 298 | } 299 | else if (color_code == 20) 300 | { 301 | return MakeFromMixedColors(0x23, 0x16, 0x2c, alpha); 302 | } 303 | else 304 | { 305 | return MakeFromFloatColors(0.0f, 0.0f, 0.0f, alpha); 306 | } 307 | } 308 | } // namespace color_builder 309 | CRU_NAMESPACE_END 310 | } // namespace common_robotics_utilities 311 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/conversions.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace common_robotics_utilities 9 | { 10 | CRU_NAMESPACE_BEGIN 11 | namespace conversions 12 | { 13 | Eigen::Quaterniond QuaternionFromRPY(const double R, 14 | const double P, 15 | const double Y); 16 | 17 | // URDF RPY IS ACTUALLY APPLIED Y*P*R 18 | Eigen::Quaterniond QuaternionFromUrdfRPY(const double R, 19 | const double P, 20 | const double Y); 21 | 22 | // Returns XYZ Euler angles 23 | Eigen::Vector3d EulerAnglesFromRotationMatrix( 24 | const Eigen::Matrix3d& rot_matrix); 25 | 26 | // Returns XYZ Euler angles 27 | Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat); 28 | 29 | // Returns XYZ Euler angles 30 | Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans); 31 | 32 | Eigen::Isometry3d TransformFromXYZRPY(const double x, 33 | const double y, 34 | const double z, 35 | const double roll, 36 | const double pitch, 37 | const double yaw); 38 | 39 | Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation, 40 | const Eigen::Vector3d& rotation); 41 | 42 | Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components); 43 | 44 | // URDF RPY IS ACTUALLY APPLIED Y*P*R 45 | Eigen::Isometry3d TransformFromUrdfXYZRPY(const double x, 46 | const double y, 47 | const double z, 48 | const double roll, 49 | const double pitch, 50 | const double yaw); 51 | 52 | // URDF RPY IS ACTUALLY APPLIED Y*P*R 53 | Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::Vector3d& translation, 54 | const Eigen::Vector3d& rotation); 55 | 56 | // URDF RPY IS ACTUALLY APPLIED Y*P*R 57 | Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::VectorXd& components); 58 | 59 | Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform); 60 | 61 | Eigen::Vector3d StdVectorDoubleToEigenVector3d( 62 | const std::vector& vector); 63 | 64 | Eigen::VectorXd StdVectorDoubleToEigenVectorXd( 65 | const std::vector& vector); 66 | 67 | std::vector EigenVector3dToStdVectorDouble( 68 | const Eigen::Vector3d& point); 69 | 70 | std::vector EigenVectorXdToStdVectorDouble( 71 | const Eigen::VectorXd& eigen_vector); 72 | 73 | // Takes as is the ROS custom! 74 | Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond( 75 | const std::vector& vector); 76 | 77 | // Returns as is the ROS custom! 78 | std::vector EigenQuaterniondToStdVectorDouble( 79 | const Eigen::Quaterniond& quat); 80 | } // namespace conversions 81 | CRU_NAMESPACE_END 82 | } // namespace common_robotics_utilities 83 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/cru_namespace.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /** Downstream projects can adjust these macros to tweak the project namespace. 4 | 5 | When set, they should refer to a C++ inline namespace: 6 | https://en.cppreference.com/w/cpp/language/namespace#Inline_namespaces 7 | 8 | The inline namespace provides symbol versioning to allow multiple copies of 9 | common_robotics_utilities to be linked into the same image. In many cases, 10 | the namespace will also be marked hidden so that linker symbols are private. 11 | 12 | Example: 13 | #define CRU_NAMESPACE_BEGIN \ 14 | inline namespace v1 __attribute__ ((visibility ("hidden"))) { 15 | #define CRU_NAMESPACE_END } 16 | */ 17 | 18 | #ifndef CRU_NAMESPACE_BEGIN 19 | # define CRU_NAMESPACE_BEGIN inline namespace v1 { 20 | #endif 21 | 22 | #ifndef CRU_NAMESPACE_END 23 | # define CRU_NAMESPACE_END } 24 | #endif 25 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/maybe.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace common_robotics_utilities 10 | { 11 | CRU_NAMESPACE_BEGIN 12 | namespace internal 13 | { 14 | /// Helper type for OwningMaybe that stores one of two values without 15 | /// requiring default constructibility for either value. 16 | template 17 | struct TaggedUnion 18 | { 19 | private: 20 | void ConstructCopy(const TaggedUnion& other) 21 | { 22 | switch (other.type_) 23 | { 24 | case TYPE_A: 25 | { 26 | ::new(&value_a_) A(other.value_a_); 27 | type_ = TYPE_A; 28 | break; 29 | } 30 | case TYPE_B: 31 | { 32 | ::new(&value_b_) B(other.value_b_); 33 | type_ = TYPE_B; 34 | break; 35 | } 36 | } 37 | } 38 | 39 | void ConstructMove(TaggedUnion&& other) 40 | { 41 | switch (other.type_) 42 | { 43 | case TYPE_A: 44 | { 45 | ::new(&value_a_) A(std::move(other.value_a_)); 46 | type_ = TYPE_A; 47 | break; 48 | } 49 | case TYPE_B: 50 | { 51 | ::new(&value_b_) B(std::move(other.value_b_)); 52 | type_ = TYPE_B; 53 | break; 54 | } 55 | } 56 | } 57 | 58 | void Cleanup() 59 | { 60 | switch (type_) 61 | { 62 | case TYPE_A: 63 | { 64 | value_a_.~A(); 65 | break; 66 | } 67 | case TYPE_B: 68 | { 69 | value_b_.~B(); 70 | break; 71 | } 72 | } 73 | } 74 | 75 | public: 76 | union 77 | { 78 | A value_a_; 79 | B value_b_; 80 | }; 81 | 82 | enum {TYPE_A, TYPE_B} type_{}; 83 | 84 | explicit TaggedUnion(const A& value_a) 85 | : value_a_(value_a), type_(TYPE_A) {} 86 | 87 | explicit TaggedUnion(A&& value_a) 88 | : value_a_(std::move(value_a)), type_(TYPE_A) {} 89 | 90 | explicit TaggedUnion(const B& value_b) 91 | : value_b_(value_b), type_(TYPE_B) {} 92 | 93 | explicit TaggedUnion(B&& value_b) 94 | : value_b_(std::move(value_b)), type_(TYPE_B) {} 95 | 96 | TaggedUnion(const TaggedUnion& other) { ConstructCopy(other); } 97 | 98 | TaggedUnion(TaggedUnion&& other) { ConstructMove(std::move(other)); } 99 | 100 | ~TaggedUnion() { Cleanup(); } 101 | 102 | TaggedUnion& operator=(const TaggedUnion& other) 103 | { 104 | if (this != std::addressof(other)) 105 | { 106 | Cleanup(); 107 | 108 | ConstructCopy(other); 109 | } 110 | return *this; 111 | } 112 | 113 | TaggedUnion& operator=(TaggedUnion&& other) 114 | { 115 | if (this != std::addressof(other)) 116 | { 117 | Cleanup(); 118 | 119 | ConstructMove(std::move(other)); 120 | } 121 | return *this; 122 | } 123 | }; 124 | } // namespace internal 125 | 126 | /// An implementation of the Maybe/Optional pattern, in which an OwningMaybe 127 | /// wraps an instance of T or no value. This is equivalent to std::optional 128 | /// in C++17, in which OwningMaybe owns the contained instance of T. If the 129 | /// instance of T is expensive or difficult to copy/move, and is guaranteed to 130 | /// live beyond the lifetime of the Maybe, consider using a ReferencingMaybe 131 | /// instead. 132 | template 133 | class OwningMaybe 134 | { 135 | private: 136 | struct DummyType 137 | { 138 | char dummy{}; 139 | }; 140 | 141 | using StorageType = 142 | internal::TaggedUnion::type>; 143 | 144 | StorageType item_storage_; 145 | 146 | public: 147 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 148 | 149 | OwningMaybe() : item_storage_(DummyType()) {} 150 | 151 | explicit OwningMaybe(const T& value) : item_storage_(value) {} 152 | 153 | explicit OwningMaybe(T&& value) : item_storage_(std::move(value)) {} 154 | 155 | OwningMaybe(const OwningMaybe& other) = default; 156 | 157 | OwningMaybe(OwningMaybe&& other) = default; 158 | 159 | OwningMaybe& operator=(const OwningMaybe& other) = default; 160 | 161 | OwningMaybe& operator=(OwningMaybe&& other) = default; 162 | 163 | const T& Value() const 164 | { 165 | if (HasValue()) 166 | { 167 | return item_storage_.value_b_; 168 | } 169 | else 170 | { 171 | throw std::runtime_error("OwningMaybe does not have value"); 172 | } 173 | } 174 | 175 | T& Value() 176 | { 177 | if (HasValue()) 178 | { 179 | return item_storage_.value_b_; 180 | } 181 | else 182 | { 183 | throw std::runtime_error("OwningMaybe does not have value"); 184 | } 185 | } 186 | 187 | void Reset() { item_storage_ = StorageType(DummyType()); } 188 | 189 | bool HasValue() const { return (item_storage_.type_ == StorageType::TYPE_B); } 190 | 191 | explicit operator bool() const { return HasValue(); } 192 | }; 193 | 194 | template 195 | using OwningMaybeAllocator = Eigen::aligned_allocator>; 196 | 197 | /// An implementation of the Maybe/Optional pattern, in which a 198 | /// ReferencingMaybe wraps a reference to an instance of T or no value. This 199 | /// is similar to std::optional in C++17; however, it *does not own* the item 200 | /// T, unlike std::optional, since it contains a reference to the object. The 201 | /// referenced object *must* outlive the ReferencingMaybe! 202 | template 203 | class ReferencingMaybe 204 | { 205 | private: 206 | T* item_ptr_ = nullptr; 207 | 208 | public: 209 | ReferencingMaybe() : item_ptr_(nullptr) {} 210 | 211 | explicit ReferencingMaybe(T& item) : item_ptr_(std::addressof(item)) {} 212 | 213 | ReferencingMaybe(const ReferencingMaybe& other) = default; 214 | 215 | ReferencingMaybe(ReferencingMaybe&& other) = default; 216 | 217 | ReferencingMaybe& operator=(const ReferencingMaybe& other) = default; 218 | 219 | ReferencingMaybe& operator=(ReferencingMaybe&& other) = default; 220 | 221 | void Reset() { item_ptr_ = nullptr; } 222 | 223 | const T& Value() const 224 | { 225 | if (HasValue()) 226 | { 227 | return *item_ptr_; 228 | } 229 | else 230 | { 231 | throw std::runtime_error("ReferencingMaybe does not have value"); 232 | } 233 | } 234 | 235 | T& Value() 236 | { 237 | if (HasValue()) 238 | { 239 | return *item_ptr_; 240 | } 241 | else 242 | { 243 | throw std::runtime_error("ReferencingMaybe does not have value"); 244 | } 245 | } 246 | 247 | bool HasValue() const { return item_ptr_ != nullptr; } 248 | 249 | explicit operator bool() const { return HasValue(); } 250 | }; 251 | CRU_NAMESPACE_END 252 | } // namespace common_robotics_utilities 253 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/openmp_helpers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #if defined(_OPENMP) 9 | #include 10 | #endif 11 | 12 | #include 13 | 14 | namespace common_robotics_utilities 15 | { 16 | CRU_NAMESPACE_BEGIN 17 | namespace openmp_helpers 18 | { 19 | /// Returns true if OpenMP is enabled in the build, false otherwise. 20 | constexpr bool IsOmpEnabledInBuild() 21 | { 22 | #if defined(_OPENMP) 23 | return true; 24 | #else 25 | return false; 26 | #endif 27 | } 28 | 29 | /// Returns true if called from within an OpenMP context, false otherwise. 30 | inline bool IsOmpInParallel() 31 | { 32 | #if defined(_OPENMP) 33 | return static_cast(omp_in_parallel()); 34 | #else 35 | return false; 36 | #endif 37 | } 38 | 39 | /// Returns the OpenMP thread number in the current OpenMP parallel context. 40 | /// If called from outside an OpenMP parallel context or without OpenMP enabled, 41 | /// it will return 0. 42 | inline int32_t GetContextOmpThreadNum() 43 | { 44 | #if defined(_OPENMP) 45 | return static_cast(omp_get_thread_num()); 46 | #else 47 | return 0; 48 | #endif 49 | } 50 | 51 | /// Returns the maximum number of OpenMP threads available in the current OpenMP 52 | /// parallel context. If called outside an OpenMP parallel context or without 53 | /// OpenMP enabled, it will return 1. 54 | inline int32_t GetContextMaxNumOmpThreads() 55 | { 56 | #if defined(_OPENMP) 57 | const int32_t max_threads = static_cast(omp_get_max_threads()); 58 | if (max_threads > 0) 59 | { 60 | return max_threads; 61 | } 62 | else 63 | { 64 | throw std::runtime_error("OpenMP max threads <= 0"); 65 | } 66 | #else 67 | return 1; 68 | #endif 69 | } 70 | 71 | /// Returns the current number of OpenMP threads available in the current OpenMP 72 | /// parallel context. If called outside an OpenMP parallel context or without 73 | /// OpenMP enabled, it will return 1. 74 | inline int32_t GetContextNumOmpThreads() 75 | { 76 | #if defined(_OPENMP) 77 | const int32_t num_threads = static_cast(omp_get_num_threads()); 78 | if (num_threads > 0) 79 | { 80 | return num_threads; 81 | } 82 | else 83 | { 84 | throw std::runtime_error("OpenMP num threads <= 0"); 85 | } 86 | #else 87 | return 1; 88 | #endif 89 | } 90 | 91 | /// Returns the maximum number of OpenMP threads available in an OpenMP parallel 92 | /// context. If called without OpenMP enabled, it will return 1. 93 | /// Use caution if calling from within an existing OpenMP context, since nested 94 | /// contexts are not enabled by default. 95 | inline int32_t GetMaxNumOmpThreads() 96 | { 97 | #if defined(_OPENMP) 98 | int32_t max_num_threads = 0; 99 | #pragma omp parallel 100 | { 101 | max_num_threads = GetContextMaxNumOmpThreads(); 102 | } 103 | return max_num_threads; 104 | #else 105 | return 1; 106 | #endif 107 | } 108 | 109 | /// Returns the current number of OpenMP threads available in an OpenMP parallel 110 | /// context. If called without OpenMP enabled, it will return 1. 111 | /// Use caution if calling from within an existing OpenMP context, since nested 112 | /// contexts are not enabled by default. 113 | inline int32_t GetNumOmpThreads() 114 | { 115 | #if defined(_OPENMP) 116 | int32_t num_threads = 0; 117 | #pragma omp parallel 118 | { 119 | num_threads = GetContextNumOmpThreads(); 120 | } 121 | return num_threads; 122 | #else 123 | return 1; 124 | #endif 125 | } 126 | 127 | /// Returns the maximum possible number of OpenMP threads in the program. If 128 | /// called without OpenMP enabled, it will return 1. 129 | inline int32_t GetOmpThreadLimit() 130 | { 131 | #if defined(_OPENMP) 132 | const int32_t thread_limit = static_cast(omp_get_thread_limit()); 133 | if (thread_limit > 0) 134 | { 135 | return thread_limit; 136 | } 137 | else 138 | { 139 | throw std::runtime_error("OpenMP thread limit <= 0"); 140 | } 141 | #else 142 | return 1; 143 | #endif 144 | } 145 | 146 | /// RAII wrapper for changing the number of OpenMP threads temporarily. 147 | /// This wrapper changes OpenMP settings for *all* of the current program! 148 | /// It will throw an exception if called from within an existing OpenMP parallel 149 | /// context. 150 | class ChangeOmpNumThreadsWrapper 151 | { 152 | public: 153 | explicit ChangeOmpNumThreadsWrapper(const int32_t num_threads) 154 | { 155 | if (IsOmpInParallel()) 156 | { 157 | throw std::runtime_error( 158 | "Cannot create ChangeOmpNumThreadsWrapper inside an OpenMP parallel " 159 | "context"); 160 | } 161 | if (num_threads <= 0) 162 | { 163 | throw std::invalid_argument("num_threads must be greater than zero"); 164 | } 165 | starting_num_omp_threads_ = GetNumOmpThreads(); 166 | #if defined(_OPENMP) 167 | omp_set_num_threads(num_threads); 168 | #endif 169 | } 170 | 171 | virtual ~ChangeOmpNumThreadsWrapper() 172 | { 173 | #if defined(_OPENMP) 174 | omp_set_num_threads(starting_num_omp_threads_); 175 | #endif 176 | } 177 | 178 | private: 179 | int32_t starting_num_omp_threads_ = 0; 180 | }; 181 | 182 | /// RAII wrapper for disabling OpenMP temporarily. 183 | /// This wrapper changes OpenMP settings for *all* of the current program! 184 | /// It will throw an exception if called from within an existing OpenMP parallel 185 | /// context. 186 | class DisableOmpWrapper : public ChangeOmpNumThreadsWrapper 187 | { 188 | public: 189 | DisableOmpWrapper() : ChangeOmpNumThreadsWrapper(1) {} 190 | }; 191 | } // namespace openmp_helpers 192 | CRU_NAMESPACE_END 193 | } // namespace common_robotics_utilities 194 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/path_processing.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace common_robotics_utilities 16 | { 17 | CRU_NAMESPACE_BEGIN 18 | namespace path_processing 19 | { 20 | template> 21 | inline double ComputePercentCollisionFree( 22 | const Container& path, 23 | const std::function& edge_validity_check_fn) 25 | { 26 | if (path.size() >= 2) 27 | { 28 | const size_t num_edges = path.size() - 1; 29 | size_t collision_free_edges = 0; 30 | for (size_t idx = 1; idx < path.size(); idx++) 31 | { 32 | const Configuration& q1 = path[idx - 1]; 33 | const Configuration& q2 = path[idx]; 34 | const bool edge_valid = edge_validity_check_fn(q1, q2); 35 | if (edge_valid) 36 | { 37 | collision_free_edges += 1; 38 | } 39 | else 40 | { 41 | break; 42 | } 43 | } 44 | return static_cast(collision_free_edges) 45 | / static_cast(num_edges); 46 | } 47 | else if (path.size() == 1) 48 | { 49 | if (edge_validity_check_fn(path.front(), path.front())) 50 | { 51 | return 1.0; 52 | } 53 | else 54 | { 55 | return 0.0; 56 | } 57 | } 58 | else 59 | { 60 | return 1.0; 61 | } 62 | } 63 | 64 | template> 65 | inline Container AttemptShortcut( 66 | const Container& current_path, 67 | const size_t start_index, 68 | const size_t end_index, 69 | const uint32_t remaining_backtracking_steps, 70 | const double resample_shortcuts_interval, 71 | const bool check_for_marginal_shortcuts, 72 | const std::function& edge_validity_check_fn, 74 | const std::function& state_distance_fn, 76 | const std::function& state_interpolation_fn) 79 | { 80 | // Check if the edge is valid 81 | if (start_index >= end_index) 82 | { 83 | throw std::invalid_argument("start_index >= end_index"); 84 | } 85 | const Configuration& start_config = current_path[start_index]; 86 | const Configuration& end_config = current_path[end_index]; 87 | const bool edge_valid = edge_validity_check_fn(start_config, end_config); 88 | if (edge_valid) 89 | { 90 | // Make the shortcut 91 | Container shortcut; 92 | // Copy the start config 93 | shortcut.emplace_back(start_config); 94 | // Insert resampled states in the shortcut if needed 95 | if (resample_shortcuts_interval > 0.0) 96 | { 97 | const double distance = state_distance_fn(start_config, end_config); 98 | const double raw_num_intervals = distance / resample_shortcuts_interval; 99 | const uint32_t num_segments = 100 | static_cast(std::ceil(raw_num_intervals)); 101 | for (uint32_t segment = 1u; segment < num_segments; segment++) 102 | { 103 | const double interpolation_ratio 104 | = static_cast(segment) / static_cast(num_segments); 105 | shortcut.emplace_back( 106 | state_interpolation_fn(start_config, end_config, 107 | interpolation_ratio)); 108 | } 109 | } 110 | // Copy end config 111 | shortcut.emplace_back(end_config); 112 | if (shortcut.size() <= 2 || !check_for_marginal_shortcuts) 113 | { 114 | return shortcut; 115 | } 116 | // Check if this was a marginal path that could clip obstacles 117 | else if (ComputePercentCollisionFree( 118 | shortcut, edge_validity_check_fn) 119 | == 1.0) 120 | { 121 | return shortcut; 122 | } 123 | } 124 | // If we haven't already returned, the single shortcut has failed 125 | if (remaining_backtracking_steps > 0) 126 | { 127 | const size_t window = end_index - start_index; 128 | if (window >= 2) 129 | { 130 | const size_t half_window = window / 2; 131 | const size_t middle_index = start_index + half_window; 132 | // Attempt to shortcut each half independently 133 | const uint32_t available_backtracking_steps 134 | = remaining_backtracking_steps - 1; 135 | const auto first_half_shortcut 136 | = AttemptShortcut( 137 | current_path, start_index, middle_index, 138 | available_backtracking_steps, resample_shortcuts_interval, 139 | check_for_marginal_shortcuts, edge_validity_check_fn, 140 | state_distance_fn, state_interpolation_fn); 141 | const auto second_half_shortcut 142 | = AttemptShortcut( 143 | current_path, middle_index, end_index, 144 | available_backtracking_steps, resample_shortcuts_interval, 145 | check_for_marginal_shortcuts, edge_validity_check_fn, 146 | state_distance_fn, state_interpolation_fn); 147 | Container shortcut; 148 | if (first_half_shortcut.size() > 0 && second_half_shortcut.size() > 0) 149 | { 150 | shortcut.insert( 151 | shortcut.end(), first_half_shortcut.begin(), 152 | first_half_shortcut.end()); 153 | // Skip the first configuration, since this is a duplicate of the last 154 | // configuration in the first half shortcut 155 | shortcut.insert( 156 | shortcut.end(), second_half_shortcut.begin() + 1, 157 | second_half_shortcut.end()); 158 | } 159 | else if (first_half_shortcut.size() > 0) 160 | { 161 | shortcut.insert( 162 | shortcut.end(), first_half_shortcut.begin(), 163 | first_half_shortcut.end()); 164 | // Skip the middle configuration, since this is a duplicate of the 165 | // last configuration in the first half shortcut, but include the end 166 | // index 167 | shortcut.insert( 168 | shortcut.end(), 169 | current_path.begin() + static_cast(middle_index) + 1, 170 | current_path.begin() + static_cast(end_index) + 1); 171 | } 172 | else if (second_half_shortcut.size() > 0) 173 | { 174 | // Skip the middle configuration, since this is a duplicate of the 175 | // first configuration in the second half shortcut 176 | shortcut.insert( 177 | shortcut.end(), 178 | current_path.begin() + static_cast(start_index), 179 | current_path.begin() + static_cast(middle_index)); 180 | shortcut.insert( 181 | shortcut.end(), second_half_shortcut.begin(), 182 | second_half_shortcut.end()); 183 | } 184 | return shortcut; 185 | } 186 | } 187 | // If we get here, the shortcut failed and we return an empty shortcut path 188 | return Container(); 189 | } 190 | 191 | template> 193 | inline Container ShortcutSmoothPath( 194 | const Container& path, 195 | const uint32_t max_iterations, 196 | const uint32_t max_failed_iterations, 197 | const uint32_t max_backtracking_steps, 198 | const double max_shortcut_fraction, 199 | const double resample_shortcuts_interval, 200 | const bool check_for_marginal_shortcuts, 201 | const std::function& edge_validity_check_fn, 203 | const std::function& state_distance_fn, 205 | const std::function& state_interpolation_fn, 208 | const utility::UniformUnitRealFunction& uniform_unit_real_fn) 209 | { 210 | Container current_path = path; 211 | uint32_t num_iterations = 0; 212 | uint32_t failed_iterations = 0; 213 | while (num_iterations < max_iterations 214 | && failed_iterations < max_failed_iterations 215 | && current_path.size() > 2) 216 | { 217 | num_iterations++; 218 | // Attempt a shortcut 219 | const int64_t base_index = utility::GetUniformRandomIndex( 220 | uniform_unit_real_fn, static_cast(current_path.size())); 221 | // Pick an offset fraction 222 | const double offset_fraction = math::Interpolate( 223 | -max_shortcut_fraction, max_shortcut_fraction, uniform_unit_real_fn()); 224 | // Compute the offset index 225 | const int64_t offset_index 226 | = base_index + static_cast(std::floor( 227 | static_cast(current_path.size()) * offset_fraction)); 228 | // We need to clamp it to the bounds of the current path 229 | const int64_t safe_offset_index = utility::ClampValue( 230 | offset_index, INT64_C(0), 231 | static_cast(current_path.size() - 1)); 232 | // Get start & end indices 233 | const size_t start_index = 234 | static_cast(std::min(base_index, safe_offset_index)); 235 | const size_t end_index = 236 | static_cast(std::max(base_index, safe_offset_index)); 237 | if (end_index <= start_index + 1) 238 | { 239 | continue; 240 | } 241 | const auto shortcut = AttemptShortcut( 242 | current_path, start_index, end_index, max_backtracking_steps, 243 | resample_shortcuts_interval, check_for_marginal_shortcuts, 244 | edge_validity_check_fn, state_distance_fn, state_interpolation_fn); 245 | // An empty shortcut means it failed, since the shortcut must include the 246 | // start and end configurations 247 | if (shortcut.size() > 0) 248 | { 249 | Container shortened_path; 250 | if (start_index > 0) 251 | { 252 | // Copy the path before the shortcut (excluding start_index) 253 | shortened_path.insert( 254 | shortened_path.end(), current_path.begin(), 255 | current_path.begin() + static_cast(start_index)); 256 | } 257 | // Copy the shortcut 258 | shortened_path.insert( 259 | shortened_path.end(), shortcut.begin(), shortcut.end()); 260 | if (end_index < current_path.size() - 1) 261 | { 262 | // Copy the path after the shortcut (excluding end_index) 263 | shortened_path.insert( 264 | shortened_path.end(), 265 | current_path.begin() + static_cast(end_index) + 1, 266 | current_path.end()); 267 | } 268 | // Swap in as the new current path 269 | current_path = shortened_path; 270 | } 271 | else 272 | { 273 | failed_iterations++; 274 | } 275 | } 276 | return current_path; 277 | } 278 | 279 | template> 280 | inline Container ResamplePath( 281 | const Container& path, 282 | const double resampled_state_distance, 283 | const std::function& state_distance_fn, 285 | const std::function& state_interpolation_fn) 288 | { 289 | if (path.size() <= 1) 290 | { 291 | return path; 292 | } 293 | if (resampled_state_distance <= 0.0) 294 | { 295 | throw std::invalid_argument("resampled_state_distance must be > 0"); 296 | } 297 | Container resampled_path; 298 | // Add the first state 299 | resampled_path.push_back(path[0]); 300 | // Loop through the path, adding interpolated states as needed 301 | for (size_t idx = 1; idx < path.size(); idx++) 302 | { 303 | // Get the states from the original path 304 | const Configuration& previous_state = path[idx - 1]; 305 | const Configuration& current_state = path[idx]; 306 | // We want to add all the intermediate states to our returned path 307 | const double distance = state_distance_fn(previous_state, current_state); 308 | const double raw_num_intervals = distance / resampled_state_distance; 309 | const uint32_t num_segments 310 | = static_cast(std::ceil(raw_num_intervals)); 311 | // If there's only one segment, we just add the end state of the window 312 | if (num_segments == 0u) 313 | { 314 | // Do nothing because this means distance was exactly 0 315 | } 316 | else if (num_segments == 1u) 317 | { 318 | // Add a single point for the other end of the segment 319 | resampled_path.push_back(current_state); 320 | } 321 | // If there is more than one segment, interpolate between previous_state and 322 | // current_state (including the current_state) 323 | else 324 | { 325 | for (uint32_t segment = 1u; segment <= num_segments; segment++) 326 | { 327 | const double interpolation_ratio 328 | = static_cast(segment) / static_cast(num_segments); 329 | const Configuration interpolated_state 330 | = state_interpolation_fn( 331 | previous_state, current_state, interpolation_ratio); 332 | resampled_path.push_back(interpolated_state); 333 | } 334 | } 335 | } 336 | return resampled_path; 337 | } 338 | } // namespace path_processing 339 | CRU_NAMESPACE_END 340 | } // namespace common_robotics_utilities 341 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/random_rotation_generator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace common_robotics_utilities 12 | { 13 | CRU_NAMESPACE_BEGIN 14 | namespace random_rotation_generator 15 | { 16 | /// Generator for uniform random quaternions and Euler angles. 17 | class RandomRotationGenerator 18 | { 19 | private: 20 | std::uniform_real_distribution uniform_unit_dist_; 21 | 22 | public: 23 | RandomRotationGenerator() : uniform_unit_dist_(0.0, 1.0) {} 24 | 25 | // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 26 | // see pages 124-132. 27 | static Eigen::Quaterniond GenerateUniformRandomQuaternion( 28 | const std::function& uniform_unit_dist) 29 | { 30 | const double x0 = uniform_unit_dist(); 31 | const double r1 = std::sqrt(1.0 - x0); 32 | const double r2 = std::sqrt(x0); 33 | const double t1 = 2.0 * M_PI * uniform_unit_dist(); 34 | const double t2 = 2.0 * M_PI * uniform_unit_dist(); 35 | const double c1 = std::cos(t1); 36 | const double s1 = std::sin(t1); 37 | const double c2 = std::cos(t2); 38 | const double s2 = std::sin(t2); 39 | const double x = s1 * r1; 40 | const double y = c1 * r1; 41 | const double z = s2 * r2; 42 | const double w = c2 * r2; 43 | return Eigen::Quaterniond(w, x, y, z); 44 | } 45 | 46 | // From Effective Sampling and Distance Metrics for 3D Rigid Body Path 47 | // Planning, by James Kuffner, ICRA 2004. 48 | static Eigen::Vector3d GenerateUniformRandomEulerAngles( 49 | const std::function& uniform_unit_dist) 50 | { 51 | const double roll = 2.0 * M_PI * uniform_unit_dist() - M_PI; 52 | const double pitch_init 53 | = std::acos(1.0 - (2.0 * uniform_unit_dist())) + M_PI_2; 54 | const double pitch 55 | = (uniform_unit_dist() < 0.5) 56 | ? ((pitch_init < M_PI) ? pitch_init + M_PI : pitch_init - M_PI) 57 | : pitch_init; 58 | const double yaw = 2.0 * M_PI * uniform_unit_dist() - M_PI; 59 | return Eigen::Vector3d(roll, pitch, yaw); 60 | } 61 | 62 | template 63 | Eigen::Quaterniond GetQuaternion(Generator& prng) 64 | { 65 | std::function uniform_rand_fn 66 | = [&] () { return uniform_unit_dist_(prng); }; 67 | return GenerateUniformRandomQuaternion(uniform_rand_fn); 68 | } 69 | 70 | template 71 | std::vector GetRawQuaternion(Generator& prng) 72 | { 73 | const Eigen::Quaterniond quat = GetQuaternion(prng); 74 | return std::vector{quat.x(), quat.y(), quat.z(), quat.w()}; 75 | } 76 | 77 | template 78 | Eigen::Vector3d GetEulerAngles(Generator& prng) 79 | { 80 | std::function uniform_rand_fn 81 | = [&] () { return uniform_unit_dist_(prng); }; 82 | return GenerateUniformRandomEulerAngles(uniform_rand_fn); 83 | } 84 | 85 | template 86 | std::vector GetRawEulerAngles(Generator& prng) 87 | { 88 | const Eigen::Vector3d angles = GetEulerAngles(prng); 89 | return std::vector{angles.x(), angles.y(), angles.z()}; 90 | } 91 | }; 92 | } // namespace random_rotation_generator 93 | CRU_NAMESPACE_END 94 | } // namespace common_robotics_utilities 95 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/ros_conversions.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 10 | #include 11 | #include 12 | #include 13 | #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 14 | #include 15 | #include 16 | #include 17 | #else 18 | #error "Undefined or unknown COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION" 19 | #endif 20 | 21 | namespace common_robotics_utilities 22 | { 23 | CRU_NAMESPACE_BEGIN 24 | namespace ros_conversions 25 | { 26 | 27 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 28 | using GeometryPoint = geometry_msgs::msg::Point; 29 | using GeometryPointStamped = geometry_msgs::msg::PointStamped; 30 | using GeometryPose = geometry_msgs::msg::Pose; 31 | using GeometryPoseStamped = geometry_msgs::msg::PoseStamped; 32 | using GeometryQuaternion = geometry_msgs::msg::Quaternion; 33 | using GeometryTransform = geometry_msgs::msg::Transform; 34 | using GeometryTransformStamped = geometry_msgs::msg::TransformStamped; 35 | using GeometryVector3 = geometry_msgs::msg::Vector3; 36 | #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 37 | using GeometryPoint = geometry_msgs::Point; 38 | using GeometryPointStamped = geometry_msgs::PointStamped; 39 | using GeometryPose = geometry_msgs::Pose; 40 | using GeometryPoseStamped = geometry_msgs::PoseStamped; 41 | using GeometryQuaternion = geometry_msgs::Quaternion; 42 | using GeometryTransform = geometry_msgs::Transform; 43 | using GeometryTransformStamped = geometry_msgs::TransformStamped; 44 | using GeometryVector3 = geometry_msgs::Vector3; 45 | #endif 46 | 47 | Eigen::Vector3d GeometryPointToEigenVector3d(const GeometryPoint& point); 48 | 49 | GeometryPoint EigenVector3dToGeometryPoint(const Eigen::Vector3d& point); 50 | 51 | Eigen::Vector4d GeometryPointToEigenVector4d(const GeometryPoint& point); 52 | 53 | GeometryPoint EigenVector4dToGeometryPoint(const Eigen::Vector4d& point); 54 | 55 | GeometryPointStamped EigenVector3dToGeometryPointStamped( 56 | const Eigen::Vector3d& point, const std::string& frame_id); 57 | 58 | Eigen::Vector3d GeometryVector3ToEigenVector3d(const GeometryVector3& vector); 59 | 60 | GeometryVector3 EigenVector3dToGeometryVector3(const Eigen::Vector3d& vector); 61 | 62 | Eigen::Vector4d GeometryVector3ToEigenVector4d(const GeometryVector3& vector); 63 | 64 | GeometryVector3 EigenVector4dToGeometryVector3(const Eigen::Vector4d& vector); 65 | 66 | Eigen::Quaterniond GeometryQuaternionToEigenQuaterniond( 67 | const GeometryQuaternion& quat); 68 | 69 | GeometryQuaternion EigenQuaterniondToGeometryQuaternion( 70 | const Eigen::Quaterniond& quat); 71 | 72 | Eigen::Isometry3d GeometryPoseToEigenIsometry3d(const GeometryPose& pose); 73 | 74 | GeometryPose EigenIsometry3dToGeometryPose(const Eigen::Isometry3d& transform); 75 | 76 | GeometryPoseStamped EigenIsometry3dToGeometryPoseStamped( 77 | const Eigen::Isometry3d& transform, const std::string& frame_id); 78 | 79 | Eigen::Isometry3d GeometryTransformToEigenIsometry3d( 80 | const GeometryTransform& transform); 81 | 82 | GeometryTransform EigenIsometry3dToGeometryTransform( 83 | const Eigen::Isometry3d& transform); 84 | 85 | GeometryTransformStamped EigenIsometry3dToGeometryTransformStamped( 86 | const Eigen::Isometry3d& transform, const std::string& frame_id, 87 | const std::string& child_frame_id); 88 | 89 | Eigen::Matrix3Xd VectorGeometryPointToEigenMatrix3Xd( 90 | const std::vector& vector_geom); 91 | 92 | std::vector EigenMatrix3XdToVectorGeometryPoint( 93 | const Eigen::Matrix3Xd& eigen_matrix); 94 | 95 | std::vector 96 | VectorEigenVector3dToVectorGeometryPoint( 97 | const common_robotics_utilities::math::VectorVector3d& vector_eigen); 98 | 99 | common_robotics_utilities::math::VectorVector3d 100 | VectorGeometryPointToVectorEigenVector3d( 101 | const std::vector& vector_geom); 102 | 103 | common_robotics_utilities::math::VectorVector3d 104 | VectorGeometryVector3ToEigenVector3d( 105 | const std::vector& vector_geom); 106 | 107 | common_robotics_utilities::math::VectorIsometry3d 108 | VectorGeometryPoseToVectorIsometry3d( 109 | const std::vector& vector_geom); 110 | 111 | common_robotics_utilities::math::VectorIsometry3d 112 | VectorGeometryPoseToVectorIsometry3d( 113 | const std::vector& vector_geom); 114 | 115 | std::vector VectorIsometry3dToVectorGeometryPose( 116 | const common_robotics_utilities::math::VectorIsometry3d& vector_eigen); 117 | 118 | std::vector VectorIsometry3dToVectorGeometryTransform( 119 | const common_robotics_utilities::math::VectorIsometry3d& vector_eigen); 120 | } // namespace ros_conversions 121 | CRU_NAMESPACE_END 122 | } // namespace common_robotics_utilities 123 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/ros_helpers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 10 | // The rosidl_runtime_cpp/traits.hpp header is missing this include, so we 11 | // include it first as a workaround. 12 | #include 13 | #include 14 | // On pre-Humble distributions, a single message header is included to ensure 15 | // that rosidl_generator_traits::to_yaml is found by the compiler. The version 16 | // header is only present in Humble or later, so we use the absence of the file 17 | // to detect earlier distributions. 18 | #if !__has_include() 19 | #include 20 | #endif 21 | #include 22 | #include 23 | #include 24 | #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 25 | #include 26 | #include 27 | #include 28 | #else 29 | #error "Undefined or unknown COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION" 30 | #endif 31 | 32 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 33 | /// Define an ostream operator for ROS 2 message types. 34 | namespace std 35 | { 36 | template 37 | enable_if_t::value, ostream>& 38 | operator<<(ostream& os, const T& message) 39 | { 40 | #if !__has_include() 41 | // ROS 2 Galactic only supports block-style output. 42 | os << rosidl_generator_traits::to_yaml(message); 43 | #else 44 | // ROS 2 Humble and later support optional flow-style output found via ADL. 45 | constexpr bool use_flow_style = false; 46 | os << to_yaml(message, use_flow_style); 47 | #endif 48 | return os; 49 | } 50 | } // namespace std 51 | #endif 52 | 53 | namespace common_robotics_utilities 54 | { 55 | CRU_NAMESPACE_BEGIN 56 | namespace ros_helpers 57 | { 58 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 59 | using RosTime = rclcpp::Time; 60 | #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 61 | using RosTime = ros::Time; 62 | #endif 63 | 64 | template > 65 | void SetMessageTimestamps(Container& messages, const RosTime& timestamp) 66 | { 67 | for (auto& message : messages) 68 | { 69 | message.header.stamp = timestamp; 70 | } 71 | } 72 | 73 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 74 | using VisualizationMarker = visualization_msgs::msg::Marker; 75 | using VisualizationMarkerArray = visualization_msgs::msg::MarkerArray; 76 | #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 77 | using VisualizationMarker = visualization_msgs::Marker; 78 | using VisualizationMarkerArray = visualization_msgs::MarkerArray; 79 | #endif 80 | 81 | inline void SetMessageTimestamps( 82 | VisualizationMarkerArray& markers, const RosTime& timestamp) 83 | { 84 | SetMessageTimestamps(markers.markers, timestamp); 85 | } 86 | 87 | } // namespace ros_helpers 88 | CRU_NAMESPACE_END 89 | } // namespace common_robotics_utilities 90 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/simple_dtw.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace common_robotics_utilities 13 | { 14 | CRU_NAMESPACE_BEGIN 15 | namespace simple_dtw 16 | { 17 | template, 19 | typename SecondContainer=std::vector> 20 | class SimpleDTW 21 | { 22 | protected: 23 | void InitializeMatrix(const ssize_t first_sequence_size, 24 | const ssize_t second_sequence_size) 25 | { 26 | const ssize_t rows = first_sequence_size + 1; 27 | const ssize_t cols = second_sequence_size + 1; 28 | if (dtw_matrix_.rows() < rows || dtw_matrix_.cols() < cols) 29 | { 30 | dtw_matrix_ = Eigen::MatrixXd::Zero(rows, cols); 31 | if (rows > 1 && cols > 1) 32 | { 33 | for (ssize_t row = 1; row < rows; row++) 34 | { 35 | dtw_matrix_(row, 0) = std::numeric_limits::infinity(); 36 | } 37 | for (ssize_t col = 1; col < cols; col++) 38 | { 39 | dtw_matrix_(0, col) = std::numeric_limits::infinity(); 40 | } 41 | } 42 | } 43 | } 44 | 45 | Eigen::MatrixXd dtw_matrix_; 46 | 47 | public: 48 | SimpleDTW() 49 | { 50 | InitializeMatrix(0, 0); 51 | } 52 | 53 | SimpleDTW(const ssize_t first_sequence_size, 54 | const ssize_t second_sequence_size) 55 | { 56 | InitializeMatrix(first_sequence_size, second_sequence_size); 57 | } 58 | 59 | double EvaluateWarpingCost( 60 | const FirstContainer& first_sequence, 61 | const SecondContainer& second_sequence, 62 | const std::function& distance_fn) 64 | { 65 | if (first_sequence.empty()) 66 | { 67 | throw std::invalid_argument("first_sequence is empty"); 68 | } 69 | if (second_sequence.empty()) 70 | { 71 | throw std::invalid_argument("second_sequence is empty"); 72 | } 73 | const ssize_t first_sequence_size 74 | = static_cast(first_sequence.size()); 75 | const ssize_t second_sequence_size 76 | = static_cast(second_sequence.size()); 77 | InitializeMatrix(first_sequence_size, second_sequence_size); 78 | // Compute DTW cost for the two sequences 79 | for (ssize_t i = 1; i <= first_sequence_size; i++) 80 | { 81 | const FirstDatatype& first_item 82 | = first_sequence[static_cast(i - 1)]; 83 | for (ssize_t j = 1; j <= second_sequence_size; j++) 84 | { 85 | const SecondDatatype& second_item 86 | = second_sequence[static_cast(j - 1)]; 87 | const double index_cost = distance_fn(first_item, second_item); 88 | double prev_cost = 0.0; 89 | // Get the next neighboring values from the matrix to use for the update 90 | const double im1j = dtw_matrix_(i - 1, j); 91 | const double im1jm1 = dtw_matrix_(i - 1, j - 1); 92 | const double ijm1 = dtw_matrix_(i, j - 1); 93 | // Start the update step 94 | if (im1j < im1jm1 && im1j < ijm1) 95 | { 96 | prev_cost = im1j; 97 | } 98 | else if (ijm1 < im1j && ijm1 < im1jm1) 99 | { 100 | prev_cost = ijm1; 101 | } 102 | else 103 | { 104 | prev_cost = im1jm1; 105 | } 106 | // Update the value in the matrix 107 | const double new_cost = index_cost + prev_cost; 108 | dtw_matrix_(i, j) = new_cost; 109 | } 110 | } 111 | //Return total path cost 112 | const double warping_cost 113 | = dtw_matrix_(first_sequence_size, second_sequence_size); 114 | return warping_cost; 115 | } 116 | }; 117 | 118 | template, 120 | typename SecondContainer=std::vector> 121 | inline double EvaluateWarpingCost( 122 | const FirstContainer& first_sequence, 123 | const SecondContainer& second_sequence, 124 | const std::function& distance_fn) 126 | { 127 | SimpleDTW dtw_evaluator; 129 | return dtw_evaluator.EvaluateWarpingCost( 130 | first_sequence, second_sequence, distance_fn); 131 | } 132 | } // namespace simple_dtw 133 | CRU_NAMESPACE_END 134 | } // namespace common_robotics_utilities 135 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/simple_hausdorff_distance.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace common_robotics_utilities 14 | { 15 | CRU_NAMESPACE_BEGIN 16 | /// Compute Hausdorff distance between two distributions. 17 | /// "The Hausdorff distance is the longest distance you can be forced to travel 18 | /// by an adversary who chooses a point in one of the two sets, from where you 19 | /// then must travel to the other set. In other words, it is the greatest of all 20 | /// the distances from a point in one set to the closest point in the other 21 | /// set." - from Wikipedia 22 | namespace simple_hausdorff_distance 23 | { 24 | namespace internal 25 | { 26 | /// Compute Hausdorff distance between @param first_distribution and @param 27 | /// second_distribution using @param distance_function to compute distance 28 | /// between an element in @param first_distribution and an element in @param 29 | /// second_distribution. @return distance between distributions. 30 | /// Computation is performed in parallel, and the larger of the two input 31 | /// distributions is automatically selected to be the outer (parallelized) loop. 32 | template, 34 | typename SecondContainer=std::vector> 35 | inline double ComputeDistanceParallel( 36 | const FirstContainer& first_distribution, 37 | const SecondContainer& second_distribution, 38 | const std::function& distance_fn, 40 | const parallelism::DegreeOfParallelism& parallelism) 41 | { 42 | if (first_distribution.empty()) 43 | { 44 | throw std::invalid_argument("first_distribution is empty"); 45 | } 46 | if (second_distribution.empty()) 47 | { 48 | throw std::invalid_argument("second_distribution is empty"); 49 | } 50 | 51 | // Make per-thread storage 52 | std::vector per_thread_storage( 53 | static_cast(parallelism.GetNumThreads()), 0.0); 54 | 55 | const auto per_item_work = [&](const int32_t thread_num, const int64_t index) 56 | { 57 | const FirstDatatype& first = first_distribution[static_cast(index)]; 58 | 59 | double minimum_distance = std::numeric_limits::infinity(); 60 | 61 | for (size_t jdx = 0; jdx < second_distribution.size(); jdx++) 62 | { 63 | const SecondDatatype& second = second_distribution[jdx]; 64 | const double current_distance = distance_fn(first, second); 65 | if (current_distance < minimum_distance) 66 | { 67 | minimum_distance = current_distance; 68 | } 69 | } 70 | 71 | if (minimum_distance > per_thread_storage.at(thread_num)) 72 | { 73 | per_thread_storage.at(thread_num) = minimum_distance; 74 | } 75 | }; 76 | 77 | parallelism::StaticParallelForIndexLoop( 78 | parallelism, 0, static_cast(first_distribution.size()), 79 | per_item_work, parallelism::ParallelForBackend::BEST_AVAILABLE); 80 | 81 | double maximum_minimum_distance = 0.0; 82 | for (const double& temp_minimum_distance : per_thread_storage) 83 | { 84 | if (temp_minimum_distance > maximum_minimum_distance) 85 | { 86 | maximum_minimum_distance = temp_minimum_distance; 87 | } 88 | } 89 | 90 | return maximum_minimum_distance; 91 | } 92 | 93 | /// Compute Hausdorff distance between @param first_distribution and @param 94 | /// second_distribution using @param distance_function to compute distance 95 | /// between an element in @param first_distribution and an element in @param 96 | /// second_distribution. @return distance between distributions. 97 | template, 99 | typename SecondContainer=std::vector> 100 | inline double ComputeDistanceSerial( 101 | const FirstContainer& first_distribution, 102 | const SecondContainer& second_distribution, 103 | const std::function& distance_fn) 105 | { 106 | if (first_distribution.empty()) 107 | { 108 | throw std::invalid_argument("first_distribution is empty"); 109 | } 110 | if (second_distribution.empty()) 111 | { 112 | throw std::invalid_argument("second_distribution is empty"); 113 | } 114 | 115 | double maximum_minimum_distance = 0.0; 116 | 117 | for (size_t idx = 0; idx < first_distribution.size(); idx++) 118 | { 119 | const FirstDatatype& first = first_distribution[idx]; 120 | 121 | double minimum_distance = std::numeric_limits::infinity(); 122 | 123 | for (size_t jdx = 0; jdx < second_distribution.size(); jdx++) 124 | { 125 | const SecondDatatype& second = second_distribution[jdx]; 126 | const double current_distance = distance_fn(first, second); 127 | if (current_distance < minimum_distance) 128 | { 129 | minimum_distance = current_distance; 130 | } 131 | } 132 | 133 | if (minimum_distance > maximum_minimum_distance) 134 | { 135 | maximum_minimum_distance = minimum_distance; 136 | } 137 | } 138 | 139 | return maximum_minimum_distance; 140 | } 141 | 142 | /// Compute Hausdorff distance between @param first_distribution and @param 143 | /// second_distribution using @param distance_matrix which stores the pairwise 144 | /// distance between all elements in @param first_distribution and @param 145 | /// second_distribution. @return distance between distributions. 146 | /// Computation is performed in parallel, and the larger of the two input 147 | /// distributions is automatically selected to be the outer (parallelized) loop. 148 | template, 150 | typename SecondContainer=std::vector> 151 | inline double ComputeDistanceParallel( 152 | const FirstContainer& first_distribution, 153 | const SecondContainer& second_distribution, 154 | const Eigen::MatrixXd& distance_matrix, 155 | const parallelism::DegreeOfParallelism& parallelism) 156 | { 157 | if (first_distribution.empty()) 158 | { 159 | throw std::invalid_argument("first_distribution is empty"); 160 | } 161 | if (second_distribution.empty()) 162 | { 163 | throw std::invalid_argument("second_distribution is empty"); 164 | } 165 | if (static_cast(distance_matrix.rows()) != first_distribution.size() 166 | || static_cast(distance_matrix.cols()) 167 | != second_distribution.size()) 168 | { 169 | throw std::invalid_argument("distance_matrix is the wrong size"); 170 | } 171 | 172 | // Make per-thread storage 173 | std::vector per_thread_storage( 174 | static_cast(parallelism.GetNumThreads()), 0.0); 175 | 176 | const auto per_item_work = [&](const int32_t thread_num, const int64_t index) 177 | { 178 | double minimum_distance = std::numeric_limits::infinity(); 179 | 180 | for (size_t jdx = 0; jdx < second_distribution.size(); jdx++) 181 | { 182 | const double current_distance = distance_matrix(index, jdx); 183 | if (current_distance < minimum_distance) 184 | { 185 | minimum_distance = current_distance; 186 | } 187 | } 188 | 189 | if (minimum_distance > per_thread_storage.at(thread_num)) 190 | { 191 | per_thread_storage.at(thread_num) = minimum_distance; 192 | } 193 | }; 194 | 195 | parallelism::StaticParallelForIndexLoop( 196 | parallelism, 0, static_cast(first_distribution.size()), 197 | per_item_work, parallelism::ParallelForBackend::BEST_AVAILABLE); 198 | 199 | double maximum_minimum_distance = 0.0; 200 | for (const double& temp_minimum_distance : per_thread_storage) 201 | { 202 | if (temp_minimum_distance > maximum_minimum_distance) 203 | { 204 | maximum_minimum_distance = temp_minimum_distance; 205 | } 206 | } 207 | 208 | return maximum_minimum_distance; 209 | } 210 | 211 | /// Compute Hausdorff distance between @param first_distribution and @param 212 | /// second_distribution using @param distance_matrix which stores the pairwise 213 | /// distance between all elements in @param first_distribution and @param 214 | /// second_distribution. @return distance between distributions. 215 | template, 217 | typename SecondContainer=std::vector> 218 | inline double ComputeDistanceSerial( 219 | const FirstContainer& first_distribution, 220 | const SecondContainer& second_distribution, 221 | const Eigen::MatrixXd& distance_matrix) 222 | { 223 | if (first_distribution.empty()) 224 | { 225 | throw std::invalid_argument("first_distribution is empty"); 226 | } 227 | if (second_distribution.empty()) 228 | { 229 | throw std::invalid_argument("second_distribution is empty"); 230 | } 231 | if (static_cast(distance_matrix.rows()) != first_distribution.size() 232 | || static_cast(distance_matrix.cols()) 233 | != second_distribution.size()) 234 | { 235 | throw std::invalid_argument("distance_matrix is the wrong size"); 236 | } 237 | 238 | double maximum_minimum_distance = 0.0; 239 | 240 | for (size_t idx = 0; idx < first_distribution.size(); idx++) 241 | { 242 | double minimum_distance = std::numeric_limits::infinity(); 243 | 244 | for (size_t jdx = 0; jdx < second_distribution.size(); jdx++) 245 | { 246 | const double current_distance 247 | = distance_matrix(static_cast(idx), 248 | static_cast(jdx)); 249 | if (current_distance < minimum_distance) 250 | { 251 | minimum_distance = current_distance; 252 | } 253 | } 254 | 255 | if (minimum_distance > maximum_minimum_distance) 256 | { 257 | maximum_minimum_distance = minimum_distance; 258 | } 259 | } 260 | 261 | return maximum_minimum_distance; 262 | } 263 | } // namespace internal 264 | 265 | /// Compute Hausdorff distance between @param first_distribution and @param 266 | /// second_distribution using @param distance_function to compute distance 267 | /// between an element in @param first_distribution and an element in @param 268 | /// second_distribution. @return distance between distributions. 269 | /// @param parallelism selects if/how the computation should be performed in 270 | /// parallel. If computation is performed in parallel, the larger of the two 271 | /// input distributions is automatically selected to be the outer (parallelized) 272 | /// loop. 273 | template, 275 | typename SecondContainer=std::vector> 276 | inline double ComputeDistance( 277 | const FirstContainer& first_distribution, 278 | const SecondContainer& second_distribution, 279 | const std::function& distance_fn, 281 | const parallelism::DegreeOfParallelism& parallelism) 282 | { 283 | if (parallelism.IsParallel()) 284 | { 285 | return internal::ComputeDistanceParallel 286 | ( 287 | first_distribution, second_distribution, distance_fn, 288 | parallelism); 289 | } 290 | else 291 | { 292 | return internal::ComputeDistanceSerial 293 | ( 294 | first_distribution, second_distribution, distance_fn); 295 | } 296 | } 297 | 298 | /// Compute Hausdorff distance between @param first_distribution and @param 299 | /// second_distribution using @param distance_matrix which stores the pairwise 300 | /// distance between all elements in @param first_distribution and @param 301 | /// second_distribution. @return distance between distributions. 302 | /// @param parallelism selects if/how the computation should be performed in 303 | /// parallel. If computation is performed in parallel, the larger of the two 304 | /// input distributions is automatically selected to be the outer (parallelized) 305 | /// loop. 306 | template, 308 | typename SecondContainer=std::vector> 309 | inline double ComputeDistance( 310 | const FirstContainer& first_distribution, 311 | const SecondContainer& second_distribution, 312 | const Eigen::MatrixXd& distance_matrix, 313 | const parallelism::DegreeOfParallelism& parallelism) 314 | { 315 | if (parallelism.IsParallel()) 316 | { 317 | return internal::ComputeDistanceParallel 318 | ( 319 | first_distribution, second_distribution, distance_matrix, 320 | parallelism); 321 | } 322 | else 323 | { 324 | return internal::ComputeDistanceSerial 325 | ( 326 | first_distribution, second_distribution, distance_matrix); 327 | } 328 | } 329 | } // namespace simple_hausdorff_distance 330 | CRU_NAMESPACE_END 331 | } // namespace common_robotics_utilities 332 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/simple_knearest_neighbors.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace common_robotics_utilities 16 | { 17 | CRU_NAMESPACE_BEGIN 18 | namespace simple_knearest_neighbors 19 | { 20 | /// Type to wrap an index and its corresponding distance. 21 | class IndexAndDistance 22 | { 23 | private: 24 | int64_t index_ = -1; 25 | double distance_ = std::numeric_limits::infinity(); 26 | 27 | public: 28 | IndexAndDistance() {} 29 | 30 | IndexAndDistance(const int64_t index, const double distance) 31 | : index_(index), distance_(distance) {} 32 | 33 | IndexAndDistance(const size_t index, const double distance) 34 | : index_(static_cast(index)), distance_(distance) {} 35 | 36 | void SetIndexAndDistance(const int64_t index, const double distance) 37 | { 38 | index_ = index; 39 | distance_ = distance; 40 | } 41 | 42 | void SetIndexAndDistance(const size_t index, const double distance) 43 | { 44 | SetIndexAndDistance(static_cast(index), distance); 45 | } 46 | 47 | void SetFromOther(const IndexAndDistance& other) 48 | { 49 | SetIndexAndDistance(other.Index(), other.Distance()); 50 | } 51 | 52 | int64_t Index() const { return index_; } 53 | 54 | double Distance() const { return distance_; } 55 | }; 56 | 57 | /// Comparison function for std::max_element. 58 | inline bool IndexAndDistanceCompare( 59 | const IndexAndDistance& index1, const IndexAndDistance& index2) 60 | { 61 | return (index1.Distance() < index2.Distance()); 62 | } 63 | 64 | /// @return vector> of the nearest @param K neighbors to 65 | /// @param current in the specified range [range_start, range_end) of 66 | /// @param items, with distance computed by @param distance_fn. 67 | template> 68 | inline std::vector GetKNearestNeighborsInRangeSerial( 69 | const Container& items, const size_t range_start, const size_t range_end, 70 | const Value& current, 71 | const std::function& distance_fn, 72 | const int64_t K) 73 | { 74 | if (range_end < range_start) 75 | { 76 | throw std::invalid_argument("range_end < range_start"); 77 | } 78 | if (range_end > items.size()) 79 | { 80 | throw std::invalid_argument("range_end > items.size()"); 81 | } 82 | 83 | const size_t range_size = range_end - range_start; 84 | 85 | if (K < 0) 86 | { 87 | throw std::invalid_argument("K must be >= 0"); 88 | } 89 | else if (K == 0 || range_size == 0) 90 | { 91 | return std::vector(); 92 | } 93 | else if (range_size <= static_cast(K)) 94 | { 95 | std::vector k_nearests(range_size); 96 | for (size_t idx = range_start; idx < range_end; idx++) 97 | { 98 | const Item& item = items[idx]; 99 | const double distance = distance_fn(item, current); 100 | k_nearests[idx - range_start].SetIndexAndDistance(idx, distance); 101 | } 102 | return k_nearests; 103 | } 104 | else 105 | { 106 | // Collect index + distance for all items. 107 | std::vector all_distances(range_size); 108 | for (size_t idx = range_start; idx < range_end; idx++) 109 | { 110 | const Item& item = items[idx]; 111 | const double distance = distance_fn(item, current); 112 | all_distances[idx - range_start] = IndexAndDistance(idx, distance); 113 | } 114 | 115 | // Sort the K smallest elements by distance. 116 | // Uses an O(range_size * log(K)) approach. 117 | std::partial_sort( 118 | all_distances.begin(), all_distances.begin() + K, all_distances.end(), 119 | IndexAndDistanceCompare); 120 | 121 | // Return the first K elements by resizing down. 122 | all_distances.resize(static_cast(K)); 123 | return all_distances; 124 | } 125 | } 126 | 127 | /// @return vector> of the nearest @param K neighbors to 128 | /// @param current in the specified range [range_start, range_end) of 129 | /// @param items, with distance computed by @param distance_fn and search 130 | /// performed in parallel. 131 | /// @param parallelism control if/how search is performed in parallel. 132 | template> 133 | inline std::vector GetKNearestNeighborsInRangeParallel( 134 | const Container& items, const size_t range_start, const size_t range_end, 135 | const Value& current, 136 | const std::function& distance_fn, 137 | const int64_t K, const parallelism::DegreeOfParallelism& parallelism) 138 | { 139 | if (range_end < range_start) 140 | { 141 | throw std::invalid_argument("range_end < range_start"); 142 | } 143 | if (range_end > items.size()) 144 | { 145 | throw std::invalid_argument("range_end > items.size()"); 146 | } 147 | 148 | const size_t range_size = range_end - range_start; 149 | 150 | // Handle the easy cases where no comparisons must be performed. 151 | if (K < 0) 152 | { 153 | throw std::invalid_argument("K must be >= 0"); 154 | } 155 | else if (K == 0 || range_size == 0) 156 | { 157 | return std::vector(); 158 | } 159 | else 160 | { 161 | // Per-thread work calculation common to both range_size <= K and full case. 162 | const int64_t num_threads = parallelism.GetNumThreads(); 163 | 164 | // Easy case where we only need to compute distance for each element. 165 | if (range_size <= static_cast(K)) 166 | { 167 | std::vector k_nearests(range_size); 168 | 169 | // Helper lambda for each item's work. 170 | const auto per_item_work = [&](const int32_t, const int64_t index) 171 | { 172 | const Item& item = items[static_cast(index)]; 173 | const double distance = distance_fn(item, current); 174 | k_nearests[static_cast(index) - range_start] 175 | .SetIndexAndDistance(index, distance); 176 | }; 177 | 178 | // Dispatch, preferably using OpenMP if available, std::async otherwise. 179 | parallelism::StaticParallelForIndexLoop( 180 | parallelism, static_cast(range_start), 181 | static_cast(range_end), per_item_work, 182 | parallelism::ParallelForBackend::BEST_AVAILABLE); 183 | 184 | return k_nearests; 185 | } 186 | // Where real work is required, divide items into per-thread blocks, find 187 | // the K nearest in each block (in parallel), then merge those results 188 | // together. 189 | else 190 | { 191 | // Allocate per-worker-thread storage. 192 | std::vector> 193 | per_thread_nearests(num_threads); 194 | 195 | // Helper lambda for each range's work. 196 | const auto per_range_work = [&]( 197 | const parallelism::ThreadWorkRange& work_range) 198 | { 199 | per_thread_nearests[work_range.GetThreadNum()] = 200 | GetKNearestNeighborsInRangeSerial( 201 | items, static_cast(work_range.GetRangeStart()), 202 | static_cast(work_range.GetRangeEnd()), current, 203 | distance_fn, K); 204 | }; 205 | 206 | // Dispatch, preferably using OpenMP if available, std::async otherwise. 207 | parallelism::StaticParallelForRangeLoop( 208 | parallelism, static_cast(range_start), 209 | static_cast(range_end), per_range_work, 210 | parallelism::ParallelForBackend::BEST_AVAILABLE); 211 | 212 | // Merge the per-thread K-nearest together. 213 | // Uses an O((K * num_threads) * log(K)) approach. 214 | std::vector all_nearests; 215 | all_nearests.reserve(static_cast(K) * per_thread_nearests.size()); 216 | for (const auto& thread_nearests : per_thread_nearests) 217 | { 218 | all_nearests.insert( 219 | all_nearests.end(), thread_nearests.begin(), thread_nearests.end()); 220 | } 221 | 222 | std::partial_sort( 223 | all_nearests.begin(), all_nearests.begin() + K, all_nearests.end(), 224 | IndexAndDistanceCompare); 225 | 226 | // Return the first K elements by resizing down. 227 | all_nearests.resize(static_cast(K)); 228 | return all_nearests; 229 | } 230 | } 231 | } 232 | 233 | /// @return vector> of the nearest @param K neighbors to 234 | /// @param current in @param items, with distance computed by @param 235 | /// distance_fn. 236 | template> 237 | inline std::vector GetKNearestNeighborsSerial( 238 | const Container& items, const Value& current, 239 | const std::function& distance_fn, 240 | const int64_t K) 241 | { 242 | return GetKNearestNeighborsInRangeSerial( 243 | items, 0, items.size(), current, distance_fn, K); 244 | } 245 | 246 | /// @return vector> of the nearest @param K neighbors to 247 | /// @param current in @param items, with distance computed by @param distance_fn 248 | /// and search performed in parallel. 249 | template> 250 | inline std::vector GetKNearestNeighborsParallel( 251 | const Container& items, const Value& current, 252 | const std::function& distance_fn, 253 | const int64_t K, const parallelism::DegreeOfParallelism& parallelism) 254 | { 255 | return GetKNearestNeighborsInRangeParallel( 256 | items, 0, items.size(), current, distance_fn, K, parallelism); 257 | } 258 | 259 | /// @return vector> of the nearest @param K neighbors to 260 | /// @param current in the specified range [range_start, range_end) of 261 | /// @param items, with distance computed by @param distance_fn and 262 | /// @param parallelism control if/how search is performed in parallel. 263 | template> 264 | inline std::vector GetKNearestNeighborsInRange( 265 | const Container& items, const size_t range_start, const size_t range_end, 266 | const Value& current, 267 | const std::function& distance_fn, 268 | const int64_t K, const parallelism::DegreeOfParallelism& parallelism) 269 | { 270 | if (parallelism.IsParallel()) 271 | { 272 | return GetKNearestNeighborsInRangeParallel( 273 | items, range_start, range_end, current, distance_fn, K, parallelism); 274 | } 275 | else 276 | { 277 | return GetKNearestNeighborsInRangeSerial( 278 | items, range_start, range_end, current, distance_fn, K); 279 | } 280 | } 281 | 282 | /// @return vector> of the nearest @param K neighbors to 283 | /// @param current in @param items, with distance computed by @param distance_fn 284 | /// @param parallelism control if/how search is performed in parallel. 285 | template> 286 | inline std::vector GetKNearestNeighbors( 287 | const Container& items, const Value& current, 288 | const std::function& distance_fn, 289 | const int64_t K, const parallelism::DegreeOfParallelism& parallelism) 290 | { 291 | return GetKNearestNeighborsInRange( 292 | items, 0, items.size(), current, distance_fn, K, parallelism); 293 | } 294 | } // namespace simple_knearest_neighbors 295 | CRU_NAMESPACE_END 296 | } // namespace common_robotics_utilities 297 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/simple_prngs.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace common_robotics_utilities 11 | { 12 | CRU_NAMESPACE_BEGIN 13 | namespace simple_prngs 14 | { 15 | /// Implementation of the "Split-Mix 64" PRNG. 16 | class SplitMix64PRNG 17 | { 18 | private: 19 | 20 | uint64_t state_ = 0; 21 | 22 | uint64_t Next() 23 | { 24 | uint64_t z = (state_ += UINT64_C(0x9E3779B97F4A7C15)); 25 | z = (z ^ (z >> 30)) * UINT64_C(0xBF58476D1CE4E5B9); 26 | z = (z ^ (z >> 27)) * UINT64_C(0x94D049BB133111EB); 27 | return z ^ (z >> 31); 28 | } 29 | 30 | public: 31 | 32 | explicit SplitMix64PRNG(const uint64_t seed_val) 33 | { 34 | seed(seed_val); 35 | } 36 | 37 | static constexpr uint64_t min(void) 38 | { 39 | return 0u; 40 | } 41 | 42 | static constexpr uint64_t max(void) 43 | { 44 | return std::numeric_limits::max(); 45 | } 46 | 47 | void seed(const uint64_t seed_val) 48 | { 49 | state_ = seed_val; 50 | } 51 | 52 | void discard(const uint64_t z) 53 | { 54 | // This suppresses "set but not used" warnings 55 | uint64_t temp __attribute__((unused)); 56 | temp = 0u; 57 | for (uint64_t i = 0; i < z; i++) 58 | { 59 | // This should prevent the compiler from optimizing out the loop 60 | temp = Next(); 61 | __asm__ __volatile__(""); 62 | } 63 | } 64 | 65 | uint64_t operator() (void) 66 | { 67 | return Next(); 68 | } 69 | }; 70 | 71 | /// Implementation of the "XOR-Shift-128-Plus" PRNG. 72 | class XorShift128PlusPRNG 73 | { 74 | private: 75 | 76 | uint64_t state_1_ = 0; 77 | uint64_t state_2_ = 0; 78 | 79 | uint64_t Next() 80 | { 81 | uint64_t s1 = state_1_; 82 | const uint64_t s0 = state_2_; 83 | state_1_ = s0; 84 | s1 ^= s1 << 23; // a 85 | state_2_ = s1 ^ s0 ^ (s1 >> 18) ^ (s0 >> 5); // b, c 86 | return state_2_ + s0; 87 | } 88 | 89 | public: 90 | 91 | explicit XorShift128PlusPRNG(const uint64_t seed_val) 92 | { 93 | seed(seed_val); 94 | } 95 | 96 | static constexpr uint64_t min(void) 97 | { 98 | return 0u; 99 | } 100 | 101 | static constexpr uint64_t max(void) 102 | { 103 | return std::numeric_limits::max(); 104 | } 105 | 106 | void seed(const uint64_t seed_val) 107 | { 108 | SplitMix64PRNG temp_seed_gen(seed_val); 109 | state_1_ = temp_seed_gen(); 110 | state_2_ = temp_seed_gen(); 111 | } 112 | 113 | void discard(const uint64_t z) 114 | { 115 | // This suppresses "set but not used" warnings 116 | uint64_t temp __attribute__((unused)); 117 | temp = 0u; 118 | for (uint64_t i = 0; i < z; i++) 119 | { 120 | temp = Next(); 121 | // This should prevent the compiler from optimizing out the loop 122 | __asm__ __volatile__(""); 123 | } 124 | } 125 | 126 | inline uint64_t operator() (void) 127 | { 128 | return Next(); 129 | } 130 | }; 131 | 132 | /// Implementation of the "XOR-Shift-1024-Star" PRNG. 133 | class XorShift1024StarPRNG 134 | { 135 | private: 136 | 137 | std::array state_ = {{0, 0, 0, 0, 0, 0, 0, 0, 138 | 0, 0, 0, 0, 0, 0, 0, 0}}; 139 | size_t p_ = 0; 140 | 141 | uint64_t Next() 142 | { 143 | const uint64_t s0 = state_.at(p_); 144 | p_ = (p_ + 1) & 15; 145 | uint64_t s1 = state_.at(p_); 146 | s1 ^= s1 << 31; // a 147 | state_.at(p_) = s1 ^ s0 ^ (s1 >> 11) ^ (s0 >> 30); // b,c 148 | return state_.at(p_) * UINT64_C(1181783497276652981); 149 | } 150 | 151 | public: 152 | 153 | explicit XorShift1024StarPRNG(const uint64_t seed_val) 154 | { 155 | seed(seed_val); 156 | p_ = 0; 157 | } 158 | 159 | static constexpr uint64_t min(void) 160 | { 161 | return 0u; 162 | } 163 | 164 | static constexpr uint64_t max(void) 165 | { 166 | return std::numeric_limits::max(); 167 | } 168 | 169 | void seed(const uint64_t seed_val) 170 | { 171 | SplitMix64PRNG temp_seed_gen(seed_val); 172 | for (size_t idx = 0u; idx < state_.size(); idx++) 173 | { 174 | state_[idx] = temp_seed_gen(); 175 | } 176 | } 177 | 178 | void discard(const uint64_t z) 179 | { 180 | // This suppresses "set but not used" warnings 181 | uint64_t temp __attribute__((unused)); 182 | temp = 0u; 183 | for (uint64_t i = 0; i < z; i++) 184 | { 185 | temp = Next(); 186 | // This should prevent the compiler from optimizing out the loop 187 | __asm__ __volatile__(""); 188 | } 189 | } 190 | 191 | uint64_t operator() (void) 192 | { 193 | return Next(); 194 | } 195 | }; 196 | } // namespace simple_prngs 197 | CRU_NAMESPACE_END 198 | } // namespace common_robotics_utilities 199 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/simple_robot_model_interface.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace common_robotics_utilities 10 | { 11 | CRU_NAMESPACE_BEGIN 12 | namespace simple_robot_model_interface 13 | { 14 | /// This is basically the absolute minimal robot model interface needed to do 15 | /// motion planning and forward kinematics. It does not include link geometry. 16 | template> 18 | class SimpleRobotModelInterface 19 | { 20 | public: 21 | virtual ~SimpleRobotModelInterface() {} 22 | 23 | /// Clone the current robot model. 24 | virtual SimpleRobotModelInterface* Clone() const = 0; 26 | 27 | /// Return the current position (i.e. joint values). 28 | virtual const Configuration& GetPosition() const = 0; 29 | 30 | /// Set a new position (i.e. joint values) and return the new value. 31 | virtual const Configuration& SetPosition(const Configuration& config) = 0; 32 | 33 | /// Get names of links in the robot. 34 | virtual std::vector GetLinkNames() const = 0; 35 | 36 | /// Get the transform of the link with index @param link_index relative to 37 | /// world. 38 | virtual Eigen::Isometry3d GetLinkTransform( 39 | const int64_t link_index) const = 0; 40 | 41 | /// Get the transform of the link with name @param link_name relative to 42 | /// world. 43 | virtual Eigen::Isometry3d GetLinkTransform( 44 | const std::string& link_name) const = 0; 45 | 46 | /// Get the transforms of all links in the robot relative to world in the same 47 | /// order as returned by GetLinkNames(). 48 | virtual math::VectorIsometry3d GetLinkTransforms() const = 0; 49 | 50 | /// Return a map of for all links in the robot relative to 51 | /// the world. 52 | virtual math::MapStringIsometry3d GetLinkTransformsMap() const = 0; 53 | 54 | /// Return C-space distance between @param config1 and @param config2. 55 | virtual double ComputeConfigurationDistance( 56 | const Configuration& config1, const Configuration& config2) const = 0; 57 | 58 | /// Return C-space absolute-value distance between @param config1 and @param 59 | /// config2 for each dimension of the C-space separately. 60 | Eigen::VectorXd ComputePerDimensionConfigurationDistance( 61 | const Configuration& config1, const Configuration& config2) const 62 | { 63 | return ComputePerDimensionConfigurationSignedDistance( 64 | config1, config2).cwiseAbs(); 65 | } 66 | 67 | /// Return C-space signed distance between @param config1 and @param config2 68 | /// for each dimension of the C-space separately. 69 | virtual Eigen::VectorXd ComputePerDimensionConfigurationSignedDistance( 70 | const Configuration& config1, const Configuration& config2) const = 0; 71 | 72 | /// Return C-space distance between the current configuration and @param 73 | /// config. 74 | double ComputeConfigurationDistanceTo(const Configuration& config) const 75 | { 76 | return ComputeConfigurationDistance(GetPosition(), config); 77 | } 78 | 79 | /// Return C-space abolsute-value distance between the current configuration 80 | /// and @param config for each dimension of the C-space separately. 81 | Eigen::VectorXd ComputePerDimensionConfigurationDistanceTo( 82 | const Configuration& config) const 83 | { 84 | return ComputePerDimensionConfigurationDistance(GetPosition(), config); 85 | } 86 | 87 | /// Return C-space signed distance between the current configuration and 88 | /// @param config for each dimension of the C-space separately. 89 | Eigen::VectorXd ComputePerDimensionConfigurationSignedDistanceTo( 90 | const Configuration& config) const 91 | { 92 | return ComputePerDimensionConfigurationSignedDistance( 93 | GetPosition(), config); 94 | } 95 | 96 | /// Interpolate a configuration of the robot between @param start and @param 97 | /// end for the provided ratio @param ratio. 98 | virtual Configuration InterpolateBetweenConfigurations( 99 | const Configuration& start, const Configuration& end, 100 | const double ratio) const = 0; 101 | 102 | /// Average the provided set of configurations @param configurations. 103 | virtual Configuration AverageConfigurations( 104 | const std::vector& configurations) const = 0; 105 | 106 | /// Compute the translation-only part of the Jacobian at the provided point 107 | /// @param link_relative_point on link @param link_name in the robot. 108 | virtual Eigen::Matrix 109 | ComputeLinkPointTranslationJacobian( 110 | const std::string& link_name, 111 | const Eigen::Vector4d& link_relative_point) const = 0; 112 | 113 | /// Compute the Jacobian at the provided point @param link_relative_point on 114 | /// link @param link_name in the robot. 115 | virtual Eigen::Matrix 116 | ComputeLinkPointJacobian( 117 | const std::string& link_name, 118 | const Eigen::Vector4d& link_relative_point) const = 0; 119 | }; 120 | } // namespace simple_robot_model_interface 121 | CRU_NAMESPACE_END 122 | } // namespace common_robotics_utilities 123 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/time_optimal_trajectory_parametrization.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace common_robotics_utilities 11 | { 12 | CRU_NAMESPACE_BEGIN 13 | namespace time_optimal_trajectory_parametrization 14 | { 15 | /// Stores a position+velocity+time point in a trajectory. 16 | class PositionVelocityTimePoint 17 | { 18 | public: 19 | PositionVelocityTimePoint() {} 20 | 21 | PositionVelocityTimePoint( 22 | const Eigen::VectorXd& position, const Eigen::VectorXd& velocity, 23 | const double time) 24 | : position_(position), velocity_(velocity), time_(time) 25 | { 26 | if (position_.size() != velocity_.size()) 27 | { 28 | throw std::invalid_argument("position_.size() != velocity_.size()"); 29 | } 30 | if (time_ < 0.0) 31 | { 32 | throw std::invalid_argument("time must be >= 0"); 33 | } 34 | } 35 | 36 | const Eigen::VectorXd& Position() const { return position_; } 37 | 38 | const Eigen::VectorXd& Velocity() const { return velocity_; } 39 | 40 | double Time() const { return time_; } 41 | 42 | private: 43 | Eigen::VectorXd position_; 44 | Eigen::VectorXd velocity_; 45 | double time_ = 0.0; 46 | }; 47 | 48 | /// Basic interface to a position+velocity trajectory. 49 | class PositionVelocityTrajectoryInterface 50 | { 51 | public: 52 | virtual ~PositionVelocityTrajectoryInterface() {} 53 | 54 | virtual std::unique_ptr 55 | Clone() const = 0; 56 | 57 | virtual double Duration() const = 0; 58 | 59 | virtual PositionVelocityTimePoint GetPositionVelocityTimePoint( 60 | const double time) const = 0; 61 | }; 62 | 63 | /// Parametrize the provided @param path with the provided velocity and 64 | /// acceleration limits into a trajectory using Time-Optimal Trajectory 65 | /// Parametrization. 66 | std::unique_ptr ParametrizePathTOTP( 67 | const std::vector& path, 68 | const Eigen::VectorXd& max_velocity, 69 | const Eigen::VectorXd& max_acceleration, 70 | const double max_deviation = 0.0, const double timestep = 0.001); 71 | } // namespace time_optimal_trajectory_parametrization 72 | CRU_NAMESPACE_END 73 | } // namespace common_robotics_utilities 74 | -------------------------------------------------------------------------------- /include/common_robotics_utilities/zlib_helpers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace common_robotics_utilities 10 | { 11 | CRU_NAMESPACE_BEGIN 12 | namespace zlib_helpers 13 | { 14 | std::vector DecompressBytes(const std::vector& compressed); 15 | 16 | std::vector CompressBytes(const std::vector& uncompressed); 17 | 18 | std::vector LoadFromFileAndDecompress(const std::string& filename); 19 | 20 | void CompressAndWriteToFile( 21 | const std::vector& uncompressed, const std::string& filename); 22 | } // namespace zlib_helpers 23 | CRU_NAMESPACE_END 24 | } // namespace common_robotics_utilities 25 | -------------------------------------------------------------------------------- /package.xml.ros1: -------------------------------------------------------------------------------- 1 | 2 | 3 | common_robotics_utilities 4 | 0.0.1 5 | C++ utilities used in lab and TRI projects 6 | Calder Phillips-Grafflin 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | geometry_msgs 13 | visualization_msgs 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /package.xml.ros2: -------------------------------------------------------------------------------- 1 | 2 | 3 | common_robotics_utilities 4 | 0.1.0 5 | C++ utilities used in lab and TRI projects 6 | Calder Phillips-Grafflin 7 | BSD 8 | 9 | ament_cmake_ros 10 | 11 | rclcpp 12 | geometry_msgs 13 | visualization_msgs 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/common_robotics_utilities/base64_helpers.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace common_robotics_utilities 9 | { 10 | CRU_NAMESPACE_BEGIN 11 | namespace base64_helpers 12 | { 13 | /// Implementations derived from post at http://stackoverflow.com/a/41094722 14 | static const std::array B64IndexTable = 15 | {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 62, 63, 62, 62, 63, 17 | 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 18 | 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 19 | 25, 0, 0, 0, 0, 63, 0, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 20 | 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51}; 21 | 22 | static const std::array B64ValueTable = 23 | {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 24 | 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z', 'a', 'b', 'c', 'd', 25 | 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 26 | 't', 'u', 'v', 'w', 'x', 'y', 'z', '0', '1', '2', '3', '4', '5', '6', '7', 27 | '8', '9', '+', '/'}; 28 | 29 | std::vector Decode(const std::string& encoded) 30 | { 31 | const size_t encoded_length = encoded.size(); 32 | const size_t pad = encoded_length > 0 && 33 | (encoded_length % 4 || encoded[encoded_length - 1] == '='); 34 | const size_t L = ((encoded_length + 3) / 4 - pad) * 4; 35 | std::vector buffer(L / 4 * 3 + pad, 0x00); 36 | for (size_t i = 0, j = 0; i < L; i += 4) 37 | { 38 | const int32_t n = 39 | B64IndexTable[static_cast(encoded[i])] << 18 40 | | B64IndexTable[static_cast(encoded[i + 1])] << 12 41 | | B64IndexTable[static_cast(encoded[i + 2])] << 6 42 | | B64IndexTable[static_cast(encoded[i + 3])]; 43 | buffer[j++] = static_cast(n >> 16); 44 | buffer[j++] = static_cast(n >> 8 & 0xFF); 45 | buffer[j++] = static_cast(n & 0xFF); 46 | } 47 | if (pad > 0) 48 | { 49 | int32_t n = 50 | B64IndexTable[static_cast(encoded[L])] << 18 51 | | B64IndexTable[static_cast(encoded[L + 1])] << 12; 52 | buffer[buffer.size() - 1] = static_cast(n >> 16); 53 | if (encoded_length > L + 2 && encoded[L + 2] != '=') 54 | { 55 | n |= B64IndexTable[static_cast(encoded[L + 2])] << 6; 56 | buffer.push_back(static_cast(n >> 8 & 0xFF)); 57 | } 58 | } 59 | return buffer; 60 | } 61 | 62 | std::string Encode(const std::vector& binary) 63 | { 64 | const size_t binary_length = binary.size(); 65 | /* 3-byte blocks to 4-byte */ 66 | const size_t encoded_length = 4 * ((binary_length + 2) / 3); 67 | if (encoded_length < binary_length) 68 | { 69 | return std::string(); /* integer overflow */ 70 | } 71 | std::string encoded; 72 | encoded.resize(encoded_length); 73 | size_t input_position = 0; 74 | size_t output_position = 0; 75 | while (binary_length - input_position >= 3) 76 | { 77 | encoded[output_position++] = B64ValueTable[ 78 | static_cast(binary[input_position + 0] >> 2)]; 79 | encoded[output_position++] = B64ValueTable[ 80 | static_cast( 81 | ((binary[input_position + 0] & 0x03) << 4) 82 | | (binary[input_position + 1] >> 4))]; 83 | encoded[output_position++] = B64ValueTable[ 84 | static_cast( 85 | ((binary[input_position + 1] & 0x0f) << 2) 86 | | (binary[input_position + 2] >> 6))]; 87 | encoded[output_position++] = B64ValueTable[ 88 | static_cast(binary[input_position + 2] & 0x3f)]; 89 | input_position += 3; 90 | } 91 | if (input_position < binary_length) 92 | { 93 | encoded[output_position++] = B64ValueTable[ 94 | static_cast(binary[input_position + 0] >> 2)]; 95 | if ((binary_length - input_position) == 1) 96 | { 97 | encoded[output_position++] = B64ValueTable[ 98 | static_cast((binary[input_position + 0] & 0x03) << 4)]; 99 | encoded[output_position++] = '='; 100 | } 101 | else 102 | { 103 | encoded[output_position++] = B64ValueTable[ 104 | static_cast( 105 | ((binary[input_position + 0] & 0x03) << 4) 106 | | (binary[input_position + 1] >> 4))]; 107 | encoded[output_position++] = B64ValueTable[ 108 | static_cast((binary[input_position + 1] & 0x0f) << 2)]; 109 | } 110 | encoded[output_position++] = '='; 111 | } 112 | return encoded; 113 | } 114 | } // namespace base64_helpers 115 | CRU_NAMESPACE_END 116 | } // namespace common_robotics_utilities 117 | -------------------------------------------------------------------------------- /src/common_robotics_utilities/conversions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace common_robotics_utilities 8 | { 9 | CRU_NAMESPACE_BEGIN 10 | namespace conversions 11 | { 12 | Eigen::Quaterniond QuaternionFromRPY(const double R, 13 | const double P, 14 | const double Y) 15 | { 16 | const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX()); 17 | const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY()); 18 | const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ()); 19 | const Eigen::Quaterniond quat(roll * pitch * yaw); 20 | return quat; 21 | } 22 | 23 | // URDF RPY IS ACTUALLY APPLIED Y*P*R 24 | Eigen::Quaterniond QuaternionFromUrdfRPY(const double R, 25 | const double P, 26 | const double Y) 27 | { 28 | const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX()); 29 | const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY()); 30 | const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ()); 31 | const Eigen::Quaterniond quat(yaw * pitch * roll); 32 | return quat; 33 | } 34 | 35 | // Returns XYZ Euler angles 36 | Eigen::Vector3d EulerAnglesFromRotationMatrix( 37 | const Eigen::Matrix3d& rot_matrix) 38 | { 39 | // Use XYZ angles 40 | const Eigen::Vector3d euler_angles = rot_matrix.eulerAngles(0, 1, 2); 41 | return euler_angles; 42 | } 43 | 44 | // Returns XYZ Euler angles 45 | Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat) 46 | { 47 | return EulerAnglesFromRotationMatrix(quat.toRotationMatrix()); 48 | } 49 | 50 | // Returns XYZ Euler angles 51 | Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans) 52 | { 53 | return EulerAnglesFromRotationMatrix(trans.rotation()); 54 | } 55 | 56 | Eigen::Isometry3d TransformFromXYZRPY(const double x, 57 | const double y, 58 | const double z, 59 | const double roll, 60 | const double pitch, 61 | const double yaw) 62 | { 63 | const Eigen::Isometry3d transform 64 | = Eigen::Translation3d(x, y, z) * QuaternionFromRPY(roll, pitch, yaw); 65 | return transform; 66 | } 67 | 68 | Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation, 69 | const Eigen::Vector3d& rotation) 70 | { 71 | const Eigen::Isometry3d transform 72 | = static_cast(translation) 73 | * QuaternionFromRPY(rotation.x(), rotation.y(), rotation.z()); 74 | return transform; 75 | } 76 | 77 | Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components) 78 | { 79 | if (components.size() == 6) 80 | { 81 | return Eigen::Translation3d(components(0), 82 | components(1), 83 | components(2)) 84 | * QuaternionFromRPY(components(3), 85 | components(4), 86 | components(5)); 87 | } 88 | else 89 | { 90 | throw std::invalid_argument( 91 | "VectorXd source vector is not 6 elements in size"); 92 | } 93 | } 94 | 95 | // URDF RPY IS ACTUALLY APPLIED Y*P*R 96 | Eigen::Isometry3d TransformFromUrdfXYZRPY(const double x, 97 | const double y, 98 | const double z, 99 | const double roll, 100 | const double pitch, 101 | const double yaw) 102 | { 103 | const Eigen::Isometry3d transform = Eigen::Translation3d(x, y, z) 104 | * QuaternionFromUrdfRPY(roll, pitch, yaw); 105 | return transform; 106 | } 107 | 108 | // URDF RPY IS ACTUALLY APPLIED Y*P*R 109 | Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::Vector3d& translation, 110 | const Eigen::Vector3d& rotation) 111 | { 112 | const Eigen::Isometry3d transform 113 | = static_cast(translation) 114 | * QuaternionFromUrdfRPY(rotation.x(), rotation.y(), rotation.z()); 115 | return transform; 116 | } 117 | 118 | // URDF RPY IS ACTUALLY APPLIED Y*P*R 119 | Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::VectorXd& components) 120 | { 121 | if (components.size() == 6) 122 | { 123 | return Eigen::Translation3d(components(0), 124 | components(1), 125 | components(2)) 126 | * QuaternionFromUrdfRPY(components(3), 127 | components(4), 128 | components(5)); 129 | } 130 | else 131 | { 132 | throw std::invalid_argument( 133 | "VectorXd source vector is not 6 elements in size"); 134 | } 135 | } 136 | 137 | Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform) 138 | { 139 | Eigen::VectorXd components = Eigen::VectorXd::Zero(6); 140 | const Eigen::Vector3d translation = transform.translation(); 141 | const Eigen::Vector3d rotation 142 | = EulerAnglesFromRotationMatrix(transform.rotation()); 143 | components << translation, rotation; 144 | return components; 145 | } 146 | 147 | Eigen::Vector3d StdVectorDoubleToEigenVector3d( 148 | const std::vector& vector) 149 | { 150 | if (vector.size() == 3) 151 | { 152 | return Eigen::Vector3d(vector.at(0), vector.at(1), vector.at(2)); 153 | } 154 | else 155 | { 156 | throw std::invalid_argument( 157 | "Vector3d source vector is not 3 elements in size"); 158 | } 159 | } 160 | 161 | Eigen::VectorXd StdVectorDoubleToEigenVectorXd( 162 | const std::vector& vector) 163 | { 164 | Eigen::VectorXd eigen_vector(vector.size()); 165 | for (size_t idx = 0; idx < vector.size(); idx++) 166 | { 167 | const double val = vector.at(idx); 168 | eigen_vector(static_cast(idx)) = val; 169 | } 170 | return eigen_vector; 171 | } 172 | 173 | std::vector EigenVector3dToStdVectorDouble( 174 | const Eigen::Vector3d& point) 175 | { 176 | return std::vector{point.x(), point.y(), point.z()}; 177 | } 178 | 179 | std::vector EigenVectorXdToStdVectorDouble( 180 | const Eigen::VectorXd& eigen_vector) 181 | { 182 | std::vector vector(static_cast(eigen_vector.size())); 183 | for (size_t idx = 0; idx < vector.size(); idx++) 184 | { 185 | const double val = eigen_vector(static_cast(idx)); 186 | vector.at(idx) = val; 187 | } 188 | return vector; 189 | } 190 | 191 | // Takes as is the ROS custom! 192 | Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond( 193 | const std::vector& vector) 194 | { 195 | if (vector.size() == 4) 196 | { 197 | return Eigen::Quaterniond( 198 | vector.at(3), vector.at(0), vector.at(1), vector.at(2)); 199 | } 200 | else 201 | { 202 | throw std::invalid_argument( 203 | "Quaterniond source vector is not 4 elements in size"); 204 | } 205 | } 206 | 207 | // Returns as is the ROS custom! 208 | std::vector EigenQuaterniondToStdVectorDouble( 209 | const Eigen::Quaterniond& quat) 210 | { 211 | return std::vector{quat.x(), quat.y(), quat.z(), quat.w()}; 212 | } 213 | } // namespace conversions 214 | CRU_NAMESPACE_END 215 | } // namespace common_robotics_utilities 216 | -------------------------------------------------------------------------------- /src/common_robotics_utilities/ros_conversions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace common_robotics_utilities 9 | { 10 | CRU_NAMESPACE_BEGIN 11 | namespace ros_conversions 12 | { 13 | Eigen::Vector3d GeometryPointToEigenVector3d( 14 | const GeometryPoint& point) 15 | { 16 | Eigen::Vector3d eigen_point(point.x, point.y, point.z); 17 | return eigen_point; 18 | } 19 | 20 | GeometryPoint EigenVector3dToGeometryPoint( 21 | const Eigen::Vector3d& point) 22 | { 23 | GeometryPoint geom_point; 24 | geom_point.x = point.x(); 25 | geom_point.y = point.y(); 26 | geom_point.z = point.z(); 27 | return geom_point; 28 | } 29 | 30 | Eigen::Vector4d GeometryPointToEigenVector4d( 31 | const GeometryPoint& point) 32 | { 33 | Eigen::Vector4d eigen_point(point.x, point.y, point.z, 1.0); 34 | return eigen_point; 35 | } 36 | 37 | GeometryPoint EigenVector4dToGeometryPoint( 38 | const Eigen::Vector4d& point) 39 | { 40 | GeometryPoint geom_point; 41 | geom_point.x = point(0); 42 | geom_point.y = point(1); 43 | geom_point.z = point(2); 44 | return geom_point; 45 | } 46 | 47 | GeometryPointStamped EigenVector3dToGeometryPointStamped( 48 | const Eigen::Vector3d& point, const std::string& frame_id) 49 | { 50 | GeometryPointStamped point_stamped; 51 | point_stamped.header.frame_id = frame_id; 52 | point_stamped.point = EigenVector3dToGeometryPoint(point); 53 | return point_stamped; 54 | } 55 | 56 | Eigen::Vector3d GeometryVector3ToEigenVector3d( 57 | const GeometryVector3& vector) 58 | { 59 | Eigen::Vector3d eigen_vector(vector.x, vector.y, vector.z); 60 | return eigen_vector; 61 | } 62 | 63 | GeometryVector3 EigenVector3dToGeometryVector3( 64 | const Eigen::Vector3d& vector) 65 | { 66 | GeometryVector3 geom_vector; 67 | geom_vector.x = vector.x(); 68 | geom_vector.y = vector.y(); 69 | geom_vector.z = vector.z(); 70 | return geom_vector; 71 | } 72 | 73 | Eigen::Vector4d GeometryVector3ToEigenVector4d( 74 | const GeometryVector3& vector) 75 | { 76 | Eigen::Vector4d eigen_vector(vector.x, vector.y, vector.z, 0.0); 77 | return eigen_vector; 78 | } 79 | 80 | GeometryVector3 EigenVector4dToGeometryVector3( 81 | const Eigen::Vector4d& vector) 82 | { 83 | GeometryVector3 geom_vector; 84 | geom_vector.x = vector(0); 85 | geom_vector.y = vector(1); 86 | geom_vector.z = vector(2); 87 | return geom_vector; 88 | } 89 | 90 | Eigen::Quaterniond GeometryQuaternionToEigenQuaterniond( 91 | const GeometryQuaternion& quat) 92 | { 93 | Eigen::Quaterniond eigen_quaternion(quat.w, quat.x, quat.y, quat.z); 94 | return eigen_quaternion; 95 | } 96 | 97 | GeometryQuaternion EigenQuaterniondToGeometryQuaternion( 98 | const Eigen::Quaterniond& quat) 99 | { 100 | GeometryQuaternion geom_quaternion; 101 | geom_quaternion.w = quat.w(); 102 | geom_quaternion.x = quat.x(); 103 | geom_quaternion.y = quat.y(); 104 | geom_quaternion.z = quat.z(); 105 | return geom_quaternion; 106 | } 107 | 108 | Eigen::Isometry3d GeometryPoseToEigenIsometry3d( 109 | const GeometryPose& pose) 110 | { 111 | const Eigen::Translation3d trans(pose.position.x, 112 | pose.position.y, 113 | pose.position.z); 114 | const Eigen::Quaterniond quat(pose.orientation.w, 115 | pose.orientation.x, 116 | pose.orientation.y, 117 | pose.orientation.z); 118 | const Eigen::Isometry3d eigen_pose = trans * quat; 119 | return eigen_pose; 120 | } 121 | 122 | GeometryPose EigenIsometry3dToGeometryPose( 123 | const Eigen::Isometry3d& transform) 124 | { 125 | const Eigen::Vector3d trans = transform.translation(); 126 | const Eigen::Quaterniond quat(transform.rotation()); 127 | GeometryPose geom_pose; 128 | geom_pose.position.x = trans.x(); 129 | geom_pose.position.y = trans.y(); 130 | geom_pose.position.z = trans.z(); 131 | geom_pose.orientation.w = quat.w(); 132 | geom_pose.orientation.x = quat.x(); 133 | geom_pose.orientation.y = quat.y(); 134 | geom_pose.orientation.z = quat.z(); 135 | return geom_pose; 136 | } 137 | 138 | GeometryPoseStamped EigenIsometry3dToGeometryPoseStamped( 139 | const Eigen::Isometry3d& transform, const std::string& frame_id) 140 | { 141 | GeometryPoseStamped pose_stamped; 142 | pose_stamped.header.frame_id = frame_id; 143 | pose_stamped.pose = EigenIsometry3dToGeometryPose(transform); 144 | return pose_stamped; 145 | } 146 | 147 | Eigen::Isometry3d GeometryTransformToEigenIsometry3d( 148 | const GeometryTransform& transform) 149 | { 150 | const Eigen::Translation3d trans(transform.translation.x, 151 | transform.translation.y, 152 | transform.translation.z); 153 | const Eigen::Quaterniond quat(transform.rotation.w, 154 | transform.rotation.x, 155 | transform.rotation.y, 156 | transform.rotation.z); 157 | const Eigen::Isometry3d eigen_transform = trans * quat; 158 | return eigen_transform; 159 | } 160 | 161 | GeometryTransform EigenIsometry3dToGeometryTransform( 162 | const Eigen::Isometry3d& transform) 163 | { 164 | const Eigen::Vector3d trans = transform.translation(); 165 | const Eigen::Quaterniond quat(transform.rotation()); 166 | GeometryTransform geom_transform; 167 | geom_transform.translation.x = trans.x(); 168 | geom_transform.translation.y = trans.y(); 169 | geom_transform.translation.z = trans.z(); 170 | geom_transform.rotation.w = quat.w(); 171 | geom_transform.rotation.x = quat.x(); 172 | geom_transform.rotation.y = quat.y(); 173 | geom_transform.rotation.z = quat.z(); 174 | return geom_transform; 175 | } 176 | 177 | GeometryTransformStamped EigenIsometry3dToGeometryTransformStamped( 178 | const Eigen::Isometry3d& transform, const std::string& frame_id, 179 | const std::string& child_frame_id) 180 | { 181 | GeometryTransformStamped transform_stamped; 182 | transform_stamped.header.frame_id = frame_id; 183 | transform_stamped.child_frame_id = child_frame_id; 184 | transform_stamped.transform = EigenIsometry3dToGeometryTransform(transform); 185 | return transform_stamped; 186 | } 187 | 188 | Eigen::Matrix3Xd VectorGeometryPointToEigenMatrix3Xd( 189 | const std::vector& vector_geom) 190 | { 191 | Eigen::Matrix3Xd eigen_matrix = Eigen::MatrixXd(3, vector_geom.size()); 192 | for (size_t idx = 0; idx < vector_geom.size(); idx++) 193 | { 194 | eigen_matrix.block<3,1>(0, static_cast(idx)) 195 | = GeometryPointToEigenVector3d(vector_geom.at(idx)); 196 | } 197 | return eigen_matrix; 198 | } 199 | 200 | std::vector EigenMatrix3XdToVectorGeometryPoint( 201 | const Eigen::Matrix3Xd& eigen_matrix) 202 | { 203 | std::vector vector_geom( 204 | static_cast(eigen_matrix.cols())); 205 | for (size_t idx = 0; idx < vector_geom.size(); idx++) 206 | { 207 | vector_geom.at(idx) 208 | = EigenVector3dToGeometryPoint( 209 | eigen_matrix.block<3,1>(0, static_cast(idx))); 210 | } 211 | return vector_geom; 212 | } 213 | 214 | std::vector 215 | VectorEigenVector3dToVectorGeometryPoint( 216 | const common_robotics_utilities::math::VectorVector3d& vector_eigen) 217 | { 218 | std::vector vector_geom(vector_eigen.size()); 219 | for (size_t idx = 0; idx < vector_eigen.size(); idx++) 220 | { 221 | vector_geom.at(idx) = EigenVector3dToGeometryPoint(vector_eigen.at(idx)); 222 | } 223 | return vector_geom; 224 | } 225 | 226 | common_robotics_utilities::math::VectorVector3d 227 | VectorGeometryPointToVectorEigenVector3d( 228 | const std::vector& vector_geom) 229 | { 230 | common_robotics_utilities::math::VectorVector3d vector_eigen( 231 | vector_geom.size()); 232 | for (size_t idx = 0; idx < vector_geom.size(); idx++) 233 | { 234 | vector_eigen.at(idx) = GeometryPointToEigenVector3d(vector_geom.at(idx)); 235 | } 236 | return vector_eigen; 237 | } 238 | 239 | common_robotics_utilities::math::VectorVector3d 240 | VectorGeometryVector3ToEigenVector3d( 241 | const std::vector& vector_geom) 242 | { 243 | common_robotics_utilities::math::VectorVector3d vector_eigen( 244 | vector_geom.size()); 245 | for (size_t idx = 0; idx < vector_geom.size(); idx++) 246 | { 247 | vector_eigen.at(idx) = GeometryVector3ToEigenVector3d(vector_geom.at(idx)); 248 | } 249 | return vector_eigen; 250 | } 251 | 252 | common_robotics_utilities::math::VectorIsometry3d 253 | VectorGeometryPoseToVectorIsometry3d( 254 | const std::vector& vector_geom) 255 | { 256 | common_robotics_utilities::math::VectorIsometry3d vector_eigen( 257 | vector_geom.size()); 258 | for (size_t idx = 0; idx < vector_geom.size(); idx++) 259 | { 260 | vector_eigen.at(idx) = GeometryPoseToEigenIsometry3d(vector_geom.at(idx)); 261 | } 262 | return vector_eigen; 263 | } 264 | 265 | common_robotics_utilities::math::VectorIsometry3d 266 | VectorGeometryPoseToVectorIsometry3d( 267 | const std::vector& vector_geom) 268 | { 269 | common_robotics_utilities::math::VectorIsometry3d vector_eigen( 270 | vector_geom.size()); 271 | for (size_t idx = 0; idx < vector_geom.size(); idx++) 272 | { 273 | vector_eigen.at(idx) = 274 | GeometryTransformToEigenIsometry3d(vector_geom.at(idx)); 275 | } 276 | return vector_eigen; 277 | } 278 | 279 | std::vector VectorIsometry3dToVectorGeometryPose( 280 | const common_robotics_utilities::math::VectorIsometry3d& vector_eigen) 281 | { 282 | std::vector vector_geom(vector_eigen.size()); 283 | for (size_t idx = 0; idx < vector_eigen.size(); idx++) 284 | { 285 | vector_geom.at(idx) = EigenIsometry3dToGeometryPose(vector_eigen.at(idx)); 286 | } 287 | return vector_geom; 288 | } 289 | 290 | std::vector VectorIsometry3dToVectorGeometryTransform( 291 | const common_robotics_utilities::math::VectorIsometry3d& vector_eigen) 292 | { 293 | std::vector vector_geom(vector_eigen.size()); 294 | for (size_t idx = 0; idx < vector_eigen.size(); idx++) 295 | { 296 | vector_geom.at(idx) = 297 | EigenIsometry3dToGeometryTransform(vector_eigen.at(idx)); 298 | } 299 | return vector_geom; 300 | } 301 | } // namespace ros_conversions 302 | CRU_NAMESPACE_END 303 | } // namespace common_robotics_utilities 304 | -------------------------------------------------------------------------------- /src/common_robotics_utilities/serialization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace common_robotics_utilities 10 | { 11 | CRU_NAMESPACE_BEGIN 12 | namespace serialization 13 | { 14 | constexpr uint64_t SerializedSizeVector2d() 15 | { 16 | return static_cast(sizeof(double) * 2); 17 | } 18 | 19 | constexpr uint64_t SerializedSizeVector3d() 20 | { 21 | return static_cast(sizeof(double) * 3); 22 | } 23 | 24 | constexpr uint64_t SerializedSizeVector4d() 25 | { 26 | return static_cast(sizeof(double) * 4); 27 | } 28 | 29 | constexpr uint64_t SerializedSizeQuaterniond() 30 | { 31 | return static_cast(sizeof(double) * 4); 32 | } 33 | 34 | constexpr uint64_t SerializedSizeIsometry3d() 35 | { 36 | return static_cast(sizeof(double) * 16); 37 | } 38 | 39 | uint64_t SerializeMatrixXd( 40 | const Eigen::MatrixXd& value, std::vector& buffer) 41 | { 42 | const uint64_t start_buffer_size = buffer.size(); 43 | // First, write uint64_t rows and cols headers 44 | const uint64_t rows = static_cast(value.rows()); 45 | SerializeMemcpyable(rows, buffer); 46 | const uint64_t cols = static_cast(value.cols()); 47 | SerializeMemcpyable(cols, buffer); 48 | // Expand the buffer to handle everything 49 | const size_t serialized_length = sizeof(double) * rows * cols; 50 | const size_t previous_buffer_size = buffer.size(); 51 | buffer.resize(previous_buffer_size + serialized_length, 0x00); 52 | // Serialize the contained items 53 | memcpy(&buffer[previous_buffer_size], value.data(), serialized_length); 54 | // Figure out how many bytes were written 55 | const uint64_t end_buffer_size = buffer.size(); 56 | const uint64_t bytes_written = end_buffer_size - start_buffer_size; 57 | return bytes_written; 58 | } 59 | 60 | uint64_t SerializeVectorXd( 61 | const Eigen::VectorXd& value, std::vector& buffer) 62 | { 63 | return SerializeMemcpyableVectorLike(value, buffer); 64 | } 65 | 66 | uint64_t SerializeVector2d( 67 | const Eigen::Vector2d& value, std::vector& buffer) 68 | { 69 | // Takes a state to serialize and a buffer to serialize into 70 | // Return number of bytes written to buffer 71 | std::vector temp_buffer(SerializedSizeVector2d(), 0x00); 72 | memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector2d()); 73 | buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end()); 74 | return SerializedSizeVector2d(); 75 | } 76 | 77 | uint64_t SerializeVector3d( 78 | const Eigen::Vector3d& value, std::vector& buffer) 79 | { 80 | // Takes a state to serialize and a buffer to serialize into 81 | // Return number of bytes written to buffer 82 | std::vector temp_buffer(SerializedSizeVector3d(), 0x00); 83 | memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector3d()); 84 | buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end()); 85 | return SerializedSizeVector3d(); 86 | } 87 | 88 | uint64_t SerializeVector4d( 89 | const Eigen::Vector4d& value, std::vector& buffer) 90 | { 91 | // Takes a state to serialize and a buffer to serialize into 92 | // Return number of bytes written to buffer 93 | std::vector temp_buffer(SerializedSizeVector4d(), 0x00); 94 | memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector4d()); 95 | buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end()); 96 | return SerializedSizeVector4d(); 97 | } 98 | 99 | uint64_t SerializeQuaterniond( 100 | const Eigen::Quaterniond& value, std::vector& buffer) 101 | { 102 | // Takes a state to serialize and a buffer to serialize into 103 | // Return number of bytes written to buffer 104 | std::vector temp_buffer(SerializedSizeQuaterniond(), 0x00); 105 | memcpy(&temp_buffer.front(), 106 | value.coeffs().data(), 107 | SerializedSizeQuaterniond()); 108 | buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end()); 109 | return SerializedSizeQuaterniond(); 110 | } 111 | 112 | uint64_t SerializeIsometry3d( 113 | const Eigen::Isometry3d& value, std::vector& buffer) 114 | { 115 | // Takes a state to serialize and a buffer to serialize into 116 | // Return number of bytes written to buffer 117 | std::vector temp_buffer(SerializedSizeIsometry3d(), 0x00); 118 | memcpy(&temp_buffer.front(), 119 | value.matrix().data(), 120 | SerializedSizeIsometry3d()); 121 | buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end()); 122 | return SerializedSizeIsometry3d(); 123 | } 124 | 125 | Deserialized DeserializeMatrixXd( 126 | const std::vector& buffer, const uint64_t starting_offset) 127 | { 128 | uint64_t current_position = starting_offset; 129 | // Load the rows and cols header 130 | const Deserialized deserialized_rows 131 | = DeserializeMemcpyable(buffer, current_position); 132 | const uint64_t rows = deserialized_rows.Value(); 133 | current_position += deserialized_rows.BytesRead(); 134 | const Deserialized deserialized_cols 135 | = DeserializeMemcpyable(buffer, current_position); 136 | const uint64_t cols = deserialized_cols.Value(); 137 | current_position += deserialized_cols.BytesRead(); 138 | // Deserialize the items 139 | const size_t serialized_length = sizeof(double) * rows * cols; 140 | if ((current_position + serialized_length) > buffer.size()) 141 | { 142 | throw std::invalid_argument("Not enough room in the provided buffer"); 143 | } 144 | Eigen::MatrixXd deserialized = Eigen::MatrixXd::Zero( 145 | static_cast(rows), static_cast(cols)); 146 | memcpy(deserialized.data(), &buffer[current_position], serialized_length); 147 | current_position += serialized_length; 148 | // Figure out how many bytes were read 149 | const uint64_t bytes_read = current_position - starting_offset; 150 | return MakeDeserialized(deserialized, bytes_read); 151 | } 152 | 153 | Deserialized DeserializeVectorXd( 154 | const std::vector& buffer, const uint64_t starting_offset) 155 | { 156 | return DeserializeMemcpyableVectorLike( 157 | buffer, starting_offset); 158 | } 159 | 160 | Deserialized DeserializeVector2d( 161 | const std::vector& buffer, const uint64_t starting_offset) 162 | { 163 | if (starting_offset >= buffer.size()) 164 | { 165 | throw std::invalid_argument( 166 | "starting_offset is outside the provided buffer"); 167 | } 168 | if ((starting_offset + SerializedSizeVector2d()) > buffer.size()) 169 | { 170 | throw std::invalid_argument("Not enough room in the provided buffer"); 171 | } 172 | // Takes a buffer to read from and the starting index in the buffer 173 | // Return the loaded state and how many bytes we read from the buffer 174 | Eigen::Vector2d temp_value; 175 | memcpy(temp_value.data(), &buffer[starting_offset], SerializedSizeVector2d()); 176 | return MakeDeserialized(temp_value, SerializedSizeVector2d()); 177 | } 178 | 179 | Deserialized DeserializeVector3d( 180 | const std::vector& buffer, const uint64_t starting_offset) 181 | { 182 | if (starting_offset >= buffer.size()) 183 | { 184 | throw std::invalid_argument( 185 | "starting_offset is outside the provided buffer"); 186 | } 187 | if ((starting_offset + SerializedSizeVector3d()) > buffer.size()) 188 | { 189 | throw std::invalid_argument("Not enough room in the provided buffer"); 190 | } 191 | // Takes a buffer to read from and the starting index in the buffer 192 | // Return the loaded state and how many bytes we read from the buffer 193 | Eigen::Vector3d temp_value; 194 | memcpy(temp_value.data(), &buffer[starting_offset], SerializedSizeVector3d()); 195 | return MakeDeserialized(temp_value, SerializedSizeVector3d()); 196 | } 197 | 198 | Deserialized DeserializeVector4d( 199 | const std::vector& buffer, const uint64_t starting_offset) 200 | { 201 | if (starting_offset >= buffer.size()) 202 | { 203 | throw std::invalid_argument( 204 | "starting_offset is outside the provided buffer"); 205 | } 206 | if ((starting_offset + SerializedSizeVector4d()) > buffer.size()) 207 | { 208 | throw std::invalid_argument("Not enough room in the provided buffer"); 209 | } 210 | // Takes a buffer to read from and the starting index in the buffer 211 | // Return the loaded state and how many bytes we read from the buffer 212 | Eigen::Vector4d temp_value; 213 | memcpy(temp_value.data(), &buffer[starting_offset], SerializedSizeVector4d()); 214 | return MakeDeserialized(temp_value, SerializedSizeVector4d()); 215 | } 216 | 217 | Deserialized DeserializeQuaterniond( 218 | const std::vector& buffer, const uint64_t starting_offset) 219 | { 220 | if (starting_offset >= buffer.size()) 221 | { 222 | throw std::invalid_argument( 223 | "starting_offset is outside the provided buffer"); 224 | } 225 | if ((starting_offset + SerializedSizeQuaterniond()) > buffer.size()) 226 | { 227 | throw std::invalid_argument("Not enough room in the provided buffer"); 228 | } 229 | // Takes a buffer to read from and the starting index in the buffer 230 | // Return the loaded state and how many bytes we read from the buffer 231 | Eigen::Quaterniond temp_value; 232 | memcpy(temp_value.coeffs().data(), 233 | &buffer[starting_offset], 234 | SerializedSizeQuaterniond()); 235 | return MakeDeserialized(temp_value, SerializedSizeQuaterniond()); 236 | } 237 | 238 | Deserialized DeserializeIsometry3d( 239 | const std::vector& buffer, const uint64_t starting_offset) 240 | { 241 | if (starting_offset >= buffer.size()) 242 | { 243 | throw std::invalid_argument( 244 | "starting_offset is outside the provided buffer"); 245 | } 246 | if ((starting_offset + SerializedSizeIsometry3d()) > buffer.size()) 247 | { 248 | throw std::invalid_argument("Not enough room in the provided buffer"); 249 | } 250 | // Takes a buffer to read from and the starting index in the buffer 251 | // Return the loaded state and how many bytes we read from the buffer 252 | Eigen::Isometry3d temp_value; 253 | memcpy(temp_value.matrix().data(), 254 | &buffer[starting_offset], 255 | SerializedSizeIsometry3d()); 256 | return MakeDeserialized(temp_value, SerializedSizeIsometry3d()); 257 | } 258 | } // namespace serialization 259 | CRU_NAMESPACE_END 260 | } // namespace common_robotics_utilities 261 | -------------------------------------------------------------------------------- /src/common_robotics_utilities/zlib_helpers.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace common_robotics_utilities 13 | { 14 | CRU_NAMESPACE_BEGIN 15 | namespace zlib_helpers 16 | { 17 | std::vector DecompressBytes(const std::vector& compressed) 18 | { 19 | if (compressed.size() <= std::numeric_limits::max()) 20 | { 21 | z_stream strm; 22 | std::vector buffer; 23 | const size_t BUFSIZE = 1024 * 1024; 24 | uint8_t temp_buffer[BUFSIZE]; 25 | strm.zalloc = Z_NULL; 26 | strm.zfree = Z_NULL; 27 | strm.opaque = Z_NULL; 28 | int ret = inflateInit(&strm); 29 | if (ret != Z_OK) 30 | { 31 | inflateEnd(&strm); 32 | throw std::runtime_error( 33 | "ZLIB can't init inflate stream (ret = " + std::to_string(ret) + ")"); 34 | } 35 | strm.avail_in = static_cast(compressed.size()); 36 | strm.next_in = const_cast(reinterpret_cast( 37 | compressed.data())); 38 | do 39 | { 40 | strm.next_out = temp_buffer; 41 | strm.avail_out = BUFSIZE; 42 | ret = inflate(&strm, Z_NO_FLUSH); 43 | if (buffer.size() < strm.total_out) 44 | { 45 | buffer.insert(buffer.end(), temp_buffer, 46 | temp_buffer + BUFSIZE - strm.avail_out); 47 | } 48 | } 49 | while (ret == Z_OK); 50 | if (ret != Z_STREAM_END) 51 | { 52 | inflateEnd(&strm); 53 | throw std::runtime_error( 54 | "ZLIB can't inflate stream (ret = " + std::to_string(ret) + ")"); 55 | } 56 | inflateEnd(&strm); 57 | std::vector decompressed(buffer); 58 | return decompressed; 59 | } 60 | else 61 | { 62 | throw std::invalid_argument("compressed too large"); 63 | } 64 | } 65 | 66 | std::vector CompressBytes(const std::vector& uncompressed) 67 | { 68 | if (uncompressed.size() <= std::numeric_limits::max()) 69 | { 70 | z_stream strm; 71 | std::vector buffer; 72 | const size_t BUFSIZE = 1024 * 1024; 73 | uint8_t temp_buffer[BUFSIZE]; 74 | strm.zalloc = Z_NULL; 75 | strm.zfree = Z_NULL; 76 | strm.opaque = Z_NULL; 77 | strm.avail_in = static_cast(uncompressed.size()); 78 | strm.next_in = const_cast(reinterpret_cast( 79 | uncompressed.data())); 80 | strm.next_out = temp_buffer; 81 | strm.avail_out = BUFSIZE; 82 | int ret = deflateInit(&strm, Z_BEST_SPEED); 83 | if (ret != Z_OK) 84 | { 85 | deflateEnd(&strm); 86 | throw std::runtime_error( 87 | "ZLIB can't init deflate stream (ret = " + std::to_string(ret) + ")"); 88 | } 89 | while (strm.avail_in != 0) 90 | { 91 | ret = deflate(&strm, Z_NO_FLUSH); 92 | if (ret != Z_OK) 93 | { 94 | deflateEnd(&strm); 95 | throw std::runtime_error( 96 | "ZLIB can't deflate stream (ret = " + std::to_string(ret) + ")"); 97 | } 98 | if (strm.avail_out == 0) 99 | { 100 | buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE); 101 | strm.next_out = temp_buffer; 102 | strm.avail_out = BUFSIZE; 103 | } 104 | } 105 | int deflate_ret = Z_OK; 106 | while (deflate_ret == Z_OK) 107 | { 108 | if (strm.avail_out == 0) 109 | { 110 | buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE); 111 | strm.next_out = temp_buffer; 112 | strm.avail_out = BUFSIZE; 113 | } 114 | deflate_ret = deflate(&strm, Z_FINISH); 115 | } 116 | if (deflate_ret != Z_STREAM_END) 117 | { 118 | deflateEnd(&strm); 119 | throw std::runtime_error( 120 | "ZLIB can't deflate stream (ret = " + std::to_string(deflate_ret) + 121 | ")"); 122 | } 123 | buffer.insert(buffer.end(), temp_buffer, 124 | temp_buffer + BUFSIZE - strm.avail_out); 125 | deflateEnd(&strm); 126 | std::vector compressed(buffer); 127 | return compressed; 128 | } 129 | else 130 | { 131 | throw std::invalid_argument("uncompressed too large"); 132 | } 133 | } 134 | 135 | std::vector LoadFromFileAndDecompress(const std::string& filename) 136 | { 137 | std::ifstream input_file( 138 | filename, std::ios::binary | std::ios::in | std::ios::ate); 139 | if (!input_file.is_open()) 140 | { 141 | throw std::runtime_error("Failed to open file [" + filename + "]"); 142 | } 143 | std::streamsize size = input_file.tellg(); 144 | input_file.seekg(0, std::ios::beg); 145 | std::vector file_buffer(static_cast(size)); 146 | if (!(input_file.read(reinterpret_cast(file_buffer.data()), size))) 147 | { 148 | throw std::runtime_error("Failed to read entire contents of file"); 149 | } 150 | const std::vector decompressed = DecompressBytes(file_buffer); 151 | return decompressed; 152 | } 153 | 154 | void CompressAndWriteToFile( 155 | const std::vector& uncompressed, const std::string& filename) 156 | { 157 | const auto compressed = CompressBytes(uncompressed); 158 | std::ofstream output_file(filename, std::ios::out | std::ios::binary); 159 | output_file.write(reinterpret_cast(compressed.data()), 160 | static_cast(compressed.size())); 161 | output_file.close(); 162 | } 163 | } // namespace zlib_helpers 164 | CRU_NAMESPACE_END 165 | } // namespace common_robotics_utilities 166 | -------------------------------------------------------------------------------- /test/hausdorff_distance_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace common_robotics_utilities 11 | { 12 | namespace 13 | { 14 | class HausdorffDistanceTestSuite 15 | : public testing::TestWithParam {}; 16 | 17 | TEST_P(HausdorffDistanceTestSuite, Test) 18 | { 19 | const parallelism::DegreeOfParallelism parallelism = GetParam(); 20 | std::cout << "# of threads = " << parallelism.GetNumThreads() << std::endl; 21 | 22 | const std::vector d1 = {1.5, 2.5, 3.5, 4.5, 5.5}; 23 | const std::vector d2 = {10.5, 11.5, 12.5, 13.5, 14.5, 15.5}; 24 | 25 | const std::vector i1 = {1, 2, 3, 4, 5}; 26 | const std::vector i2 = {10, 11, 12, 13, 14, 15}; 27 | 28 | const std::function dd_dist_fn = []( 29 | const double& first, const double& second) 30 | { 31 | return std::abs(first - second); 32 | }; 33 | 34 | const std::function di_dist_fn = []( 35 | const double& first, const int32_t& second) 36 | { 37 | return std::abs(first - static_cast(second)); 38 | }; 39 | 40 | const std::function id_dist_fn = []( 41 | const int32_t& first, const double& second) 42 | { 43 | return std::abs(static_cast(first) - second); 44 | }; 45 | 46 | const std::function ii_dist_fn = []( 47 | const double& first, const double& second) 48 | { 49 | return static_cast(std::abs(first - second)); 50 | }; 51 | 52 | // Compute pairwise distance matrices 53 | const Eigen::MatrixXd d1d2_dist_matrix = math::BuildPairwiseDistanceMatrix( 54 | d1, d2, dd_dist_fn, parallelism); 55 | const Eigen::MatrixXd d1i2_dist_matrix = math::BuildPairwiseDistanceMatrix( 56 | d1, i2, di_dist_fn, parallelism); 57 | const Eigen::MatrixXd i1d2_dist_matrix = math::BuildPairwiseDistanceMatrix( 58 | i1, d2, id_dist_fn, parallelism); 59 | const Eigen::MatrixXd i1i2_dist_matrix = math::BuildPairwiseDistanceMatrix( 60 | i1, i2, ii_dist_fn, parallelism); 61 | 62 | const Eigen::MatrixXd d1d1_dist_matrix = math::BuildPairwiseDistanceMatrix( 63 | d1, d1, dd_dist_fn, parallelism); 64 | const Eigen::MatrixXd i1i1_dist_matrix = math::BuildPairwiseDistanceMatrix( 65 | i1, i1, ii_dist_fn, parallelism); 66 | 67 | // Compute distribution-distribution distances 68 | const double func_dist_d1d2 = simple_hausdorff_distance::ComputeDistance( 69 | d1, d2, dd_dist_fn, parallelism); 70 | const double matrix_dist_d1d2 = 71 | simple_hausdorff_distance::ComputeDistance( 72 | d1, d2, d1d2_dist_matrix, parallelism); 73 | EXPECT_EQ(func_dist_d1d2, matrix_dist_d1d2); 74 | EXPECT_EQ(func_dist_d1d2, 9.0); 75 | 76 | const double func_dist_d1i2 = simple_hausdorff_distance::ComputeDistance( 77 | d1, i2, di_dist_fn, parallelism); 78 | const double matrix_dist_d1i2 = 79 | simple_hausdorff_distance::ComputeDistance( 80 | d1, i2, d1i2_dist_matrix, parallelism); 81 | EXPECT_EQ(func_dist_d1i2, matrix_dist_d1i2); 82 | EXPECT_EQ(func_dist_d1i2, 8.5); 83 | 84 | const double func_dist_i1d2 = simple_hausdorff_distance::ComputeDistance( 85 | i1, d2, id_dist_fn, parallelism); 86 | const double matrix_dist_i1d2 = 87 | simple_hausdorff_distance::ComputeDistance( 88 | i1, d2, i1d2_dist_matrix, parallelism); 89 | EXPECT_EQ(func_dist_i1d2, matrix_dist_i1d2); 90 | EXPECT_EQ(func_dist_i1d2, 9.5); 91 | 92 | const double func_dist_i1i2 = simple_hausdorff_distance::ComputeDistance( 93 | i1, i2, ii_dist_fn, parallelism); 94 | const double matrix_dist_i1i2 = 95 | simple_hausdorff_distance::ComputeDistance( 96 | i1, i2, i1i2_dist_matrix, parallelism); 97 | EXPECT_EQ(func_dist_i1i2, matrix_dist_i1i2); 98 | EXPECT_EQ(func_dist_i1i2, 9.0); 99 | 100 | const double func_dist_d1d1 = simple_hausdorff_distance::ComputeDistance( 101 | d1, d1, dd_dist_fn, parallelism); 102 | const double matrix_dist_d1d1 = 103 | simple_hausdorff_distance::ComputeDistance( 104 | d1, d1, d1d1_dist_matrix, parallelism); 105 | EXPECT_EQ(func_dist_d1d1, matrix_dist_d1d1); 106 | EXPECT_EQ(func_dist_d1d1, 0.0); 107 | 108 | const double func_dist_i1i1 = simple_hausdorff_distance::ComputeDistance( 109 | i1, i1, ii_dist_fn, parallelism); 110 | const double matrix_dist_i1i1 = 111 | simple_hausdorff_distance::ComputeDistance( 112 | i1, i1, i1i1_dist_matrix, parallelism); 113 | EXPECT_EQ(func_dist_i1i1, matrix_dist_i1i1); 114 | EXPECT_EQ(func_dist_i1i1, 0.0); 115 | } 116 | 117 | INSTANTIATE_TEST_SUITE_P( 118 | SerialHausdorffDistanceTest, HausdorffDistanceTestSuite, 119 | testing::Values(parallelism::DegreeOfParallelism::None())); 120 | 121 | // For fallback testing on platforms with no OpenMP support, specify 2 threads. 122 | int32_t GetNumThreads() 123 | { 124 | if (openmp_helpers::IsOmpEnabledInBuild()) 125 | { 126 | return openmp_helpers::GetNumOmpThreads(); 127 | } 128 | else 129 | { 130 | return 2; 131 | } 132 | } 133 | 134 | INSTANTIATE_TEST_SUITE_P( 135 | ParallelHausdorffDistanceTest, HausdorffDistanceTestSuite, 136 | testing::Values(parallelism::DegreeOfParallelism(GetNumThreads()))); 137 | } // namespace 138 | } // namespace common_robotics_utilities 139 | 140 | int main(int argc, char** argv) 141 | { 142 | testing::InitGoogleTest(&argc, argv); 143 | return RUN_ALL_TESTS(); 144 | } 145 | 146 | -------------------------------------------------------------------------------- /test/math_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace common_robotics_utilities 10 | { 11 | namespace 12 | { 13 | GTEST_TEST(MathTest, EnforceContinuousRevoluteBounds) 14 | { 15 | EXPECT_DOUBLE_EQ(math::EnforceContinuousRevoluteBounds(0.0), 0.0); 16 | EXPECT_DOUBLE_EQ(math::EnforceContinuousRevoluteBounds(1.0), 1.0); 17 | EXPECT_DOUBLE_EQ(math::EnforceContinuousRevoluteBounds(-1.0), -1.0); 18 | EXPECT_DOUBLE_EQ(math::EnforceContinuousRevoluteBounds(M_PI), -M_PI); 19 | EXPECT_DOUBLE_EQ(math::EnforceContinuousRevoluteBounds(-M_PI), -M_PI); 20 | EXPECT_DOUBLE_EQ( 21 | math::EnforceContinuousRevoluteBounds(M_PI + 1.0), (-M_PI + 1.0)); 22 | EXPECT_DOUBLE_EQ( 23 | math::EnforceContinuousRevoluteBounds(-M_PI - 1.0), (M_PI - 1.0)); 24 | EXPECT_DOUBLE_EQ(math::EnforceContinuousRevoluteBounds(2.0 * M_PI), 0.0); 25 | EXPECT_DOUBLE_EQ(math::EnforceContinuousRevoluteBounds(2.0 * - M_PI), 0.0); 26 | 27 | const std::vector angles = 28 | {0.0, 1.0, M_PI_4, M_PI_2, M_PI - 1e-6, -1.0, -M_PI_4, -M_PI_2, -M_PI}; 29 | const std::vector wraps = {0.0, 1.0, 2.0, 3.0, -1.0, -2.0, -3.0}; 30 | constexpr double full_wrap = M_PI * 2.0; 31 | 32 | for (const double& angle : angles) 33 | { 34 | for (const double& wrap : wraps) 35 | { 36 | const double value = angle + (full_wrap * wrap); 37 | const double wrapped_angle = math::EnforceContinuousRevoluteBounds(value); 38 | EXPECT_NEAR(angle, wrapped_angle, 1e-9); 39 | } 40 | } 41 | } 42 | 43 | GTEST_TEST(MathTest, ContinuousRevoluteSignedDistance) 44 | { 45 | const std::vector starts = 46 | {0.0, 1.0, M_PI_4, M_PI_2, M_PI, -1.0, -M_PI_4, -M_PI_2, -M_PI}; 47 | const std::vector offsets = 48 | {0.0, 1.0, M_PI_4, M_PI_2, M_PI - 1e-6, -1.0, -M_PI_4, -M_PI_2, -M_PI}; 49 | 50 | for (const double& start : starts) 51 | { 52 | for (const double& offset : offsets) 53 | { 54 | const double raw_end = start + offset; 55 | const double end = math::EnforceContinuousRevoluteBounds(raw_end); 56 | 57 | const double raw_signed_distance = 58 | math::ContinuousRevoluteSignedDistance(start, raw_end); 59 | const double signed_distance = 60 | math::ContinuousRevoluteSignedDistance(start, end); 61 | EXPECT_DOUBLE_EQ(raw_signed_distance, signed_distance); 62 | EXPECT_NEAR(raw_signed_distance, offset, 1e-9); 63 | EXPECT_NEAR(signed_distance, offset, 1e-9); 64 | 65 | const double raw_distance = 66 | math::ContinuousRevoluteDistance(start, raw_end); 67 | const double distance = 68 | math::ContinuousRevoluteDistance(start, end); 69 | EXPECT_DOUBLE_EQ(raw_distance, distance); 70 | EXPECT_NEAR(raw_distance, std::abs(offset), 1e-9); 71 | EXPECT_NEAR(distance, std::abs(offset), 1e-9); 72 | } 73 | } 74 | } 75 | 76 | GTEST_TEST(MathTest, InterpolateContinuousRevolute) 77 | { 78 | const std::vector starts = 79 | {0.0, 1.0, M_PI_4, M_PI_2, M_PI, -1.0, -M_PI_4, -M_PI_2, -M_PI}; 80 | const std::vector offsets = 81 | {0.0, 1.0, M_PI_4, M_PI_2, M_PI - 1e-6, -1.0, -M_PI_4, -M_PI_2, -M_PI}; 82 | const std::vector ratios = {0.0, 0.25, 0.5, 0.75, 1.0}; 83 | 84 | for (const double& start : starts) 85 | { 86 | for (const double& offset : offsets) 87 | { 88 | const double raw_end = start + offset; 89 | const double end = math::EnforceContinuousRevoluteBounds(raw_end); 90 | 91 | for (const double& ratio : ratios) 92 | { 93 | const double interp = 94 | math::InterpolateContinuousRevolute(start, end, ratio); 95 | const double check = 96 | math::EnforceContinuousRevoluteBounds(start + (offset * ratio)); 97 | EXPECT_NEAR(interp, check, 1e-9); 98 | } 99 | } 100 | } 101 | } 102 | } // namespace 103 | } // namespace common_robotics_utilities 104 | 105 | int main(int argc, char** argv) 106 | { 107 | testing::InitGoogleTest(&argc, argv); 108 | return RUN_ALL_TESTS(); 109 | } 110 | -------------------------------------------------------------------------------- /test/print_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 7 | #include 8 | #include 9 | #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 10 | #include 11 | #else 12 | #error "Undefined or unknown COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION" 13 | #endif 14 | 15 | #include 16 | 17 | namespace 18 | { 19 | namespace test 20 | { 21 | struct DummyType 22 | { 23 | int32_t value = 0; 24 | }; 25 | 26 | std::ostream& operator<<(std::ostream& os, const test::DummyType& dummy) 27 | { 28 | return os << dummy.value; 29 | } 30 | } // namespace test 31 | } // namespace 32 | 33 | namespace common_robotics_utilities 34 | { 35 | namespace 36 | { 37 | GTEST_TEST(PrintTest, CanPrintIfADLCanFindStreamOperator) 38 | { 39 | std::cout << print::Print(std::vector{{1, 1, 1}}) 40 | << std::endl; 41 | std::cout << print::Print(std::vector(3)) << std::endl; 42 | } 43 | 44 | GTEST_TEST(PrintTest, CanPrintROSMessages) 45 | { 46 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 47 | using Point = geometry_msgs::msg::Point; 48 | #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 49 | using Point = geometry_msgs::Point; 50 | #endif 51 | std::cout << print::Print(std::vector(3)) << std::endl; 52 | } 53 | } // namespace 54 | } // namespace common_robotics_utilities 55 | 56 | int main(int argc, char** argv) 57 | { 58 | testing::InitGoogleTest(&argc, argv); 59 | return RUN_ALL_TESTS(); 60 | } 61 | -------------------------------------------------------------------------------- /test/ros_helpers_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace common_robotics_utilities 6 | { 7 | namespace ros_helpers 8 | { 9 | namespace 10 | { 11 | 12 | GTEST_TEST(ROSHelpersTest, SetMessageTimestamps) 13 | { 14 | VisualizationMarkerArray marker_array; 15 | marker_array.markers.resize(5); 16 | // Note that the ROS time type depends on which ROS variant is in use. 17 | #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 18 | // Base time measured against ROS clock. 19 | const rclcpp::Time base_stamp(0LL, RCL_ROS_TIME); 20 | // Advance time by one second. 21 | const rclcpp::Time expected_stamp = 22 | base_stamp + rclcpp::Duration(1, 0); 23 | #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 24 | // Default constructed base time. 25 | const ros::Time base_stamp; 26 | // Advance time by one second. 27 | const ros::Time expected_stamp = 28 | base_stamp + ros::Duration(1.0); 29 | #endif 30 | SetMessageTimestamps(marker_array, expected_stamp); 31 | for (const auto& marker : marker_array.markers) { 32 | EXPECT_EQ(marker.header.stamp, expected_stamp); 33 | } 34 | // Test that ROS messages have an ostream operator available. 35 | std::cout << "Set message times to:\n" 36 | << marker_array.markers.at(0).header.stamp << std::endl; 37 | } 38 | 39 | } // namespace 40 | } // namespace ros_helpers_test 41 | } // namespace common_robotics_utilities 42 | 43 | int main(int argc, char** argv) 44 | { 45 | testing::InitGoogleTest(&argc, argv); 46 | return RUN_ALL_TESTS(); 47 | } 48 | -------------------------------------------------------------------------------- /test/task_planning_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace common_robotics_utilities 14 | { 15 | namespace simple_task_planner 16 | { 17 | namespace 18 | { 19 | class PutInBoxState 20 | { 21 | public: 22 | PutInBoxState() {} 23 | 24 | PutInBoxState(const int32_t objects_available, 25 | const bool object_put_away, 26 | const bool box_open) 27 | : objects_available_(objects_available), 28 | object_put_away_(object_put_away), 29 | box_open_(box_open) {} 30 | 31 | int32_t ObjectsAvailable() const { return objects_available_; } 32 | 33 | bool ObjectPutAway() const { return object_put_away_; } 34 | 35 | bool BoxOpen() const { return box_open_; } 36 | 37 | std::string Print() const 38 | { 39 | std::string rep 40 | = "Objects available: " + std::to_string(objects_available_) 41 | + " Object put away: " 42 | + common_robotics_utilities::print::Print(object_put_away_) 43 | + " Box open: " 44 | + common_robotics_utilities::print::Print(box_open_); 45 | return rep; 46 | } 47 | 48 | bool operator==(const PutInBoxState& other) const 49 | { 50 | if (ObjectPutAway() == other.ObjectPutAway() && 51 | BoxOpen() == other.BoxOpen()) 52 | { 53 | return ((ObjectsAvailable() > 0 && other.ObjectsAvailable() > 0) || 54 | (ObjectsAvailable() == 0 && other.ObjectsAvailable() == 0) || 55 | (ObjectsAvailable() < 0 && other.ObjectsAvailable() < 0)); 56 | } 57 | else 58 | { 59 | return false; 60 | } 61 | } 62 | 63 | private: 64 | int32_t objects_available_ = -1; 65 | bool object_put_away_ = false; 66 | bool box_open_ = false; 67 | }; 68 | 69 | struct PutInBoxStateHasher 70 | { 71 | size_t operator()(const PutInBoxState& state) const 72 | { 73 | const uint64_t BOX_OPEN = 0x01; 74 | const uint64_t BOX_CLOSED = 0x02; 75 | const uint64_t OBJECT_NOT_PUT_AWAY = 0x04; 76 | const uint64_t OBJECT_PUT_AWAY = 0x08; 77 | const uint64_t OBJECTS_AVAILABLE_NONE = 0x10; 78 | const uint64_t OBJECTS_AVAILABLE_SOME = 0x20; 79 | 80 | uint64_t state_identifier = 0; 81 | 82 | if (state.BoxOpen()) 83 | { 84 | state_identifier |= BOX_OPEN; 85 | } 86 | else 87 | { 88 | state_identifier |= BOX_CLOSED; 89 | } 90 | 91 | if (state.ObjectPutAway()) 92 | { 93 | state_identifier |= OBJECT_PUT_AWAY; 94 | } 95 | else 96 | { 97 | state_identifier |= OBJECT_NOT_PUT_AWAY; 98 | } 99 | 100 | if (state.ObjectsAvailable() > 0) 101 | { 102 | state_identifier |= OBJECTS_AVAILABLE_SOME; 103 | } 104 | else if (state.ObjectsAvailable() == 0) 105 | { 106 | state_identifier |= OBJECTS_AVAILABLE_NONE; 107 | } 108 | 109 | return static_cast(state_identifier); 110 | } 111 | }; 112 | 113 | std::ostream& operator<<(std::ostream& strm, const PutInBoxState& state) 114 | { 115 | strm << state.Print(); 116 | return strm; 117 | } 118 | 119 | using PutInBoxStatesWithCosts = OutcomesWithCosts; 120 | using PutInBoxStateContainer = std::vector; 121 | using PutInBoxStatePrimitiveType = 122 | ActionPrimitiveInterface; 123 | using PutInBoxStatePrimitiveCollection = 124 | ActionPrimitiveCollection; 125 | 126 | class OpenBoxPrimitive : public PutInBoxStatePrimitiveType 127 | { 128 | public: 129 | bool IsCandidate(const PutInBoxState& state) const override 130 | { 131 | return (!state.BoxOpen()); 132 | } 133 | 134 | const std::string& Name() const override { return name_; } 135 | 136 | PutInBoxStatesWithCosts GetOutcomes(const PutInBoxState& state) const override 137 | { 138 | if (IsCandidate(state)) 139 | { 140 | PutInBoxStatesWithCosts outcomes; 141 | outcomes.emplace_back( 142 | PutInBoxState(state.ObjectsAvailable(), state.ObjectPutAway(), true), 143 | 1.0); 144 | return outcomes; 145 | } 146 | else 147 | { 148 | throw std::invalid_argument("State is not a candidate for primitive"); 149 | } 150 | } 151 | 152 | PutInBoxStateContainer Execute(const PutInBoxState& state) override 153 | { 154 | if (IsCandidate(state)) 155 | { 156 | return {PutInBoxState( 157 | state.ObjectsAvailable(), state.ObjectPutAway(), true)}; 158 | } 159 | else 160 | { 161 | throw std::invalid_argument("State is not a candidate for primitive"); 162 | } 163 | } 164 | 165 | private: 166 | const std::string name_ = "OpenBoxPrimitive"; 167 | }; 168 | 169 | class CloseBoxPrimitive : public PutInBoxStatePrimitiveType 170 | { 171 | public: 172 | bool IsCandidate(const PutInBoxState& state) const override 173 | { 174 | return (state.BoxOpen()); 175 | } 176 | 177 | const std::string& Name() const override { return name_; } 178 | 179 | PutInBoxStatesWithCosts GetOutcomes(const PutInBoxState& state) const override 180 | { 181 | if (IsCandidate(state)) 182 | { 183 | PutInBoxStatesWithCosts outcomes; 184 | outcomes.emplace_back( 185 | PutInBoxState(state.ObjectsAvailable(), state.ObjectPutAway(), false), 186 | 1.0); 187 | return outcomes; 188 | } 189 | else 190 | { 191 | throw std::invalid_argument("State is not a candidate for primitive"); 192 | } 193 | } 194 | 195 | PutInBoxStateContainer Execute(const PutInBoxState& state) override 196 | { 197 | if (IsCandidate(state)) 198 | { 199 | return {PutInBoxState( 200 | state.ObjectsAvailable(), state.ObjectPutAway(), false)}; 201 | } 202 | else 203 | { 204 | throw std::invalid_argument("State is not a candidate for primitive"); 205 | } 206 | } 207 | 208 | private: 209 | const std::string name_ = "CloseBoxPrimitive"; 210 | }; 211 | 212 | class CheckIfObjectAvailablePrimitive : public PutInBoxStatePrimitiveType 213 | { 214 | public: 215 | bool IsCandidate(const PutInBoxState& state) const override 216 | { 217 | return (state.ObjectsAvailable() < 0); 218 | } 219 | 220 | const std::string& Name() const override { return name_; } 221 | 222 | PutInBoxStatesWithCosts GetOutcomes(const PutInBoxState& state) const override 223 | { 224 | if (IsCandidate(state)) 225 | { 226 | const PutInBoxState object_available( 227 | 1, state.ObjectPutAway(), state.BoxOpen()); 228 | const PutInBoxState none_available( 229 | 0, state.ObjectPutAway(), state.BoxOpen()); 230 | 231 | PutInBoxStatesWithCosts outcomes; 232 | outcomes.emplace_back(object_available, 0.5); 233 | outcomes.emplace_back(none_available, 0.5); 234 | return outcomes; 235 | } 236 | else 237 | { 238 | throw std::invalid_argument("State is not a candidate for primitive"); 239 | } 240 | } 241 | 242 | PutInBoxStateContainer Execute(const PutInBoxState& state) override 243 | { 244 | if (IsCandidate(state)) 245 | { 246 | return {PutInBoxState( 247 | std::abs(state.ObjectsAvailable()), state.ObjectPutAway(), 248 | state.BoxOpen())}; 249 | } 250 | else 251 | { 252 | throw std::invalid_argument("State is not a candidate for primitive"); 253 | } 254 | } 255 | 256 | private: 257 | const std::string name_ = "CheckIfObjectAvailablePrimitive"; 258 | }; 259 | 260 | class PutObjectInBoxPrimitive : public PutInBoxStatePrimitiveType 261 | { 262 | public: 263 | bool IsCandidate(const PutInBoxState& state) const override 264 | { 265 | return ((state.ObjectsAvailable() > 0) && (state.ObjectPutAway() == false) 266 | && state.BoxOpen()); 267 | } 268 | 269 | const std::string& Name() const override { return name_; } 270 | 271 | PutInBoxStatesWithCosts GetOutcomes(const PutInBoxState& state) const override 272 | { 273 | if (IsCandidate(state)) 274 | { 275 | const PutInBoxState object_remaining(1, true, true); 276 | const PutInBoxState task_done(0, true, true); 277 | 278 | PutInBoxStatesWithCosts outcomes; 279 | outcomes.emplace_back(object_remaining, 1.0); 280 | outcomes.emplace_back(task_done, 1.0); 281 | return outcomes; 282 | } 283 | else 284 | { 285 | throw std::invalid_argument("State is not a candidate for primitive"); 286 | } 287 | } 288 | 289 | PutInBoxStateContainer Execute(const PutInBoxState& state) override 290 | { 291 | if (IsCandidate(state)) 292 | { 293 | return {PutInBoxState(state.ObjectsAvailable() - 1, true, true)}; 294 | } 295 | else 296 | { 297 | throw std::invalid_argument("State is not a candidate for primitive"); 298 | } 299 | } 300 | 301 | private: 302 | const std::string name_ = "PutObjectInBoxPrimitive"; 303 | }; 304 | 305 | bool IsTaskComplete(const PutInBoxState& state) 306 | { 307 | return ((state.ObjectsAvailable() == 0) && (state.BoxOpen() == false)); 308 | } 309 | 310 | bool IsSingleExecutionComplete(const PutInBoxState& state) 311 | { 312 | if (IsTaskComplete(state)) 313 | { 314 | return true; 315 | } 316 | else 317 | { 318 | return ((state.ObjectsAvailable() > 0) && (state.ObjectPutAway() == true)); 319 | } 320 | } 321 | 322 | GTEST_TEST(TaskPlanningTest, Test) 323 | { 324 | // Collect the primitives 325 | PutInBoxStatePrimitiveCollection primitive_collection; 326 | primitive_collection.RegisterPrimitive(std::make_shared()); 327 | primitive_collection.RegisterPrimitive(std::make_shared()); 328 | primitive_collection.RegisterPrimitive( 329 | std::make_shared()); 330 | primitive_collection.RegisterPrimitive( 331 | std::make_shared()); 332 | 333 | // Plan a sequence for each possible starting state 334 | const PutInBoxStateContainer possible_starting_states = { 335 | PutInBoxState(-1, false, false), PutInBoxState(-1, false, true), 336 | PutInBoxState(-1, true, false), PutInBoxState(-1, true, true), 337 | PutInBoxState(0, false, false), PutInBoxState(0, false, true), 338 | PutInBoxState(0, true, false), PutInBoxState(0, true, true), 339 | PutInBoxState(1, false, false), PutInBoxState(1, false, true), 340 | PutInBoxState(1, true, false), PutInBoxState(1, true, true)}; 341 | 342 | for (const auto& starting_state : possible_starting_states) 343 | { 344 | std::cout << "++++++++++\nStarting state:\n" << starting_state << std::endl; 345 | const auto task_sequence_plan = PlanTaskStateSequence 346 | ( 347 | primitive_collection, IsSingleExecutionComplete, {starting_state}); 348 | std::cout << "Task sequence plan:" << std::endl; 349 | for (const auto& state : task_sequence_plan.Path()) 350 | { 351 | std::cout << print::Print(state.Outcome()) << std::endl; 352 | } 353 | ASSERT_TRUE(!std::isinf(task_sequence_plan.PathCost())); 354 | ASSERT_GT(task_sequence_plan.Path().size(), 0u); 355 | const auto& final_state = task_sequence_plan.Path().back(); 356 | ASSERT_TRUE(IsSingleExecutionComplete(final_state.Outcome())); 357 | } 358 | 359 | // Execute the task 360 | PutInBoxStateContainer task_execution_trace; 361 | bool task_complete = false; 362 | int32_t num_task_executions = 0; 363 | while (!task_complete) 364 | { 365 | num_task_executions++; 366 | std::cout << "++++++++++++++++++++\nStarting task execution " 367 | << num_task_executions << std::endl; 368 | PutInBoxState execution_start_state; 369 | if (task_execution_trace.empty()) 370 | { 371 | // Start with five objects to put away. 372 | execution_start_state = PutInBoxState(-5, false, false); 373 | } 374 | else 375 | { 376 | const auto& last = task_execution_trace.back(); 377 | execution_start_state = PutInBoxState( 378 | -last.ObjectsAvailable(), false, last.BoxOpen()); 379 | } 380 | const auto execution_trace = PerformSingleTaskExecution 381 | ( 382 | primitive_collection, IsSingleExecutionComplete, 383 | execution_start_state, -1); 384 | std::cout << "Single execution of task in " << execution_trace.size() 385 | << " states" << std::endl; 386 | ASSERT_GT(execution_trace.size(), 0u); 387 | const auto& last_state = execution_trace.back(); 388 | ASSERT_TRUE(IsSingleExecutionComplete(last_state)); 389 | if (IsTaskComplete(last_state)) 390 | { 391 | task_complete = true; 392 | } 393 | task_execution_trace.insert( 394 | task_execution_trace.end(), execution_trace.begin(), 395 | execution_trace.end()); 396 | } 397 | std::cout << "Completed task in " << num_task_executions << " executions" 398 | << std::endl; 399 | ASSERT_GT(task_execution_trace.size(), 0u); 400 | ASSERT_TRUE(IsTaskComplete(task_execution_trace.back())); 401 | } 402 | } // namespace 403 | } // namespace simple_task_planner 404 | } // namespace common_robotics_utilities 405 | 406 | int main(int argc, char** argv) 407 | { 408 | testing::InitGoogleTest(&argc, argv); 409 | return RUN_ALL_TESTS(); 410 | } 411 | -------------------------------------------------------------------------------- /test/utility_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | namespace common_robotics_utilities 15 | { 16 | namespace 17 | { 18 | GTEST_TEST(UtilityTest, GetUniformRandom) 19 | { 20 | std::mt19937_64 rng(42); 21 | std::uniform_real_distribution uniform_unit_real_dist(0.0, 1.0); 22 | 23 | const utility::UniformUnitRealFunction uniform_unit_real_fn = [&] () 24 | { 25 | return uniform_unit_real_dist(rng); 26 | }; 27 | 28 | constexpr int64_t num_samples = 1000000; 29 | 30 | for (int64_t sample_num = 0; sample_num < num_samples; sample_num++) 31 | { 32 | for (int64_t container_size : {10, 100, 1000, 1000000, 1000000000}) 33 | { 34 | const int64_t random_index = utility::GetUniformRandomIndex( 35 | uniform_unit_real_fn, container_size); 36 | ASSERT_GE(random_index, 0); 37 | ASSERT_LT(random_index, container_size); 38 | 39 | const int64_t range_start = container_size; 40 | const int64_t range_end = container_size * 2; 41 | 42 | const int64_t random_val = utility::GetUniformRandomInRange( 43 | uniform_unit_real_fn, range_start, range_end); 44 | ASSERT_GE(random_val, range_start); 45 | ASSERT_LE(random_val, range_end); 46 | } 47 | } 48 | 49 | ASSERT_THROW( 50 | utility::GetUniformRandomIndex(uniform_unit_real_fn, 0), 51 | std::invalid_argument); 52 | ASSERT_THROW( 53 | utility::GetUniformRandomInRange(uniform_unit_real_fn, 10, 9), 54 | std::invalid_argument); 55 | } 56 | 57 | GTEST_TEST(UtilityTest, ClampAndSpread) 58 | { 59 | const utility::LoggingFunction logging_fn = [] (const std::string& msg) 60 | { 61 | std::cout << msg << std::endl; 62 | }; 63 | ASSERT_EQ(utility::ClampValue(1.0, -0.5, 0.5), 0.5); 64 | ASSERT_EQ(utility::ClampValueAndLog(1.0, -0.5, 0.5, logging_fn), 0.5); 65 | 66 | ASSERT_EQ(utility::ClampValue(-1.0, -0.5, 0.5), -0.5); 67 | ASSERT_EQ(utility::ClampValueAndLog(-1.0, -0.5, 0.5, logging_fn), -0.5); 68 | 69 | ASSERT_EQ(utility::ClampValue(0.25, -0.5, 0.5), 0.25); 70 | ASSERT_EQ(utility::ClampValueAndLog(0.25, -0.5, 0.5, logging_fn), 0.25); 71 | 72 | ASSERT_THROW(utility::ClampValue(1.0, 1.0, -1.0), std::invalid_argument); 73 | ASSERT_THROW( 74 | utility::ClampValueAndLog(1.0, 1.0, -1.0, logging_fn), 75 | std::invalid_argument); 76 | 77 | ASSERT_EQ(utility::SpreadValue(1.0, -0.5, 0.0, 0.5), 1.0); 78 | ASSERT_EQ(utility::SpreadValueAndLog(1.0, -0.5, 0.0, 0.5, logging_fn), 1.0); 79 | 80 | ASSERT_EQ(utility::SpreadValue(-1.0, -0.5, 0.0, 0.5), -1.0); 81 | ASSERT_EQ( 82 | utility::SpreadValueAndLog(-1.0, -0.5, 0.0, 0.5, logging_fn), -1.0); 83 | 84 | ASSERT_EQ(utility::SpreadValue(0.25, -0.5, 0.0, 0.5), 0.5); 85 | ASSERT_EQ(utility::SpreadValueAndLog(0.25, -0.5, 0.0, 0.5, logging_fn), 0.5); 86 | 87 | ASSERT_EQ(utility::SpreadValue(-0.25, -0.5, 0.0, 0.5), -0.5); 88 | ASSERT_EQ( 89 | utility::SpreadValueAndLog(-0.25, -0.5, 0.0, 0.5, logging_fn), -0.5); 90 | 91 | ASSERT_THROW( 92 | utility::SpreadValue(1.0, 1.0, 0.0, -1.0), std::invalid_argument); 93 | ASSERT_THROW( 94 | utility::SpreadValueAndLog(1.0, 1.0, 0.0, -1.0, logging_fn), 95 | std::invalid_argument); 96 | } 97 | 98 | GTEST_TEST(UtilityTest, Alignment) 99 | { 100 | uint8_t* const aligned_8byte_ptr = 101 | reinterpret_cast(aligned_alloc(8, 256)); 102 | uint8_t* const aligned_16byte_ptr = 103 | reinterpret_cast(aligned_alloc(16, 256)); 104 | ASSERT_NE(aligned_8byte_ptr, nullptr); 105 | ASSERT_NE(aligned_16byte_ptr, nullptr); 106 | 107 | uint8_t* const off_aligned_8byte_ptr = aligned_8byte_ptr + 1; 108 | uint8_t* const off_aligned_16byte_ptr = aligned_16byte_ptr + 1; 109 | 110 | ASSERT_TRUE(utility::CheckAlignment(*aligned_8byte_ptr, 8)); 111 | ASSERT_TRUE(utility::CheckAlignment(*aligned_16byte_ptr, 16)); 112 | 113 | ASSERT_FALSE(utility::CheckAlignment(*off_aligned_8byte_ptr, 8)); 114 | ASSERT_FALSE(utility::CheckAlignment(*off_aligned_16byte_ptr, 16)); 115 | 116 | ASSERT_NO_THROW(utility::RequireAlignment(*aligned_8byte_ptr, 8)); 117 | ASSERT_NO_THROW(utility::RequireAlignment(*aligned_16byte_ptr, 16)); 118 | 119 | ASSERT_THROW( 120 | utility::RequireAlignment(*off_aligned_8byte_ptr, 8), std::runtime_error); 121 | ASSERT_THROW( 122 | utility::RequireAlignment(*off_aligned_16byte_ptr, 16), 123 | std::runtime_error); 124 | 125 | free(aligned_8byte_ptr); 126 | free(aligned_16byte_ptr); 127 | } 128 | 129 | GTEST_TEST(UtilityTest, GetAndSetBits) 130 | { 131 | const uint8_t test_val = 0xaa; 132 | ASSERT_EQ(utility::SetBit(test_val, 0, true), 0xab); 133 | ASSERT_EQ(utility::SetBit(test_val, 3, false), 0xa2); 134 | 135 | ASSERT_TRUE(utility::GetBit(test_val, 3)); 136 | ASSERT_FALSE(utility::GetBit(test_val, 4)); 137 | } 138 | 139 | GTEST_TEST(UtilityTest, Substrings) 140 | { 141 | const std::vector strings = {"ex_foo", "ex_bar", "ex_baz"}; 142 | ASSERT_TRUE(utility::CheckAllStringsForSubstring(strings, "ex_")); 143 | ASSERT_FALSE(utility::CheckAllStringsForSubstring(strings, "ex_b")); 144 | } 145 | 146 | GTEST_TEST(UtilityTest, CollectionsAndSubset) 147 | { 148 | const std::vector collection_1 = {1, 2, 3, 4, 5}; 149 | const std::vector collection_2 = {1, 1, 2, 2, 3, 3, 4, 4, 5, 5}; 150 | const std::vector collection_3 = {3, 5, 1, 2, 4}; 151 | 152 | ASSERT_FALSE(utility::IsSubset(collection_1, collection_2)); 153 | ASSERT_TRUE(utility::IsSubset(collection_2, collection_1)); 154 | ASSERT_TRUE(utility::CollectionsEqual(collection_1, collection_3)); 155 | ASSERT_FALSE(utility::CollectionsEqual(collection_1, collection_2)); 156 | } 157 | 158 | GTEST_TEST(UtilityTest, GetFromMapAndSet) 159 | { 160 | const std::vector test_keys = {1, 2, 3, 4, 5}; 161 | std::vector> test_keys_and_values; 162 | std::set test_set; 163 | std::unordered_set test_unordered_set; 164 | std::map test_map; 165 | std::unordered_map test_unordered_map; 166 | 167 | for (const int32_t& test_key : test_keys) 168 | { 169 | test_keys_and_values.push_back(std::make_pair(test_key, test_key)); 170 | test_set.insert(test_key); 171 | test_unordered_set.insert(test_key); 172 | test_map[test_key] = test_key; 173 | test_unordered_map[test_key] = test_key; 174 | } 175 | 176 | ASSERT_TRUE(utility::CollectionsEqual( 177 | test_keys, utility::GetKeysFromSetLike(test_set))); 178 | ASSERT_TRUE(utility::CollectionsEqual( 179 | test_keys, utility::GetKeysFromSetLike(test_unordered_set))); 180 | 181 | ASSERT_TRUE(utility::CollectionsEqual( 182 | test_keys, utility::GetKeysFromMapLike(test_map))); 183 | ASSERT_TRUE(utility::CollectionsEqual( 184 | test_keys, 185 | utility::GetKeysFromMapLike(test_unordered_map))); 186 | 187 | const auto test_map_keys_and_values = 188 | utility::GetKeysAndValues(test_map); 189 | const auto test_unordered_map_keys_and_values = 190 | utility::GetKeysAndValues(test_unordered_map); 191 | const bool test_map_match = 192 | utility::CollectionsEqual>( 193 | test_keys_and_values, test_map_keys_and_values); 194 | const bool test_unordered_map_match = 195 | utility::CollectionsEqual>( 196 | test_keys_and_values, test_unordered_map_keys_and_values); 197 | 198 | ASSERT_TRUE(test_map_match); 199 | ASSERT_TRUE(test_unordered_map_match); 200 | } 201 | 202 | GTEST_TEST(UtilityTest, MakeMapAndSet) 203 | { 204 | const std::vector test_keys = {1, 2, 3, 4, 5}; 205 | std::vector> test_keys_and_values; 206 | for (const int32_t& test_key : test_keys) 207 | { 208 | test_keys_and_values.push_back(std::make_pair(test_key, test_key)); 209 | } 210 | 211 | const auto test_map1 = 212 | utility::MakeFromKeysAndValues(test_keys, test_keys); 213 | const auto test_map2 = 214 | utility::MakeFromKeysAndValues(test_keys_and_values); 215 | 216 | const bool test_map1_match = 217 | utility::CollectionsEqual>( 218 | test_keys_and_values, 219 | utility::GetKeysAndValues(test_map1)); 220 | const bool test_map2_match = 221 | utility::CollectionsEqual>( 222 | test_keys_and_values, 223 | utility::GetKeysAndValues(test_map2)); 224 | 225 | ASSERT_TRUE(test_map1_match); 226 | ASSERT_TRUE(test_map2_match); 227 | } 228 | 229 | GTEST_TEST(UtilityTest, IsFutureReadyTest) 230 | { 231 | const auto wait_op = [] (const double wait) 232 | { 233 | std::this_thread::sleep_for(std::chrono::duration(wait)); 234 | }; 235 | std::future wait_future = 236 | std::async(std::launch::async, wait_op, 1.0); 237 | EXPECT_FALSE(utility::IsFutureReady(wait_future)); 238 | wait_future.wait(); 239 | EXPECT_TRUE(utility::IsFutureReady(wait_future)); 240 | } 241 | 242 | GTEST_TEST(UtilityTest, OnScopeExitTest) 243 | { 244 | int test_val = 0; 245 | 246 | const auto on_exit_fn = [&test_val]() { test_val = 10; }; 247 | 248 | EXPECT_EQ(test_val, 0); 249 | { 250 | const utility::OnScopeExit on_exit(on_exit_fn); 251 | EXPECT_TRUE(on_exit.IsEnabled()); 252 | } 253 | EXPECT_EQ(test_val, 10); 254 | 255 | test_val = 0; 256 | 257 | EXPECT_EQ(test_val, 0); 258 | { 259 | utility::OnScopeExit on_exit(on_exit_fn); 260 | EXPECT_TRUE(on_exit.IsEnabled()); 261 | on_exit.Disable(); 262 | EXPECT_FALSE(on_exit.IsEnabled()); 263 | } 264 | EXPECT_EQ(test_val, 0); 265 | } 266 | 267 | GTEST_TEST(UtilityTest, CopyableMoveableAtomicMethodsTest) 268 | { 269 | // Thread safety is covered elsewhere, simply exercise the API here. 270 | 271 | utility::CopyableMoveableAtomic basic_bool(false); 272 | EXPECT_FALSE(basic_bool.load()); 273 | basic_bool.store(true); 274 | EXPECT_TRUE(basic_bool.load()); 275 | basic_bool.store(false); 276 | EXPECT_FALSE(basic_bool.load()); 277 | 278 | utility::CopyableMoveableAtomic basic_int32(0); 279 | EXPECT_EQ(basic_int32.load(), 0); 280 | basic_int32.store(100); 281 | EXPECT_EQ(basic_int32.load(), 100); 282 | basic_int32.fetch_add(25); 283 | EXPECT_EQ(basic_int32.load(), 125); 284 | basic_int32.fetch_sub(50); 285 | EXPECT_EQ(basic_int32.load(), 75); 286 | } 287 | 288 | GTEST_TEST(UtilityTest, CopyableMoveableAtomicSelfAssignTest) 289 | { 290 | using CMAi32 = utility::CopyableMoveableAtomic; 291 | 292 | CMAi32 basic_int32(100); 293 | EXPECT_EQ(basic_int32.load(), 100); 294 | 295 | auto& basic_copy_assign_ret = (basic_int32 = basic_int32); 296 | EXPECT_EQ(basic_int32.load(), 100); 297 | EXPECT_EQ(basic_copy_assign_ret.load(), 100); 298 | 299 | // GCC and Clang will warn if we directly write var = std::move(var), but we 300 | // can exercise the same operation using std::swap(var, var). 301 | std::swap(basic_int32, basic_int32); 302 | EXPECT_EQ(basic_int32.load(), 100); 303 | } 304 | 305 | GTEST_TEST(UtilityTest, CopyableMoveableAtomicCopyMoveAssignTest) 306 | { 307 | using CMAi32 = utility::CopyableMoveableAtomic; 308 | 309 | // Initial value for copy operations. 310 | const CMAi32 const_int32(100); 311 | EXPECT_EQ(const_int32.load(), 100); 312 | 313 | // Copy construction works. 314 | const CMAi32 copied_int32(const_int32); 315 | EXPECT_EQ(copied_int32.load(), 100); 316 | 317 | // Copy assignment works. 318 | CMAi32 assigned_int32(0); 319 | EXPECT_EQ(assigned_int32.load(), 0); 320 | assigned_int32 = const_int32; 321 | EXPECT_EQ(assigned_int32.load(), 100); 322 | 323 | // Initial value for move operations. 324 | CMAi32 mutable_int32(200); 325 | EXPECT_EQ(mutable_int32.load(), 200); 326 | 327 | // Move construction works. 328 | CMAi32 move_constructed_int32(std::move(mutable_int32)); 329 | EXPECT_EQ(move_constructed_int32.load(), 200); 330 | // Move constructor leaves moved-from unmodified. 331 | EXPECT_EQ(mutable_int32.load(), 200); 332 | 333 | // Move assignment works. 334 | CMAi32 moved_to_int32(0); 335 | EXPECT_EQ(moved_to_int32.load(), 0); 336 | moved_to_int32 = std::move(mutable_int32); 337 | EXPECT_EQ(moved_to_int32.load(), 200); 338 | // Move assignment leaves moved-from unmodified. 339 | EXPECT_EQ(mutable_int32.load(), 200); 340 | } 341 | 342 | GTEST_TEST(UtilityTest, CopyableMoveableAtomicCrossMemoryOrderTest) 343 | { 344 | using CMAi32 = utility::CopyableMoveableAtomic; 345 | using CMAi32Relaxed = 346 | utility::CopyableMoveableAtomic; 347 | 348 | // Initial value for copy operations. 349 | const CMAi32 const_int32(100); 350 | EXPECT_EQ(const_int32.load(), 100); 351 | 352 | // Copy construction works. 353 | const CMAi32Relaxed copied_int32(const_int32); 354 | EXPECT_EQ(copied_int32.load(), 100); 355 | 356 | // Copy assignment works. 357 | CMAi32Relaxed assigned_int32(0); 358 | EXPECT_EQ(assigned_int32.load(), 0); 359 | assigned_int32 = const_int32; 360 | EXPECT_EQ(assigned_int32.load(), 100); 361 | 362 | // Initial value for move operations. 363 | CMAi32 mutable_int32(200); 364 | EXPECT_EQ(mutable_int32.load(), 200); 365 | 366 | // Move construction works. 367 | CMAi32Relaxed move_constructed_int32(std::move(mutable_int32)); 368 | EXPECT_EQ(move_constructed_int32.load(), 200); 369 | // Move constructor leaves moved-from unmodified. 370 | EXPECT_EQ(mutable_int32.load(), 200); 371 | 372 | // Move assignment works. 373 | CMAi32Relaxed moved_to_int32(0); 374 | EXPECT_EQ(moved_to_int32.load(), 0); 375 | moved_to_int32 = std::move(mutable_int32); 376 | EXPECT_EQ(moved_to_int32.load(), 200); 377 | // Move assignment leaves moved-from unmodified. 378 | EXPECT_EQ(mutable_int32.load(), 200); 379 | } 380 | } // namespace 381 | } // namespace common_robotics_utilities 382 | 383 | int main(int argc, char** argv) 384 | { 385 | testing::InitGoogleTest(&argc, argv); 386 | return RUN_ALL_TESTS(); 387 | } 388 | -------------------------------------------------------------------------------- /test/voxel_grid_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace common_robotics_utilities 11 | { 12 | namespace 13 | { 14 | GTEST_TEST(VoxelGridTest, IndexLookup) 15 | { 16 | const voxel_grid::GridSizes grid_sizes(1.0, 20.0, 20.0, 20.0); 17 | voxel_grid::VoxelGrid test_grid(grid_sizes, 0); 18 | // Load with special values 19 | int32_t fill_val = 1; 20 | std::vector check_vals; 21 | for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++) 22 | { 23 | for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++) 24 | { 25 | for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++) 26 | { 27 | test_grid.SetIndex(x_index, y_index, z_index, fill_val); 28 | check_vals.push_back(fill_val); 29 | fill_val++; 30 | } 31 | } 32 | } 33 | std::vector buffer; 34 | voxel_grid::VoxelGrid::Serialize( 35 | test_grid, buffer, serialization::SerializeMemcpyable); 36 | const voxel_grid::VoxelGrid read_grid 37 | = voxel_grid::VoxelGrid::Deserialize( 38 | buffer, 0, serialization::DeserializeMemcpyable).Value(); 39 | // Check the values 40 | size_t check_index = 0; 41 | for (int64_t x_index = 0; x_index < read_grid.GetNumXCells(); x_index++) 42 | { 43 | for (int64_t y_index = 0; y_index < read_grid.GetNumYCells(); y_index++) 44 | { 45 | for (int64_t z_index = 0; z_index < read_grid.GetNumZCells(); z_index++) 46 | { 47 | const voxel_grid::GridIndex current(x_index, y_index, z_index); 48 | const int32_t check_val = check_vals.at(check_index); 49 | const int32_t ref_val 50 | = read_grid.GetIndexImmutable(x_index, y_index, z_index).Value(); 51 | const int32_t index_ref_val 52 | = read_grid.GetIndexImmutable(current).Value(); 53 | ASSERT_EQ(ref_val, check_val); 54 | ASSERT_EQ(index_ref_val, check_val); 55 | const int64_t data_index = test_grid.GridIndexToDataIndex(current); 56 | const voxel_grid::GridIndex check_grid_index = 57 | test_grid.DataIndexToGridIndex(data_index); 58 | ASSERT_EQ(current, check_grid_index); 59 | check_index++; 60 | } 61 | } 62 | } 63 | } 64 | 65 | GTEST_TEST(VoxelGridTest, LocationLookup) 66 | { 67 | const voxel_grid::GridSizes grid_sizes(1.0, 20.0, 20.0, 20.0); 68 | voxel_grid::VoxelGrid test_grid(grid_sizes, 0); 69 | // Load with special values 70 | int32_t fill_val = 1; 71 | std::vector check_vals; 72 | for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0) 73 | { 74 | for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0) 75 | { 76 | for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0) 77 | { 78 | test_grid.SetLocation(x_pos, y_pos, z_pos, fill_val); 79 | check_vals.push_back(fill_val); 80 | fill_val++; 81 | } 82 | } 83 | } 84 | std::vector buffer; 85 | voxel_grid::VoxelGrid::Serialize( 86 | test_grid, buffer, serialization::SerializeMemcpyable); 87 | const voxel_grid::VoxelGrid read_grid 88 | = voxel_grid::VoxelGrid::Deserialize( 89 | buffer, 0, serialization::DeserializeMemcpyable).Value(); 90 | // Check the values 91 | size_t check_index = 0; 92 | for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0) 93 | { 94 | for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0) 95 | { 96 | for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0) 97 | { 98 | const int32_t check_val = check_vals.at(check_index); 99 | const int32_t ref_val 100 | = read_grid.GetLocationImmutable(x_pos, y_pos, z_pos).Value(); 101 | const int32_t ref_val_3d 102 | = read_grid.GetLocationImmutable3d( 103 | Eigen::Vector3d(x_pos, y_pos, z_pos)).Value(); 104 | const int32_t ref_val_4d 105 | = read_grid.GetLocationImmutable4d( 106 | Eigen::Vector4d(x_pos, y_pos, z_pos, 1.0)).Value(); 107 | ASSERT_EQ(ref_val, check_val); 108 | ASSERT_EQ(ref_val_3d, check_val); 109 | ASSERT_EQ(ref_val_4d, check_val); 110 | const voxel_grid::GridIndex query_index 111 | = read_grid.LocationToGridIndex(x_pos, y_pos, z_pos); 112 | const voxel_grid::GridIndex query_index_3d 113 | = read_grid.LocationToGridIndex3d( 114 | Eigen::Vector3d(x_pos, y_pos, z_pos)); 115 | const voxel_grid::GridIndex query_index_4d 116 | = read_grid.LocationToGridIndex4d( 117 | Eigen::Vector4d(x_pos, y_pos, z_pos, 1.0)); 118 | ASSERT_EQ(query_index, query_index_3d); 119 | ASSERT_EQ(query_index, query_index_4d); 120 | const Eigen::Vector4d query_location 121 | = read_grid.GridIndexToLocation(query_index); 122 | ASSERT_EQ(x_pos, query_location(0)); 123 | ASSERT_EQ(y_pos, query_location(1)); 124 | ASSERT_EQ(z_pos, query_location(2)); 125 | ASSERT_EQ(1.0, query_location(3)); 126 | const voxel_grid::GridIndex found_query_index 127 | = read_grid.LocationToGridIndex4d(query_location); 128 | ASSERT_EQ(found_query_index, query_index); 129 | check_index++; 130 | } 131 | } 132 | } 133 | } 134 | 135 | GTEST_TEST(VoxelGridTest, DshvgLookup) 136 | { 137 | const voxel_grid::GridSizes chunk_sizes(1.0, 4.0, 4.0, 4.0); 138 | voxel_grid::DynamicSpatialHashedVoxelGrid test_grid( 139 | chunk_sizes, 0, 10); 140 | // Load with special values 141 | int32_t fill_val = 1; 142 | std::vector check_vals; 143 | for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0) 144 | { 145 | for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0) 146 | { 147 | for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0) 148 | { 149 | test_grid.SetLocation( 150 | x_pos, y_pos, z_pos, voxel_grid::DSHVGSetType::SET_CELL, fill_val); 151 | check_vals.push_back(fill_val); 152 | fill_val++; 153 | } 154 | } 155 | } 156 | std::vector buffer; 157 | voxel_grid::DynamicSpatialHashedVoxelGrid::Serialize( 158 | test_grid, buffer, serialization::SerializeMemcpyable); 159 | const voxel_grid::DynamicSpatialHashedVoxelGrid read_grid 160 | = voxel_grid::DynamicSpatialHashedVoxelGrid::Deserialize( 161 | buffer, 0, serialization::DeserializeMemcpyable).Value(); 162 | // Check the values 163 | size_t check_index = 0; 164 | for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0) 165 | { 166 | for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0) 167 | { 168 | for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0) 169 | { 170 | const int32_t check_val = check_vals.at(check_index); 171 | const auto lookup = read_grid.GetLocationImmutable(x_pos, y_pos, z_pos); 172 | const int32_t ref_val = lookup.Value(); 173 | ASSERT_EQ(ref_val, check_val); 174 | ASSERT_EQ(lookup.FoundStatus(), 175 | voxel_grid::DSHVGFoundStatus::FOUND_IN_CELL); 176 | check_index++; 177 | } 178 | } 179 | } 180 | } 181 | } // namespace 182 | } // namespace common_robotics_utilities 183 | 184 | int main(int argc, char** argv) 185 | { 186 | testing::InitGoogleTest(&argc, argv); 187 | return RUN_ALL_TESTS(); 188 | } 189 | --------------------------------------------------------------------------------