From a97e0e39dbdda167cd929244450a4828d9eceb83 Mon Sep 17 00:00:00 2001 From: Calder Phillips-Grafflin Date: Fri, 5 Aug 2022 15:46:24 -0400 Subject: [PATCH 1/2] Clean up/standardize logging and move ROS 2 print support (#54) * Move ROS 2 message print support to ros_helpers.hpp * Remove unnecessary uses of iostream * Remove iostream from zlib helpers * Remove legacy graph and tree connectivity check logging * Remove legacy iostream logging and replace with standard configurable logging functions * Consistently allow null logging functions --- example/clustering_example.cpp | 6 +- .../dynamic_spatial_hashed_voxel_grid.hpp | 1 - .../path_processing.hpp | 5 - include/common_robotics_utilities/print.hpp | 58 +------ .../common_robotics_utilities/ros_helpers.hpp | 35 ++++- .../simple_graph.hpp | 8 - .../simple_hierarchical_clustering.hpp | 1 - .../simple_kmeans_clustering.hpp | 43 ++++-- .../simple_rrt_planner.hpp | 39 ----- .../simple_task_planner.hpp | 37 +---- include/common_robotics_utilities/utility.hpp | 141 +++++++----------- .../common_robotics_utilities/voxel_grid.hpp | 4 +- src/common_robotics_utilities/math.cpp | 4 +- .../zlib_helpers.cpp | 36 ++--- test/planning_test.cpp | 2 +- test/print_test.cpp | 13 +- test/ros_helpers_test.cpp | 3 + test/utility_test.cpp | 26 ++-- 18 files changed, 187 insertions(+), 275 deletions(-) diff --git a/example/clustering_example.cpp b/example/clustering_example.cpp index cfbee0d..1ae6be1 100644 --- a/example/clustering_example.cpp +++ b/example/clustering_example.cpp @@ -167,11 +167,15 @@ int main(int argc, char** argv) { return common_robotics_utilities::math::AverageEigenVector4d(cluster); }; + const auto logging_fn = [] (const std::string& message) + { + std::cout << message << std::endl; + }; const auto kmeans_start = std::chrono::steady_clock::now(); const std::vector kmeans_labels = common_robotics_utilities::simple_kmeans_clustering::Cluster( random_points, distance_fn, average_fn, num_clusters, 42, true, - use_parallel); + use_parallel, logging_fn); const auto kmeans_end = std::chrono::steady_clock::now(); const double kmeans_elapsed = std::chrono::duration(kmeans_end - kmeans_start).count(); diff --git a/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp b/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp index d152494..63aaa05 100644 --- a/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp +++ b/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp @@ -2,7 +2,6 @@ #include #include -#include #include #include #include diff --git a/include/common_robotics_utilities/path_processing.hpp b/include/common_robotics_utilities/path_processing.hpp index 96a9fde..ee0dc24 100644 --- a/include/common_robotics_utilities/path_processing.hpp +++ b/include/common_robotics_utilities/path_processing.hpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include #include @@ -182,10 +181,6 @@ inline Container AttemptShortcut( } return shortcut; } - else - { - std::cerr << "Window size < 2, cannot backtrack" << std::endl; - } } // If we get here, the shortcut failed and we return an empty shortcut path return Container(); diff --git a/include/common_robotics_utilities/print.hpp b/include/common_robotics_utilities/print.hpp index 8428ccb..35d0426 100644 --- a/include/common_robotics_utilities/print.hpp +++ b/include/common_robotics_utilities/print.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -11,58 +10,18 @@ #include #include #include +#include #include #include #include -#include -#include -#if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 -#include -// On pre-Humble distributions, a single message header is included to ensure -// that rosidl_generator_traits::to_yaml is found by the compiler. The version -// header is only present in Humble or later, so we use the absence of the file -// to detect earlier distributions. -#if !__has_include() -#include -#endif -#endif +#include +#include namespace common_robotics_utilities { namespace print { -namespace internal -{ -#if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 -struct ROSMessagePrinter -{ - template - static std::string Print(const T& message) - { -#if !__has_include() - // ROS 2 Galactic only supports block-style output. - return rosidl_generator_traits::to_yaml(message); -#else - // ROS 2 Humble and later support optional flow-style output found via ADL. - constexpr bool use_flow_style = true; - return to_yaml(message, use_flow_style); -#endif - } -}; -#endif -struct GenericPrinter -{ - template - static std::string Print(const T& value) - { - std::ostringstream strm; - strm << value; - return strm.str(); - } -}; -} // namespace internal - // Base template function for printing types template inline std::string Print(const T& toprint, @@ -71,14 +30,9 @@ inline std::string Print(const T& toprint, { CRU_UNUSED(add_delimiters); CRU_UNUSED(separator); -#if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 - using Printer = typename std::conditional< - rosidl_generator_traits::is_message::value, - internal::ROSMessagePrinter, internal::GenericPrinter>::type; -#else - using Printer = internal::GenericPrinter; -#endif - return Printer::Print(toprint); + std::ostringstream strm; + strm << toprint; + return strm.str(); } /////////////////////////////////////////////////////////////////// diff --git a/include/common_robotics_utilities/ros_helpers.hpp b/include/common_robotics_utilities/ros_helpers.hpp index d33bd37..ec54468 100644 --- a/include/common_robotics_utilities/ros_helpers.hpp +++ b/include/common_robotics_utilities/ros_helpers.hpp @@ -1,8 +1,18 @@ #pragma once +#include +#include #include #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 +#include +// On pre-Humble distributions, a single message header is included to ensure +// that rosidl_generator_traits::to_yaml is found by the compiler. The version +// header is only present in Humble or later, so we use the absence of the file +// to detect earlier distributions. +#if !__has_include() +#include +#endif #include #include #include @@ -14,11 +24,31 @@ #error "Undefined or unknown COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION" #endif +#if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 +/// Define an ostream operator for ROS 2 message types. +namespace std +{ +template +enable_if_t::value, ostream>& +operator<<(ostream& os, const T& message) +{ +#if !__has_include() + // ROS 2 Galactic only supports block-style output. + os << rosidl_generator_traits::to_yaml(message); +#else + // ROS 2 Humble and later support optional flow-style output found via ADL. + constexpr bool use_flow_style = false; + os << to_yaml(message, use_flow_style); +#endif + return os; +} +} // namespace std +#endif + namespace common_robotics_utilities { namespace ros_helpers { - #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 using RosTime = rclcpp::Time; #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 @@ -42,7 +72,8 @@ using VisualizationMarker = visualization_msgs::Marker; using VisualizationMarkerArray = visualization_msgs::MarkerArray; #endif -inline void SetMessageTimestamps(VisualizationMarkerArray& markers, const RosTime& timestamp) +inline void SetMessageTimestamps( + VisualizationMarkerArray& markers, const RosTime& timestamp) { SetMessageTimestamps(markers.markers, timestamp); } diff --git a/include/common_robotics_utilities/simple_graph.hpp b/include/common_robotics_utilities/simple_graph.hpp index 63485ed..478f1cd 100644 --- a/include/common_robotics_utilities/simple_graph.hpp +++ b/include/common_robotics_utilities/simple_graph.hpp @@ -334,20 +334,17 @@ bool CheckGraphLinkage(const GraphType& graph) const int64_t from_index = current_edge.GetFromIndex(); if (from_index < 0 || from_index >= graph.Size()) { - std::cerr << "from_index out of bounds" << std::endl; return false; } // Check to index to make sure it matches our own index const int64_t to_index = current_edge.GetToIndex(); if (to_index != idx) { - std::cerr << "to_index does not point to current node" << std::endl; return false; } // Check edge validity (edges to ourself are not allowed) if (from_index == to_index) { - std::cerr << "from_index == to_index not allowed" << std::endl; return false; } // Check to make sure that the from index node is linked to us @@ -365,7 +362,6 @@ bool CheckGraphLinkage(const GraphType& graph) } if (from_node_connection_valid == false) { - std::cerr << "from_index connection is invalid" << std::endl; return false; } } @@ -377,20 +373,17 @@ bool CheckGraphLinkage(const GraphType& graph) const int64_t from_index = current_edge.GetFromIndex(); if (from_index != idx) { - std::cerr << "from_index does not point to current node" << std::endl; return false; } // Check to index to make sure it's in bounds const int64_t to_index = current_edge.GetToIndex(); if (to_index < 0 || to_index >= graph.Size()) { - std::cerr << "To index out of bounds" << std::endl; return false; } // Check edge validity (edges to ourself are not allowed) if (from_index == to_index) { - std::cerr << "From index == to index not allowed" << std::endl; return false; } // Check to make sure that the to index node is linked to us @@ -408,7 +401,6 @@ bool CheckGraphLinkage(const GraphType& graph) } if (to_node_connection_valid == false) { - std::cerr << "To index connection is invalid" << std::endl; return false; } } diff --git a/include/common_robotics_utilities/simple_hierarchical_clustering.hpp b/include/common_robotics_utilities/simple_hierarchical_clustering.hpp index 95995e8..21a8ee0 100644 --- a/include/common_robotics_utilities/simple_hierarchical_clustering.hpp +++ b/include/common_robotics_utilities/simple_hierarchical_clustering.hpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include diff --git a/include/common_robotics_utilities/simple_kmeans_clustering.hpp b/include/common_robotics_utilities/simple_kmeans_clustering.hpp index e97cbbe..944bbac 100644 --- a/include/common_robotics_utilities/simple_kmeans_clustering.hpp +++ b/include/common_robotics_utilities/simple_kmeans_clustering.hpp @@ -7,10 +7,10 @@ #include #include #include -#include #include #include #include +#include #include #include @@ -149,7 +149,8 @@ inline std::vector ClusterWeighted( const std::function&)>& weighted_average_fn, const int32_t num_clusters, const int64_t prng_seed, - const bool do_preliminary_clustering, const bool use_parallel = false) + const bool do_preliminary_clustering, const bool use_parallel = false, + const utility::LoggingFunction& logging_fn = {}) { if (data.empty()) { @@ -157,8 +158,10 @@ inline std::vector ClusterWeighted( } if (num_clusters == 1) { - std::cerr << "[K-means clustering] Provided num_clusters = 1," - << " returning default labels of cluster == 0" << std::endl; + if (logging_fn) + { + logging_fn("[K-means clustering] Provided num_clusters = 1"); + } return std::vector(data.size(), 0u); } else if (num_clusters == 0) @@ -175,16 +178,23 @@ inline std::vector ClusterWeighted( if (subset_size >= (num_clusters * 5)) { enable_preliminary_clustering = true; - std::cout << "[K-means clustering] Preliminary clustering enabled," - << " using subset of " << subset_size << " datapoints from " - << data.size() << " total" << std::endl; + if (logging_fn) + { + logging_fn( + "[K-means clustering] Preliminary clustering enabled, using subset " + "of " + std::to_string(subset_size) + " datapoints from " + + std::to_string(data.size()) + " total"); + } } else { enable_preliminary_clustering = false; - std::cerr << "[K-means clustering] Preliminary clustering disabled as" - << " input data is too small w.r.t. number of clusters" - << std::endl; + if (logging_fn) + { + logging_fn( + "[K-means clustering] Preliminary clustering disabled as input data" + " is too small w.r.t. number of clusters"); + } } } // Prepare an RNG for cluster initialization @@ -282,8 +292,12 @@ inline std::vector ClusterWeighted( converged = CheckForConvergence(cluster_labels, new_cluster_labels); cluster_labels = new_cluster_labels; } - std::cout << "[K-means clustering] Clustering converged after " - << iteration << " iterations" << std::endl; + if (logging_fn) + { + logging_fn( + "[K-means clustering] Clustering converged after " + + std::to_string(iteration) + " iterations"); + } return cluster_labels; } @@ -301,7 +315,8 @@ inline std::vector Cluster( const std::function& distance_fn, const std::function& average_fn, const int32_t num_clusters, const int64_t prng_seed, - const bool do_preliminary_clustering, const bool use_parallel = false) + const bool do_preliminary_clustering, const bool use_parallel = false, + const utility::LoggingFunction& logging_fn = {}) { // Make a dummy set of uniform weights const std::vector data_weights(data.size(), 1.0); @@ -315,7 +330,7 @@ inline std::vector Cluster( }; return ClusterWeighted( data, data_weights, distance_fn, weighted_average_fn, num_clusters, - prng_seed, do_preliminary_clustering, use_parallel); + prng_seed, do_preliminary_clustering, use_parallel, logging_fn); } } // namespace simple_kmeans_clustering } // namespace common_robotics_utilities diff --git a/include/common_robotics_utilities/simple_rrt_planner.hpp b/include/common_robotics_utilities/simple_rrt_planner.hpp index 732f0b6..24c9262 100644 --- a/include/common_robotics_utilities/simple_rrt_planner.hpp +++ b/include/common_robotics_utilities/simple_rrt_planner.hpp @@ -319,18 +319,12 @@ class SimpleRRTPlannerTree const auto& current_state = nodes.at(current_index); if (!current_state.IsInitialized()) { - std::cerr << "Tree contains uninitialized node(s): " - << current_index << std::endl; return false; } // Check the linkage to the parent state const int64_t parent_index = current_state.GetParentIndex(); if (parent_index > static_cast(current_index)) { - std::cerr << "Invalid parent index " << parent_index << " for state " - << current_index - << " [Parent index cannot be greater than our own index]" - << std::endl; return false; } if ((parent_index >= 0) @@ -343,8 +337,6 @@ class SimpleRRTPlannerTree // Make sure the parent state is initialized. if (!parent_state.IsInitialized()) { - std::cerr << "Tree contains uninitialized node(s) " - << parent_index << std::endl; return false; } // Make sure the corresponding parent contains the current node in its @@ -356,26 +348,14 @@ class SimpleRRTPlannerTree static_cast(current_index)); if (index_found == parent_child_indices.end()) { - std::cerr << "Parent state " << parent_index - << " does not contain child index for current node " - << current_index << std::endl; return false; } } else { - std::cerr << "Invalid parent index " << parent_index << " for state " - << current_index << " [Parent index cannot be our own index]" - << std::endl; return false; } } - else if (parent_index < -1) - { - std::cerr << "Invalid parent index " << parent_index << " for state " - << current_index << " [Parent index < -1]" << std::endl; - return false; - } // Check the linkage to the child states const std::vector& current_child_indices = current_state.GetChildIndices(); @@ -390,41 +370,22 @@ class SimpleRRTPlannerTree nodes.at(static_cast(current_child_index)); if (!child_state.IsInitialized()) { - std::cerr << "Tree contains uninitialized node(s) " - << current_child_index << std::endl; return false; } // Make sure the child node points to us as the parent index const int64_t child_parent_index = child_state.GetParentIndex(); if (child_parent_index != static_cast(current_index)) { - std::cerr << "Parent index " << child_parent_index - << " for current child state " << current_child_index - << " does not match index " << current_index - << " for current node " << std::endl; return false; } } - else if (current_child_index == static_cast(current_index)) - { - std::cerr << "Invalid child index " << current_child_index - << " for state " << current_index - << " [Child index cannot be our own index]" << std::endl; - return false; - } else { - std::cerr << "Invalid child index " << current_child_index - << " for state " << current_index - << " [Child index cannot be less than our own index]" - << std::endl; return false; } } else { - std::cerr << "Invalid child index " << current_child_index - << " for state " << current_index << std::endl; return false; } } diff --git a/include/common_robotics_utilities/simple_task_planner.hpp b/include/common_robotics_utilities/simple_task_planner.hpp index 39c77ba..63b5162 100644 --- a/include/common_robotics_utilities/simple_task_planner.hpp +++ b/include/common_robotics_utilities/simple_task_planner.hpp @@ -3,7 +3,6 @@ #include #include #include -#include #include #include #include @@ -16,6 +15,7 @@ #include #include #include +#include namespace common_robotics_utilities { @@ -99,15 +99,6 @@ class ActionPrimitiveWrapper std::string name_; }; -/// Typedef of a logging function used by ActionPrimitiveCollection. -using LoggingFunction = std::function; - -/// Make a LoggingFunction using std::cout. -inline LoggingFunction MakeCoutLoggingFunction() -{ - return [] (const std::string& message) { std::cout << message << std::endl; }; -} - /// Stores a collection of action primitives, enforcing name uniqueness and /// providing useful helpers. template> @@ -115,10 +106,10 @@ class ActionPrimitiveCollection { public: explicit ActionPrimitiveCollection( - const LoggingFunction& logging_fn = MakeCoutLoggingFunction()) + const utility::LoggingFunction& logging_fn = {}) : logging_fn_(logging_fn) {} - void SetLoggingFunction(const LoggingFunction& logging_fn) + void SetLoggingFunction(const utility::LoggingFunction& logging_fn) { logging_fn_ = logging_fn; } @@ -177,7 +168,7 @@ class ActionPrimitiveCollection private: std::vector> primitives_; - LoggingFunction logging_fn_; + utility::LoggingFunction logging_fn_; }; /// Return a heuristic value for the provided state. @@ -461,7 +452,6 @@ Container PerformSingleTaskExecution( const TaskSequenceCompleteFunction& task_sequence_complete_fn, const Container& start_states, const int32_t max_primitive_executions = -1, - const bool single_step = false, const StateHeuristicFunction& state_heuristic_fn = {}, const StateHash& state_hasher = StateHash(), const StateEqual& state_equaler = StateEqual(), @@ -516,18 +506,8 @@ Container PerformSingleTaskExecution( user_pre_action_callback_fn(selected_outcome, next_primitive); } - // Prompt to continue/notify. - if (single_step) - { - std::cout << "Press ENTER to perform primitive [" - << next_primitive->Name() << "]" << std::endl; - std::cin.get(); - } - else - { - primitive_collection.Log( - "Performing primitive [" + next_primitive->Name() + "]"); - } + primitive_collection.Log( + "Performing primitive [" + next_primitive->Name() + "]"); // Execute the primitive. num_primitive_executions++; @@ -551,7 +531,6 @@ Container PerformSingleTaskExecution( const TaskSequenceCompleteFunction& task_sequence_complete_fn, const State& start_state, const int32_t max_primitive_executions = -1, - const bool single_step = false, const StateHeuristicFunction& state_heuristic_fn = {}, const StateHash& state_hasher = StateHash(), const StateEqual& state_equaler = StateEqual(), @@ -568,8 +547,8 @@ Container PerformSingleTaskExecution( return PerformSingleTaskExecution( primitive_collection, task_sequence_complete_fn, start_states, - max_primitive_executions, single_step, state_heuristic_fn, state_hasher, - state_equaler, user_pre_action_callback_fn, user_post_execution_callback, + max_primitive_executions, state_heuristic_fn, state_hasher, state_equaler, + user_pre_action_callback_fn, user_post_execution_callback, user_post_outcome_callback_fn); } } // namespace simple_task_planner diff --git a/include/common_robotics_utilities/utility.hpp b/include/common_robotics_utilities/utility.hpp index ba67179..17db93b 100644 --- a/include/common_robotics_utilities/utility.hpp +++ b/include/common_robotics_utilities/utility.hpp @@ -87,6 +87,15 @@ namespace common_robotics_utilities { namespace utility { +/// Signature of a basic logging function. +using LoggingFunction = std::function; + +/// Make a do-nothing logging function. +inline LoggingFunction NoOpLoggingFunction() +{ + return [] (const std::string&) {}; +} + /// Signature for function that returns a double uniformly sampled from /// the interval [0.0, 1.0). using UniformUnitRealFunction = std::function; @@ -125,36 +134,30 @@ SizeType GetUniformRandomInRange( return start + offset; } -template -inline T ClampValue(const T& val, const T& min, const T& max) -{ - if (max >= min) - { - return std::min(max, std::max(min, val)); - } - else - { - throw std::invalid_argument("min > max"); - } -} - template -inline T ClampValueAndWarn(const T& val, const T& min, const T& max) +inline T ClampValueAndLog(const T& val, const T& min, const T& max, + const LoggingFunction& logging_fn = {}) { if (max >= min) { if (val < min) { - const std::string msg = "Clamping " + std::to_string(val) - + " to min " + std::to_string(min) + "\n"; - std::cerr << msg << std::flush; + if (logging_fn) + { + const std::string msg = "Clamping " + std::to_string(val) + + " to min " + std::to_string(min); + logging_fn(msg); + } return min; } else if (val > max) { - const std::string msg = "Clamping " + std::to_string(val) - + " to max " + std::to_string(max) + "\n"; - std::cerr << msg << std::flush; + if (logging_fn) + { + const std::string msg = "Clamping " + std::to_string(val) + + " to max " + std::to_string(max); + logging_fn(msg); + } return max; } return val; @@ -165,21 +168,42 @@ inline T ClampValueAndWarn(const T& val, const T& min, const T& max) } } +template +inline T ClampValue(const T& val, const T& min, const T& max) +{ + return ClampValueAndLog(val, min, max, {}); +} + // Written to mimic parts of Matlab wthresh(val, 'h', thresh) behavior, // spreading the value to the thresholds instead of setting them to zero // https://www.mathworks.com/help/wavelet/ref/wthresh.html template -inline T SpreadValue(const T& val, const T& low_threshold, const T& midpoint, - const T& high_threshold) +inline T SpreadValueAndLog(const T& val, const T& low_threshold, + const T& midpoint, const T& high_threshold, + const LoggingFunction& logging_fn = {}) { if ((low_threshold <= midpoint) && (midpoint <= high_threshold)) { if (val >= midpoint && val < high_threshold) { + if (logging_fn) + { + const std::string msg = "Thresholding " + std::to_string(val) + + " to high threshold " + + std::to_string(high_threshold); + logging_fn(msg); + } return high_threshold; } else if (val < midpoint && val > low_threshold) { + if (logging_fn) + { + const std::string msg = "Thresholding " + std::to_string(val) + + " to low threshold " + + std::to_string(low_threshold); + logging_fn(msg); + } return low_threshold; } return val; @@ -190,65 +214,40 @@ inline T SpreadValue(const T& val, const T& low_threshold, const T& midpoint, } } -// Written to mimic parts of Matlab wthresh(val, 'h', thresh) behavior, -// spreading the value to the thresholds instead of setting them to zero -// https://www.mathworks.com/help/wavelet/ref/wthresh.html template -inline T SpreadValueAndWarn(const T& val, const T& low_threshold, - const T& midpoint, const T& high_threshold) +inline T SpreadValue(const T& val, const T& low_threshold, const T& midpoint, + const T& high_threshold) { - if ((low_threshold <= midpoint) && (midpoint <= high_threshold)) - { - if (val >= midpoint && val < high_threshold) - { - const std::string msg = "Thresholding " + std::to_string(val) - + " to high threshold " - + std::to_string(high_threshold) + "\n"; - std::cerr << msg << std::flush; - return high_threshold; - } - else if (val < midpoint && val > low_threshold) - { - const std::string msg = "Thresholding " + std::to_string(val) - + " to low threshold " - + std::to_string(low_threshold) + "\n"; - std::cerr << msg << std::flush; - return low_threshold; - } - return val; - } - else - { - throw std::invalid_argument("Invalid thresholds/midpoint"); - } + return SpreadValueAndLog( + val, low_threshold, midpoint, high_threshold, {}); } template inline bool CheckAlignment(const T& item, const uint64_t desired_alignment, - bool verbose=false) + const LoggingFunction& logging_fn = {}) { const T* item_ptr = std::addressof(item); const uintptr_t item_ptr_val = reinterpret_cast(item_ptr); if ((item_ptr_val % desired_alignment) == 0) { - if (verbose) + if (logging_fn) { const std::string msg = "Item @ " + std::to_string(item_ptr_val) + " aligned to " - + std::to_string(desired_alignment) + " bytes\n"; - std::cout << msg << std::flush; + + std::to_string(desired_alignment) + " bytes"; + logging_fn(msg); } return true; } else { - if (verbose) + if (logging_fn) { const std::string msg = "Item @ " + std::to_string(item_ptr_val) + " NOT aligned to " - + std::to_string(desired_alignment) + " bytes\n"; - std::cout << msg << std::flush; + + std::to_string(desired_alignment) + " bytes"; + logging_fn(msg); } return false; } @@ -510,31 +509,5 @@ inline MapLike MakeFromKeysAndValues( throw std::invalid_argument("keys.size() != values.size()"); } } - -inline void ConditionalPrint(const std::string& msg, - const int32_t msg_level, - const int32_t print_level) -{ - if (CRU_UNLIKELY(msg_level <= print_level)) - { - const std::string printstr = "[" + std::to_string(msg_level) + "/" - + std::to_string(print_level) + "] " - + msg + "\n"; - std::cout << printstr << std::flush; - } -} - -inline void ConditionalError(const std::string& msg, - const int32_t msg_level, - const int32_t print_level) -{ - if (CRU_UNLIKELY(msg_level <= print_level)) - { - const std::string printstr = "[" + std::to_string(msg_level) + "/" - + std::to_string(print_level) + "] " - + msg + "\n"; - std::cerr << printstr << std::flush; - } -} } // namespace utility } // namespace common_robotics_utilities diff --git a/include/common_robotics_utilities/voxel_grid.hpp b/include/common_robotics_utilities/voxel_grid.hpp index 038e313..30545f9 100644 --- a/include/common_robotics_utilities/voxel_grid.hpp +++ b/include/common_robotics_utilities/voxel_grid.hpp @@ -2,8 +2,8 @@ #include #include -#include #include +#include #include #include #include @@ -1249,8 +1249,6 @@ class VoxelGridBase } else { - std::cerr << "Failed to load internal data - expected " - << expected_length << " got " << data.size() << std::endl; return false; } } diff --git a/src/common_robotics_utilities/math.cpp b/src/common_robotics_utilities/math.cpp index b036887..0e5b6f5 100644 --- a/src/common_robotics_utilities/math.cpp +++ b/src/common_robotics_utilities/math.cpp @@ -360,14 +360,14 @@ Eigen::Isometry3d ExpTwist(const Eigen::Matrix& twist, } else { + // NOTE: you may encounter numerical instability using ExpTwist(...) with + // translation and rotational norm < 1e-100. if ((trans_velocity_norm >= 1e-100) || (rot_velocity_norm == 0.0)) { raw_transform.block<3, 1>(0, 3) = trans_velocity * delta_t; } else { - std::cerr << "YOU MAY ENCOUNTER NUMERICAL INSTABILITY IN EXPTWIST(...)" - " WITH TRANS & ROT VELOCITY NORM < 1e-100 ***" << std::endl; const double scaled_delta_t = delta_t * rot_velocity_norm; const Eigen::Vector3d scaled_trans_velocity = trans_velocity / rot_velocity_norm; diff --git a/src/common_robotics_utilities/zlib_helpers.cpp b/src/common_robotics_utilities/zlib_helpers.cpp index dd5c543..6c5959c 100644 --- a/src/common_robotics_utilities/zlib_helpers.cpp +++ b/src/common_robotics_utilities/zlib_helpers.cpp @@ -4,7 +4,6 @@ #include #include -#include #include #include #include @@ -28,9 +27,9 @@ std::vector DecompressBytes(const std::vector& compressed) int ret = inflateInit(&strm); if (ret != Z_OK) { - (void)inflateEnd(&strm); - std::cerr << "ZLIB can't init inflate stream ret=" << ret << std::endl; - throw std::invalid_argument("ZLIB can't init inflate stream"); + inflateEnd(&strm); + throw std::runtime_error( + "ZLIB can't init inflate stream (ret = " + std::to_string(ret) + ")"); } strm.avail_in = static_cast(compressed.size()); strm.next_in = const_cast(reinterpret_cast( @@ -49,11 +48,11 @@ std::vector DecompressBytes(const std::vector& compressed) while (ret == Z_OK); if (ret != Z_STREAM_END) { - (void)inflateEnd(&strm); - std::cerr << "ZLIB can't inflate stream ret=" << ret << std::endl; - throw std::invalid_argument("ZLIB can't inflate stream"); + inflateEnd(&strm); + throw std::runtime_error( + "ZLIB can't inflate stream (ret = " + std::to_string(ret) + ")"); } - (void)inflateEnd(&strm); + inflateEnd(&strm); std::vector decompressed(buffer); return decompressed; } @@ -82,18 +81,18 @@ std::vector CompressBytes(const std::vector& uncompressed) int ret = deflateInit(&strm, Z_BEST_SPEED); if (ret != Z_OK) { - (void)deflateEnd(&strm); - std::cerr << "ZLIB can't init deflate stream ret=" << ret << std::endl; - throw std::invalid_argument("ZLIB can't init deflate stream"); + deflateEnd(&strm); + throw std::runtime_error( + "ZLIB can't init deflate stream (ret = " + std::to_string(ret) + ")"); } while (strm.avail_in != 0) { ret = deflate(&strm, Z_NO_FLUSH); if (ret != Z_OK) { - (void)deflateEnd(&strm); - std::cerr << "ZLIB can't deflate stream ret=" << ret << std::endl; - throw std::invalid_argument("ZLIB can't deflate stream"); + deflateEnd(&strm); + throw std::runtime_error( + "ZLIB can't deflate stream (ret = " + std::to_string(ret) + ")"); } if (strm.avail_out == 0) { @@ -115,13 +114,14 @@ std::vector CompressBytes(const std::vector& uncompressed) } if (deflate_ret != Z_STREAM_END) { - (void)deflateEnd(&strm); - std::cerr << "ZLIB can't deflate stream ret=" << deflate_ret << std::endl; - throw std::invalid_argument("ZLIB can't deflate stream"); + deflateEnd(&strm); + throw std::runtime_error( + "ZLIB can't deflate stream (ret = " + std::to_string(deflate_ret) + + ")"); } buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE - strm.avail_out); - (void)deflateEnd(&strm); + deflateEnd(&strm); std::vector compressed(buffer); return compressed; } diff --git a/test/planning_test.cpp b/test/planning_test.cpp index 6d629f7..3e85d64 100644 --- a/test/planning_test.cpp +++ b/test/planning_test.cpp @@ -69,7 +69,7 @@ double WaypointDistance(const Waypoint& start, const Waypoint& end) Waypoint InterpolateWaypoint( const Waypoint& start, const Waypoint& end, const double ratio) { - const double real_ratio = utility::ClampValueAndWarn(ratio, 0.0, 1.0); + const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0); const double delta_rows = static_cast(end.first - start.first); const double delta_cols = static_cast(end.second - start.second); const double raw_interp_rows = delta_rows * real_ratio; diff --git a/test/print_test.cpp b/test/print_test.cpp index 60a83af..38deb29 100644 --- a/test/print_test.cpp +++ b/test/print_test.cpp @@ -1,6 +1,10 @@ #include +#include +#include + #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2 +#include #include #elif COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 1 #include @@ -8,9 +12,6 @@ #error "Undefined or unknown COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION" #endif -#include -#include - #include namespace @@ -22,8 +23,7 @@ struct DummyType int32_t value = 0; }; -std::ostream& operator<<( - std::ostream& os, const test::DummyType& dummy) +std::ostream& operator<<(std::ostream& os, const test::DummyType& dummy) { return os << dummy.value; } @@ -36,7 +36,8 @@ namespace print_test { GTEST_TEST(PrintTest, CanPrintIfADLCanFindStreamOperator) { - std::cout << print::Print(std::vector{{1, 1, 1}}) << std::endl; + std::cout << print::Print(std::vector{{1, 1, 1}}) + << std::endl; std::cout << print::Print(std::vector(3)) << std::endl; } diff --git a/test/ros_helpers_test.cpp b/test/ros_helpers_test.cpp index a140c46..66cd1e5 100644 --- a/test/ros_helpers_test.cpp +++ b/test/ros_helpers_test.cpp @@ -31,6 +31,9 @@ GTEST_TEST(ROSHelpersTest, SetMessageTimestamps) for (const auto& marker : marker_array.markers) { EXPECT_EQ(marker.header.stamp, expected_stamp); } + // Test that ROS messages have an ostream operator available. + std::cout << "Set message times to:\n" + << marker_array.markers.at(0).header.stamp << std::endl; } } // namespace diff --git a/test/utility_test.cpp b/test/utility_test.cpp index 094d2bd..255f482 100644 --- a/test/utility_test.cpp +++ b/test/utility_test.cpp @@ -54,35 +54,43 @@ GTEST_TEST(UtilityTest, GetUniformRandom) GTEST_TEST(UtilityTest, ClampAndSpread) { + const utility::LoggingFunction logging_fn = [] (const std::string& msg) + { + std::cout << msg << std::endl; + }; ASSERT_EQ(utility::ClampValue(1.0, -0.5, 0.5), 0.5); - ASSERT_EQ(utility::ClampValueAndWarn(1.0, -0.5, 0.5), 0.5); + ASSERT_EQ(utility::ClampValueAndLog(1.0, -0.5, 0.5, logging_fn), 0.5); ASSERT_EQ(utility::ClampValue(-1.0, -0.5, 0.5), -0.5); - ASSERT_EQ(utility::ClampValueAndWarn(-1.0, -0.5, 0.5), -0.5); + ASSERT_EQ(utility::ClampValueAndLog(-1.0, -0.5, 0.5, logging_fn), -0.5); ASSERT_EQ(utility::ClampValue(0.25, -0.5, 0.5), 0.25); - ASSERT_EQ(utility::ClampValueAndWarn(0.25, -0.5, 0.5), 0.25); + ASSERT_EQ(utility::ClampValueAndLog(0.25, -0.5, 0.5, logging_fn), 0.25); ASSERT_THROW(utility::ClampValue(1.0, 1.0, -1.0), std::invalid_argument); ASSERT_THROW( - utility::ClampValueAndWarn(1.0, 1.0, -1.0), std::invalid_argument); + utility::ClampValueAndLog(1.0, 1.0, -1.0, logging_fn), + std::invalid_argument); ASSERT_EQ(utility::SpreadValue(1.0, -0.5, 0.0, 0.5), 1.0); - ASSERT_EQ(utility::SpreadValueAndWarn(1.0, -0.5, 0.0, 0.5), 1.0); + ASSERT_EQ(utility::SpreadValueAndLog(1.0, -0.5, 0.0, 0.5, logging_fn), 1.0); ASSERT_EQ(utility::SpreadValue(-1.0, -0.5, 0.0, 0.5), -1.0); - ASSERT_EQ(utility::SpreadValueAndWarn(-1.0, -0.5, 0.0, 0.5), -1.0); + ASSERT_EQ( + utility::SpreadValueAndLog(-1.0, -0.5, 0.0, 0.5, logging_fn), -1.0); ASSERT_EQ(utility::SpreadValue(0.25, -0.5, 0.0, 0.5), 0.5); - ASSERT_EQ(utility::SpreadValueAndWarn(0.25, -0.5, 0.0, 0.5), 0.5); + ASSERT_EQ(utility::SpreadValueAndLog(0.25, -0.5, 0.0, 0.5, logging_fn), 0.5); ASSERT_EQ(utility::SpreadValue(-0.25, -0.5, 0.0, 0.5), -0.5); - ASSERT_EQ(utility::SpreadValueAndWarn(-0.25, -0.5, 0.0, 0.5), -0.5); + ASSERT_EQ( + utility::SpreadValueAndLog(-0.25, -0.5, 0.0, 0.5, logging_fn), -0.5); ASSERT_THROW( utility::SpreadValue(1.0, 1.0, 0.0, -1.0), std::invalid_argument); ASSERT_THROW( - utility::SpreadValueAndWarn(1.0, 1.0, 0.0, -1.0), std::invalid_argument); + utility::SpreadValueAndLog(1.0, 1.0, 0.0, -1.0, logging_fn), + std::invalid_argument); } GTEST_TEST(UtilityTest, Alignment) From 12806cc3c6a5691c319157b6a4cef9b762144880 Mon Sep 17 00:00:00 2001 From: Calder Phillips-Grafflin Date: Sat, 6 Aug 2022 04:56:29 -0400 Subject: [PATCH 2/2] Replace raw omp parallel for pragmas with macros (#55) --- include/common_robotics_utilities/math.hpp | 18 +++---------- .../openmp_helpers.hpp | 14 +++++++++- .../simple_graph.hpp | 11 ++------ .../simple_hausdorff_distance.hpp | 12 ++------- .../simple_hierarchical_clustering.hpp | 12 ++------- .../simple_kmeans_clustering.hpp | 17 +++--------- .../simple_knearest_neighbors.hpp | 12 ++------- .../simple_prm_planner.hpp | 27 ++++--------------- 8 files changed, 32 insertions(+), 91 deletions(-) diff --git a/include/common_robotics_utilities/math.hpp b/include/common_robotics_utilities/math.hpp index 893d33b..bc1ea6b 100644 --- a/include/common_robotics_utilities/math.hpp +++ b/include/common_robotics_utilities/math.hpp @@ -1,16 +1,12 @@ #pragma once -#if defined(_OPENMP) -#include -#endif - #include #include #include #include #include -#include +#include namespace common_robotics_utilities { @@ -539,11 +535,7 @@ Eigen::MatrixXd BuildPairwiseDistanceMatrix( { Eigen::MatrixXd distance_matrix(data.size(), data.size()); -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel) -#else - CRU_UNUSED(use_parallel); -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel) for (size_t idx = 0; idx < data.size(); idx++) { for (size_t jdx = idx; jdx < data.size(); jdx++) @@ -597,11 +589,7 @@ Eigen::MatrixXd BuildPairwiseDistanceMatrix( { Eigen::MatrixXd distance_matrix(data1.size(), data2.size()); -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel) -#else - CRU_UNUSED(use_parallel); -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel) for (size_t idx = 0; idx < data1.size(); idx++) { for (size_t jdx = 0; jdx < data2.size(); jdx++) diff --git a/include/common_robotics_utilities/openmp_helpers.hpp b/include/common_robotics_utilities/openmp_helpers.hpp index 3b1cb36..e840554 100644 --- a/include/common_robotics_utilities/openmp_helpers.hpp +++ b/include/common_robotics_utilities/openmp_helpers.hpp @@ -125,7 +125,7 @@ inline int32_t GetOmpThreadLimit() class ChangeOmpNumThreadsWrapper { public: - ChangeOmpNumThreadsWrapper(const int32_t num_threads) + explicit ChangeOmpNumThreadsWrapper(const int32_t num_threads) { if (GetContextNumOmpThreads() > 1) { @@ -164,6 +164,18 @@ class DisableOmpWrapper : public ChangeOmpNumThreadsWrapper DisableOmpWrapper() : ChangeOmpNumThreadsWrapper(1) {} }; +/// Macro to stringify tokens for the purposes of the below macros. +#define CRU_MACRO_STRINGIFY(s) #s + +/// Macros to declare OpenMP parallel for loops, handling conditionals as well +/// as the case of OpenMP being disabled entirely. +#if defined(_OPENMP) +#define CRU_OMP_PARALLEL_FOR_IF(enable_parallel) _Pragma(CRU_MACRO_STRINGIFY(omp parallel for if(enable_parallel))) +#define CRU_OMP_PARALLEL_FOR _Pragma(CRU_MACRO_STRINGIFY(omp parallel for)) +#else +#define CRU_OMP_PARALLEL_FOR_IF(enable_parallel) (void)(enable_parallel); +#define CRU_OMP_PARALLEL_FOR +#endif } // namespace openmp_helpers } // namespace common_robotics_utilities diff --git a/include/common_robotics_utilities/simple_graph.hpp b/include/common_robotics_utilities/simple_graph.hpp index 478f1cd..42f8905 100644 --- a/include/common_robotics_utilities/simple_graph.hpp +++ b/include/common_robotics_utilities/simple_graph.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(_OPENMP) -#include -#endif - #include #include #include @@ -13,6 +9,7 @@ #include #include +#include #include #include #include @@ -437,11 +434,7 @@ OutputGraphType PruneGraph( pruned_graph.ShrinkToFit(); // Second, optionally parallel pass to update edges for the kept nodes -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel) -#else - CRU_UNUSED(use_parallel); -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel) for (int64_t kept_node_index = 0; kept_node_index < pruned_graph.Size(); kept_node_index++) { diff --git a/include/common_robotics_utilities/simple_hausdorff_distance.hpp b/include/common_robotics_utilities/simple_hausdorff_distance.hpp index 740289d..249d5ed 100644 --- a/include/common_robotics_utilities/simple_hausdorff_distance.hpp +++ b/include/common_robotics_utilities/simple_hausdorff_distance.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(_OPENMP) -#include -#endif - #include #include #include @@ -55,9 +51,7 @@ inline double ComputeDistanceParallel( // Make per-thread storage std::vector per_thread_storage( openmp_helpers::GetNumOmpThreads(), 0.0); -#if defined(_OPENMP) -#pragma omp parallel for -#endif + CRU_OMP_PARALLEL_FOR for (size_t idx = 0; idx < outer_distribution.size(); idx++) { const FirstDatatype& first = outer_distribution[idx]; @@ -168,9 +162,7 @@ inline double ComputeDistanceParallel( // Make per-thread storage std::vector per_thread_storage( openmp_helpers::GetNumOmpThreads(), 0.0); -#if defined(_OPENMP) -#pragma omp parallel for -#endif + CRU_OMP_PARALLEL_FOR for (size_t idx = 0; idx < outer_distribution.size(); idx++) { double minimum_distance = std::numeric_limits::infinity(); diff --git a/include/common_robotics_utilities/simple_hierarchical_clustering.hpp b/include/common_robotics_utilities/simple_hierarchical_clustering.hpp index 21a8ee0..b60527a 100644 --- a/include/common_robotics_utilities/simple_hierarchical_clustering.hpp +++ b/include/common_robotics_utilities/simple_hierarchical_clustering.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(_OPENMP) -#include -#endif - #include #include #include @@ -125,9 +121,7 @@ inline ClosestPair GetClosestClustersParallel( { std::vector per_thread_closest_clusters( static_cast(openmp_helpers::GetNumOmpThreads()), ClosestPair()); -#if defined(_OPENMP) -#pragma omp parallel for -#endif + CRU_OMP_PARALLEL_FOR for (size_t first_cluster_idx = 0; first_cluster_idx < clusters.size(); first_cluster_idx++) { @@ -279,9 +273,7 @@ inline ClosestPair GetClosestValueToOtherParallel( { std::vector per_thread_closest_value_other( static_cast(openmp_helpers::GetNumOmpThreads()), ClosestPair()); -#if defined(_OPENMP) -#pragma omp parallel for -#endif + CRU_OMP_PARALLEL_FOR for (size_t value_idx = 0; value_idx < datapoint_mask.size(); value_idx++) { // Make sure we're not already clustered diff --git a/include/common_robotics_utilities/simple_kmeans_clustering.hpp b/include/common_robotics_utilities/simple_kmeans_clustering.hpp index 944bbac..cfaa2c7 100644 --- a/include/common_robotics_utilities/simple_kmeans_clustering.hpp +++ b/include/common_robotics_utilities/simple_kmeans_clustering.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(_OPENMP) -#include -#endif - #include #include #include @@ -14,6 +10,7 @@ #include #include +#include #include #include @@ -35,11 +32,7 @@ inline std::vector PerformSingleClusteringIteration( { std::vector new_cluster_labels(data.size()); -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel) -#else - CRU_UNUSED(use_parallel); -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel) for (size_t idx = 0; idx < data.size(); idx++) { const DataType& datapoint = data.at(idx); @@ -88,11 +81,7 @@ inline Container ComputeClusterCentersWeighted( // Compute the center of each cluster Container cluster_centers(static_cast(num_clusters)); -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel) -#else - CRU_UNUSED(use_parallel); -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel) for (size_t cluster = 0; cluster < clustered_data.size(); cluster++) { const Container& cluster_data = clustered_data.at(cluster); diff --git a/include/common_robotics_utilities/simple_knearest_neighbors.hpp b/include/common_robotics_utilities/simple_knearest_neighbors.hpp index 3cd0404..773b6e1 100644 --- a/include/common_robotics_utilities/simple_knearest_neighbors.hpp +++ b/include/common_robotics_utilities/simple_knearest_neighbors.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(_OPENMP) -#include -#endif - #include #include #include @@ -81,9 +77,7 @@ inline std::vector GetKNearestNeighborsParallel( else if (items.size() <= static_cast(K)) { std::vector k_nearests(items.size()); -#if defined(_OPENMP) -#pragma omp parallel for -#endif + CRU_OMP_PARALLEL_FOR for (size_t idx = 0; idx < items.size(); idx++) { const Item& item = items[idx]; @@ -97,9 +91,7 @@ inline std::vector GetKNearestNeighborsParallel( std::vector> per_thread_nearests( static_cast(openmp_helpers::GetNumOmpThreads()), std::vector(static_cast(K))); -#if defined(_OPENMP) -#pragma omp parallel for -#endif + CRU_OMP_PARALLEL_FOR for (size_t idx = 0; idx < items.size(); idx++) { const Item& item = items[idx]; diff --git a/include/common_robotics_utilities/simple_prm_planner.hpp b/include/common_robotics_utilities/simple_prm_planner.hpp index 00e8f60..86dfa26 100644 --- a/include/common_robotics_utilities/simple_prm_planner.hpp +++ b/include/common_robotics_utilities/simple_prm_planner.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(_OPENMP) -#include -#endif - #include #include #include @@ -14,6 +10,7 @@ #include #include +#include #include #include #include @@ -110,11 +107,7 @@ inline int64_t AddNodeToRoadmap( std::vector> nearest_neighbors_distances( nearest_neighbors.size()); -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel) -#else - CRU_UNUSED(use_parallel); -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel) for (size_t idx = 0; idx < nearest_neighbors.size(); idx++) { const auto& nearest_neighbor = nearest_neighbors.at(idx); @@ -369,11 +362,7 @@ GraphType BuildRoadMap( // add_duplicate_states is true, since the check for duplicate states would // be a race condition otherwise. const bool use_parallel_sampling = use_parallel && add_duplicate_states; -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel_sampling) -#else - CRU_UNUSED(use_parallel_sampling); -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel_sampling) for (size_t index = 0; index < roadmap_states.size(); index++) { roadmap_states.at(index) = valid_sample_fn(); @@ -403,9 +392,7 @@ GraphType BuildRoadMap( // Perform edge validity and distance checks for all nodes, optionally in // parallel. -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel) -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel) for (int64_t node_index = 0; node_index < roadmap.Size(); ++node_index) { auto& node = roadmap.GetNodeMutable(node_index); @@ -528,11 +515,7 @@ inline void UpdateRoadMapEdges( throw std::invalid_argument("Provided roadmap has invalid linkage"); } -#if defined(_OPENMP) -#pragma omp parallel for if (use_parallel) -#else - CRU_UNUSED(use_parallel); -#endif + CRU_OMP_PARALLEL_FOR_IF(use_parallel) for (int64_t current_node_index = 0; current_node_index < roadmap.Size(); current_node_index++) {