Merge branch 'calderpg:master' into master

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<int32_t> 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<double>(kmeans_end - kmeans_start).count();

include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp

@@ -2,7 +2,6 @@
 
 #include <cmath>
 #include <cstdint>
-#include <iostream>
 #include <memory>
 #include <stdexcept>
 #include <string>

include/common_robotics_utilities/math.hpp

@@ -1,16 +1,12 @@
 #pragma once
 
-#if defined(_OPENMP)
-#include <omp.h>
-#endif
-
 #include <cmath>
 #include <functional>
 #include <map>
 #include <vector>
 
 #include <Eigen/Geometry>
-#include <common_robotics_utilities/utility.hpp>
+#include <common_robotics_utilities/openmp_helpers.hpp>
 
 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++)

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
 

include/common_robotics_utilities/path_processing.hpp

@@ -4,7 +4,6 @@
 #include <cmath>
 #include <cstdint>
 #include <functional>
-#include <iostream>
 #include <random>
 #include <stdexcept>
 #include <vector>
@@ -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();

include/common_robotics_utilities/print.hpp

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <iostream>
 #include <iomanip>
 #include <array>
 #include <vector>
@@ -11,58 +10,18 @@
 #include <set>
 #include <unordered_map>
 #include <unordered_set>
+#include <sstream>
 #include <string>
 #include <type_traits>
 #include <utility>
-#include <common_robotics_utilities/utility.hpp>
-#include <Eigen/Geometry>
 
-#if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2
-#include <rosidl_runtime_cpp/traits.hpp>
-// 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(<rclcpp/version.h>)
-#include <builtin_interfaces/msg/duration.hpp>
-#endif
-#endif
+#include <Eigen/Geometry>
+#include <common_robotics_utilities/utility.hpp>
 
 namespace common_robotics_utilities
 {
 namespace print
 {
-namespace internal
-{
-#if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2
-struct ROSMessagePrinter
-{
-  template <typename T>
-  static std::string Print(const T& message)
-  {
-#if !__has_include(<rclcpp/version.h>)
-    // 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 <typename T>
-  static std::string Print(const T& value)
-  {
-    std::ostringstream strm;
-    strm << value;
-    return strm.str();
-  }
-};
-}  // namespace internal
-
 // Base template function for printing types
 template <typename T>
 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<T>::value,
-      internal::ROSMessagePrinter, internal::GenericPrinter>::type;
-#else
-  using Printer = internal::GenericPrinter;
-#endif
-  return Printer::Print(toprint);
+  std::ostringstream strm;
+  strm << toprint;
+  return strm.str();
 }
 
 ///////////////////////////////////////////////////////////////////

include/common_robotics_utilities/ros_helpers.hpp

@@ -1,8 +1,18 @@
 #pragma once
 
+#include <ostream>
+#include <type_traits>
 #include <vector>
 
 #if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2
+#include <rosidl_runtime_cpp/traits.hpp>
+// 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(<rclcpp/version.h>)
+#include <builtin_interfaces/msg/duration.hpp>
+#endif
 #include <rclcpp/time.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
@@ -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 <typename T>
+enable_if_t<rosidl_generator_traits::is_message<T>::value, ostream>&
+operator<<(ostream& os, const T& message)
+{
+#if !__has_include(<rclcpp/version.h>)
+  // 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<VisualizationMarker>(markers.markers, timestamp);
 }

include/common_robotics_utilities/simple_graph.hpp

@@ -1,9 +1,5 @@
 #pragma once
 
-#if defined(_OPENMP)
-#include <omp.h>
-#endif
-
 #include <algorithm>
 #include <cstdint>
 #include <memory>
@@ -13,6 +9,7 @@
 #include <vector>
 
 #include <Eigen/Geometry>
+#include <common_robotics_utilities/openmp_helpers.hpp>
 #include <common_robotics_utilities/print.hpp>
 #include <common_robotics_utilities/serialization.hpp>
 #include <common_robotics_utilities/utility.hpp>
@@ -334,20 +331,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 +359,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 +370,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 +398,6 @@ bool CheckGraphLinkage(const GraphType& graph)
       }
       if (to_node_connection_valid == false)
       {
-        std::cerr << "To index connection is invalid" << std::endl;
         return false;
       }
     }
@@ -445,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++)
   {

include/common_robotics_utilities/simple_hausdorff_distance.hpp

@@ -1,9 +1,5 @@
 #pragma once
 
-#if defined(_OPENMP)
-#include <omp.h>
-#endif
-
 #include <cstdint>
 #include <functional>
 #include <limits>
@@ -55,9 +51,7 @@ inline double ComputeDistanceParallel(
   // Make per-thread storage
   std::vector<double> 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<double> 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<double>::infinity();