costmaps for rviz API

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)

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

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

mrpt_path_planning/include/mpp/algos/CostEvaluatorCostMap.h

@@ -60,6 +60,10 @@ class CostEvaluatorCostMap : public CostEvaluator
 
     mrpt::opengl::CSetOfObjects::Ptr get_visualization() const override;
 
+    // Used for ROS visualization interface
+    std::shared_ptr<mrpt::maps::COccupancyGridMap2D> get_visualization_as_grid()
+        const override;
+
     using cost_gridmap_t = mrpt::containers::CDynamicGrid<double>;
 
     const cost_gridmap_t cost_gridmap() const { return costmap_; }

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;
+}

mrpt_path_planning/src/algos/CostEvaluatorCostMap.cpp

@@ -6,6 +6,7 @@
 
 #include <mpp/algos/CostEvaluatorCostMap.h>
 #include <mrpt/img/color_maps.h>
+#include <mrpt/maps/COccupancyGridMap2D.h>
 #include <mrpt/opengl/CTexturedPlane.h>
 
 using namespace mpp;
@@ -214,3 +215,62 @@ mrpt::opengl::CSetOfObjects::Ptr CostEvaluatorCostMap::get_visualization() const
 
     return glObjs;
 }
+
+mrpt::maps::COccupancyGridMap2D::Ptr
+    CostEvaluatorCostMap::get_visualization_as_grid() const
+{
+    auto grid = mrpt::maps::COccupancyGridMap2D::Create();
+
+    grid->setSize(
+        costmap_.getXMin(), costmap_.getXMax(), costmap_.getYMin(),
+        costmap_.getYMax(), costmap_.getResolution());
+
+    ASSERT_EQUAL_(grid->getSizeX(), costmap_.getSizeX());
+    ASSERT_EQUAL_(grid->getSizeY(), costmap_.getSizeY());
+
+    /* From: https://github.com/ros2/rviz/blob/rolling/docs/FEATURES.md
+     *
+     * Costmap: Paint valid points between 1 and 98 from blue to red. Paint
+     * points with value 0 in black, points with value 99 in cyan (obstacle
+     * value) and points with value 100 in purple (lethal obstacle). The valid
+     * value -1 is painted in a blueish, greenish, grayish color. Invalid points
+     * between 101 and 127 are painted in green, while invalid negative numbers
+     * are painted in shades from red to yellow.
+     *
+     * So we will use:
+     * -1=free space
+     * 1-98: obstacles.
+     */
+
+    const double MIN_COST_TO_TRANSPARENT = 0.02;
+
+    const auto nCols = costmap_.getSizeX(), nRows = costmap_.getSizeY();
+
+    mrpt::img::CImage gridRGB(nCols, nRows, mrpt::img::CH_RGB);
+    mrpt::img::CImage gridALPHA(nCols, nRows, mrpt::img::CH_GRAY);
+
+    for (size_t icy = 0; icy < nRows; icy++)
+    {
+        auto* row = reinterpret_cast<int8_t*>(grid->getRow(icy));
+        ASSERT_(row);
+
+        for (size_t icx = 0; icx < nCols; icx++)
+        {
+            const double* c = costmap_.cellByIndex(icx, icy);
+            if (!c) continue;  // should not happen?
+            const double val = *c;
+            if (val < MIN_COST_TO_TRANSPARENT)
+            {
+                row[icx] = -1;  // transparent, free space
+            }
+            else
+            {
+                const double f = val / params_.maxCost;
+                row[icx]       = std::min<int8_t>(
+                    98, std::max<int8_t>(1, static_cast<int8_t>(1 + 97 * f)));
+            }
+        }
+    }
+
+    return grid;
+}