computing navigation cost map

source/armarx/navigation/algorithms/CMakeLists.txt

+find_package(OpenCV REQUIRED )
+
 armarx_add_library(algorithms
     DEPENDENCIES
         ArmarXCoreInterfaces
         ArmarXCore
         armarx_navigation::core
         armarx_navigation::conversions
+    DEPENDENCIES_LEGACY
+        OpenCV
     SOURCES
         ./algorithms.cpp
         # A*
@@ -11,6 +15,7 @@ armarx_add_library(algorithms
         ./astar/NavigationCostMap.cpp
         ./astar/Node.cpp
         ./astar/Planner2D.cpp
+        ./astar/util.cpp
         # smoothing
         ./smoothing/ChainApproximation.cpp
         ./smoothing/CircularPathSmoothing.cpp
@@ -21,6 +26,7 @@ armarx_add_library(algorithms
         ./astar/NavigationCostMap.h
         ./astar/Node.h
         ./astar/Planner2D.h
+        ./astar/util.h
         # smoothing
         ./smoothing/ChainApproximation.h
         ./smoothing/CircularPathSmoothing.h

source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp

@@ -14,6 +14,7 @@
 #include <VirtualRobot/BoundingBox.h>
 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 #include <VirtualRobot/XML/ObjectIO.h>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -26,21 +27,6 @@ namespace bg = boost::geometry;
 namespace armarx::navigation::algorithm::astar
 {
 
-    bool
-    intersects(const VirtualRobot::BoundingBox& bb1, const VirtualRobot::BoundingBox& bb2)
-    {
-        using point_t = bg::model::point<double, 3, bg::cs::cartesian>;
-        using box_t = bg::model::box<point_t>;
-
-        const auto toPoint = [](const Eigen::Vector3f& pt)
-        { return point_t(pt.x(), pt.y(), pt.z()); };
-
-        box_t box1(toPoint(bb1.getMin()), toPoint(bb1.getMax()));
-        box_t box2(toPoint(bb2.getMin()), toPoint(bb2.getMax()));
-
-        return bg::intersects(box1, box2);
-    }
-
     NavigationCostMap::NavigationCostMap(VirtualRobot::RobotPtr robot,
                                          VirtualRobot::SceneObjectSetPtr obstacles,
                                          const Parameters& parameters) :
@@ -64,15 +50,32 @@ namespace armarx::navigation::algorithm::astar
     {
         ARMARX_TRACE;
 
-        ARMARX_DEBUG << "Scene bounds are " << sceneBoundsMin << " and " << sceneBoundsMax;
+        if (obstacles)
+        {
+            for (size_t i = 0; i < obstacles->getCollisionModels().size(); i++)
+            {
+                VirtualRobot::BoundingBox bb = obstacles->getCollisionModels()[i]->getBoundingBox();
+                sceneBoundsMin.x() = std::min(bb.getMin().x(), sceneBoundsMin.x());
+                sceneBoundsMin.y() = std::min(bb.getMin().y(), sceneBoundsMin.y());
+                sceneBoundsMax.x() = std::max(bb.getMax().x(), sceneBoundsMax.x());
+                sceneBoundsMax.y() = std::max(bb.getMax().y(), sceneBoundsMax.y());
+            }
+        }
+
+        ARMARX_INFO << "Scene bounds are " << sceneBoundsMin << " and " << sceneBoundsMax;
 
         //+1 for explicit rounding up
         c_x = (sceneBoundsMax.x() - sceneBoundsMin.x()) / parameters.cellSize + 1;
         c_y = (sceneBoundsMax.y() - sceneBoundsMin.y()) / parameters.cellSize + 1;
 
-        ARMARX_DEBUG << "Grid size: " << c_y << ", " << c_x;
+        ARMARX_INFO << "Grid size: " << c_y << ", " << c_x;
+
+        ARMARX_INFO << "Resetting grid";
+        grid = Eigen::MatrixXf(c_x, c_y); //.array();
+        // grid = Eigen::ArrayXf(); //(c_x, c_y);
+        // grid->resize(c_x, c_y);
 
-        grid = Eigen::ArrayXf(c_x, c_y);
+        ARMARX_INFO << "done.";
     }
 
 
@@ -87,6 +90,9 @@ namespace armarx::navigation::algorithm::astar
         const core::Pose globalPose = Eigen::Translation3f(conv::to3D(position)) *
                                       Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
 
+        // auto colModel = robotCollisionModel->clone();
+        // colModel->setGlobalPose(globalPose.matrix());
+
         robotCollisionModel->setGlobalPose(globalPose.matrix());
 
         VirtualRobot::BoundingBox robotBbox = robotCollisionModel->getBoundingBox(true);
@@ -119,7 +125,9 @@ namespace armarx::navigation::algorithm::astar
                     &id2);
 
             // check if objects collide
-            if (dist <= parameters.cellSize / 2)
+            if ((dist <= parameters.cellSize / 2) or
+                VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
+                    robotCollisionModel, obstacles->getSceneObject(i)->getCollisionModel()))
             {
                 minDistance = 0;
                 break;
@@ -154,7 +162,19 @@ namespace armarx::navigation::algorithm::astar
         ARMARX_TRACE;
         std::vector<Eigen::Vector2f> result;
 
-        ARMARX_CHECK_NOT_NULL(robot);
+        ARMARX_CHECK_NOT_NULL(robot) << "Robot must be set";
+
+
+        VirtualRobot::CoinVisualizationFactory factory;
+        const auto cylinder = factory.createCylinder(1500.F / 2, 2000.F);
+
+        VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(cylinder));
+
+
+        robotCollisionModel = collisionModel;
+
+
+        ARMARX_CHECK_NOT_NULL(robotCollisionModel) << "Collision model must be set!";
 
         // ARMARX_CHECK(robot->hasRobotNode(robotColModelName)) << "No robot node " << robotColModelName;
         // ARMARX_CHECK(robot->getRobotNode(robotColModelName)->getCollisionModel())
@@ -171,9 +191,13 @@ namespace armarx::navigation::algorithm::astar
 
         ARMARX_CHECK_NOT_NULL(robotCollisionModel);
 
+        ARMARX_INFO << "Creating grid";
         createUniformGrid();
 
+        ARMARX_INFO << "Filling grid";
         fillGridCosts();
+
+        ARMARX_INFO << "Filled grid";
     }
 
     void
@@ -182,16 +206,23 @@ namespace armarx::navigation::algorithm::astar
         robotCollisionModel = collisionModel;
     }
 
-    void NavigationCostMap::fillGridCosts()
+    void
+    NavigationCostMap::fillGridCosts()
     {
-        for (int x = 0; x < c_x; x++)
+        int i = 0;
+
+        for (unsigned int x = 0; x < c_x; x++)
         {
-            for (int y = 0; y < c_y; y++)
+            for (unsigned int y = 0; y < c_y; y++)
             {
                 const Index index{x, y};
                 const Position position = toPosition(index);
 
-                grid(x, y) = computeCost(position);
+                grid.value()(x, y) = computeCost(position);
+
+                i++;
+
+                ARMARX_INFO << i << "/" << c_x * c_y;
             }
         }
     }

source/armarx/navigation/algorithms/astar/NavigationCostMap.h

@@ -24,7 +24,7 @@ namespace armarx::navigation::algorithm::astar
         struct Parameters
         {
             // if set to false, distance to obstacles will be computed and not only a binary collision check
-            bool binaryGrid{false}; 
+            bool binaryGrid{false};
 
             /// How big each cell is in the uniform grid.
             float cellSize = 100.f;
@@ -40,6 +40,7 @@ namespace armarx::navigation::algorithm::astar
 
         using Index = Eigen::Array2i;
         using Position = Eigen::Vector2f;
+        using Grid = Eigen::MatrixXf;
 
         struct Vertex
         {
@@ -47,6 +48,14 @@ namespace armarx::navigation::algorithm::astar
             Position position;
         };
 
+        Position toPosition(const Index& index) const;
+
+        const Grid&
+        getGrid() const
+        {
+            return grid.value();
+        }
+
     private:
         void createUniformGrid();
         float computeCost(const NavigationCostMap::Position& position);
@@ -55,11 +64,7 @@ namespace armarx::navigation::algorithm::astar
         void fillGridCosts();
 
     private:
-
-        Eigen::ArrayXf grid;
-
-        Position toPosition(const Index& index) const;
-
+        std::optional<Grid> grid;
 
 
         size_t c_x = 0;
@@ -71,8 +76,8 @@ namespace armarx::navigation::algorithm::astar
 
         VirtualRobot::CollisionModelPtr robotCollisionModel;
 
-        const Eigen::Vector2f sceneBoundsMin;
-        const Eigen::Vector2f sceneBoundsMax;
+        Eigen::Vector2f sceneBoundsMin;
+        Eigen::Vector2f sceneBoundsMax;
 
         const Parameters parameters;
     };

source/armarx/navigation/algorithms/astar/util.cpp

+#include "util.h"
+
+#include <cstddef>
+
+#include <Eigen/src/Core/util/Meta.h>
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/mat.hpp>
+#include <opencv2/opencv.hpp>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+namespace armarx::navigation
+{
+    void
+    dumpToFile(const Eigen::MatrixXf& grid)
+    {
+
+        ARMARX_INFO << "Dumping to file.";
+
+        ARMARX_INFO << "Shape: " << grid.rows() << ", " << grid.cols();
+        cv::Size size(grid.rows(), grid.cols());
+
+        cv::Mat1f mat(size);
+
+        for (size_t r = 0; r < grid.rows(); r++)
+        {
+            for (size_t c = 0; c < grid.cols(); c++)
+            {
+                mat(cv::Point(r, c)) = grid(r, c);
+            }
+        }
+
+
+        cv::imwrite("/tmp/grid.exr", mat);
+    }
+} // namespace armarx::navigation

source/armarx/navigation/algorithms/astar/util.h

+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx::navigation
+{
+
+    void dumpToFile(const Eigen::MatrixXf& grid);
+
+}

source/armarx/navigation/components/Navigator/Navigator.cpp

@@ -55,6 +55,8 @@
 #include "RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h"
 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
+#include "armarx/navigation/algorithms/astar/NavigationCostMap.h"
+#include <armarx/navigation/algorithms/astar/util.h>
 #include <armarx/navigation/client/NavigationStackConfig.h>
 #include <armarx/navigation/client/PathBuilder.h>
 #include <armarx/navigation/client/ice/NavigatorInterface.h>
@@ -424,6 +426,8 @@ namespace armarx::navigation::components
 
         if (result and result.occupancyGrid.has_value())
         {
+            ARMARX_INFO << "Occupancy grid available!";
+
             const auto occupancyGridSceneElements = util::asSceneObjects(
                 result.occupancyGrid.value(),
                 OccupancyGridHelper::Params{.freespaceThreshold = 0.45F,
@@ -447,8 +451,31 @@ namespace armarx::navigation::components
                               .color(viz::Color::orange()));
             }
 
+            ARMARX_INFO << "Creating costmap";
+
+            algorithm::astar::NavigationCostMap costmap(
+                getRobot(),
+                scene.objects,
+                algorithm::astar::NavigationCostMap::Parameters{.binaryGrid = false,
+                                                                .cellSize = 100});
+
+            costmap.createCostmap();
+
+            ARMARX_INFO << "Done";
+
+            ARMARX_TRACE;
+            const auto grid = costmap.getGrid();
+
+            ARMARX_TRACE;
+            ARMARX_INFO << "Dumping.";
+            dumpToFile(grid);
+
             arviz.commit({layer});
         }
+        else
+        {
+            ARMARX_INFO << "Occupancy grid not available";
+        }
 
         return scene;
     }