Fix for different ceres isinf() API (#3471)

ros-navigation · Mar 14, 2023 · b499116 · b499116
1 parent b66b17c
commit b499116
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 2 deletions.
diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt
@@ -17,6 +17,10 @@ find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
 
 set(CMAKE_CXX_STANDARD 17)
 
+if(${CERES_VERSION} VERSION_LESS_EQUAL 2.0.0)
+ add_definitions(-DUSE_OLD_CERES_API)
+endif()
+
 nav2_package()
 
 set(library_name nav2_constrained_smoother)

diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp
@@ -159,7 +159,7 @@ class SmootherCostFunction
  Eigen::Matrix<T, 2, 1> center = arcCenter(
  pt_prev, pt, pt_next,
  next_to_last_length_ratio_ < 0);
- if (ceres::isinf(center[0])) {
+ if (CERES_ISINF(center[0])) {
  return;
  }
  T turning_rad = (pt - center).norm();

diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp
@@ -20,6 +20,16 @@
 
 #define EPSILON 0.0001
 
+/**
+ * Compatibility with different ceres::isinf() and ceres::IsInfinite() API
+ * used in Ceres Solver 2.1.0+ and 2.0.0- versions respectively
+ */
+#if defined(USE_OLD_CERES_API)
+ #define CERES_ISINF(x) ceres::IsInfinite(x)
+#else
+ #define CERES_ISINF(x) ceres::isinf(x)
+#endif
+
 namespace nav2_constrained_smoother
 {
 
@@ -86,7 +96,7 @@ inline Eigen::Matrix<T, 2, 1> tangentDir(
  bool is_cusp)
 {
  Eigen::Matrix<T, 2, 1> center = arcCenter(pt_prev, pt, pt_next, is_cusp);
- if (ceres::isinf(center[0])) { // straight line
+ if (CERES_ISINF(center[0])) { // straight line
  Eigen::Matrix<T, 2, 1> d1 = pt - pt_prev;
  Eigen::Matrix<T, 2, 1> d2 = pt_next - pt;