costmaps for rviz API

MRPT · Oct 6, 2024 · 07d44aa · 07d44aa
1 parent bfbcd10
commit 07d44aa
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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
@@ -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;
+}