From 07d44aa44af4df6e9941ebaf470d0368dea3da39 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Sun, 6 Oct 2024 22:26:47 +0200
Subject: [PATCH] costmaps for rviz API

---
 CMakeLists.txt                                       |  1 +
 mrpt_path_planning/CMakeLists.txt                    |  2 +-
 mrpt_path_planning/include/mpp/algos/CostEvaluator.h | 10 ++++++++++
 mrpt_path_planning/src/algos/CostEvaluator.cpp       |  8 ++++++++
 4 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b0b8e6b..d930d72 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -43,6 +43,7 @@ find_package(mrpt-containers REQUIRED)
 find_package(mrpt-graphs REQUIRED)
 find_package(mrpt-gui REQUIRED)
 find_package(mrpt-nav REQUIRED)
+find_package(mrpt-maps REQUIRED)
 
 # Targets:
 add_subdirectory(mrpt_path_planning)
diff --git a/mrpt_path_planning/CMakeLists.txt b/mrpt_path_planning/CMakeLists.txt
index 8dd76c8..a465d69 100644
--- a/mrpt_path_planning/CMakeLists.txt
+++ b/mrpt_path_planning/CMakeLists.txt
@@ -10,7 +10,7 @@ selfdriving_add_library(
 	SOURCES
 		${LIB_SRCS} ${LIB_PUBLIC_HDRS}
 	PUBLIC_LINK_LIBRARIES 
-		mrpt::nav mrpt::graphs mrpt::containers mrpt::gui
+		mrpt::nav mrpt::graphs mrpt::containers mrpt::gui mrpt::maps
 #	[PRIVATE_LINK_LIBRARIES lib3 lib4]
 	CMAKE_DEPENDENCIES 
 		mrpt-containers mrpt-graphs mrpt-gui mrpt-nav
diff --git a/mrpt_path_planning/include/mpp/algos/CostEvaluator.h b/mrpt_path_planning/include/mpp/algos/CostEvaluator.h
index 448cfe8..7f9026b 100644
--- a/mrpt_path_planning/include/mpp/algos/CostEvaluator.h
+++ b/mrpt_path_planning/include/mpp/algos/CostEvaluator.h
@@ -11,6 +11,12 @@
 #include <mrpt/rtti/CObject.h>
 #include <mrpt/version.h>
 
+// fwd decl:
+namespace mrpt::maps
+{
+class COccupancyGridMap2D;
+}
+
 namespace mpp
 {
 class CostEvaluator : public mrpt::rtti::CObject
@@ -30,6 +36,10 @@ class CostEvaluator : public mrpt::rtti::CObject
 
     // Default: empty viz
     virtual mrpt::opengl::CSetOfObjects::Ptr get_visualization() const;
+
+    // Default: empty grid. Used mostly for ROS visualization interface
+    virtual std::shared_ptr<mrpt::maps::COccupancyGridMap2D>
+        get_visualization_as_grid() const;
 };
 
 }  // namespace mpp
diff --git a/mrpt_path_planning/src/algos/CostEvaluator.cpp b/mrpt_path_planning/src/algos/CostEvaluator.cpp
index 0888a1a..da24ce3 100644
--- a/mrpt_path_planning/src/algos/CostEvaluator.cpp
+++ b/mrpt_path_planning/src/algos/CostEvaluator.cpp
@@ -5,6 +5,7 @@
  * ------------------------------------------------------------------------- */
 
 #include <mpp/algos/CostEvaluator.h>
+#include <mrpt/maps/COccupancyGridMap2D.h>
 
 using namespace mpp;
 
@@ -19,3 +20,10 @@ mrpt::opengl::CSetOfObjects::Ptr CostEvaluator::get_visualization() const
     glObj->setName("CostEvaluator.default");
     return glObj;
 }
+
+std::shared_ptr<mrpt::maps::COccupancyGridMap2D>
+    mpp::CostEvaluator::get_visualization_as_grid() const
+{
+    auto grid = mrpt::maps::COccupancyGridMap2D::Create();
+    return grid;
+}