From d37d5a670ac7dc2234b904681eea346c78a6fba9 Mon Sep 17 00:00:00 2001
From: Ben Wolsieffer <benwolsieffer@gmail.com>
Date: Thu, 5 Oct 2023 21:50:39 -0400
Subject: [PATCH] Fix teb-local-planner.

Add OpenBLAS support to libg2o and add patch for teb-local-planner.
---
 distros/ros1-overlay.nix | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/distros/ros1-overlay.nix b/distros/ros1-overlay.nix
index eb2591c58f..9e2e78ae9e 100644
--- a/distros/ros1-overlay.nix
+++ b/distros/ros1-overlay.nix
@@ -90,6 +90,13 @@ rosSelf: rosSuper: with rosSelf.lib; {
     sha256 = "0lpaskqxpklm05050wwvdqwhw30f2hpzss8sgyvczdpvvqzjg4vk";
   };
 
+  libg2o = rosSuper.libg2o.overrideAttrs ({
+    nativeBuildInputs ? [], ...
+  }: {
+    # Missing from package.xml; propagated by other dependencies in Ubuntu
+    nativeBuildInputs = nativeBuildInputs ++ [ self.openblas ];
+  });
+
   map-server = rosSuper.map-server.overrideAttrs ({
     nativeBuildInputs ? [], ...
   }: {
@@ -195,4 +202,21 @@ rosSelf: rosSuper: with rosSelf.lib; {
   }: {
     CXXFLAGS = CXXFLAGS + " -DACCEPT_USE_OF_DEPRECATED_PROJ_API_H";
   });
+
+  teb-local-planner = rosSuper.teb-local-planner.overrideAttrs ({
+    patches ? [], cmakeFlags ? [], ...
+  }: {
+    patches = patches ++ [
+      # fix: undefined reference to `int boost::math::sign<double>(double const&)'
+      # https://github.com/rst-tu-dortmund/teb_local_planner/pull/413
+      (self.fetchpatch {
+        url = "https://github.com/rst-tu-dortmund/teb_local_planner/commit/c6e0990105811cba87747e87ec878d6610ee2ac5.patch";
+        hash = "sha256-td4yejakcXUH6b6wUa2d85R4dyegc5pbZLDEzTTz/x8=";
+      })
+    ];
+    # FindSUITESPARSE.cmake uses find_path() to search for the directory
+    # containing libcholmod.so, but this is primarily designed to find include
+    # directories and doesn't search ${CMAKE_PREFIX_PATH}/lib.
+    cmakeFlags = cmakeFlags ++ [ "-DSUITESPARSE_LIBRARY_DIR=${self.suitesparse}/lib" ];
+  });
 }