From 2a69d99c61c2e5dc68d44227f981afd794883204 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 25 Oct 2021 08:26:55 +0200
Subject: [PATCH] computing navigation cost map
---
.../navigation/algorithms/CMakeLists.txt | 6 ++
.../algorithms/astar/NavigationCostMap.cpp | 79 +++++++++++++------
.../algorithms/astar/NavigationCostMap.h | 21 +++--
.../navigation/algorithms/astar/util.cpp | 37 +++++++++
.../armarx/navigation/algorithms/astar/util.h | 10 +++
.../components/Navigator/Navigator.cpp | 27 +++++++
6 files changed, 148 insertions(+), 32 deletions(-)
create mode 100644 source/armarx/navigation/algorithms/astar/util.cpp
create mode 100644 source/armarx/navigation/algorithms/astar/util.h
diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index 5507cdeb..295e35cf 100644
--- a/source/armarx/navigation/algorithms/CMakeLists.txt
+++ b/source/armarx/navigation/algorithms/CMakeLists.txt
@@ -1,9 +1,13 @@
+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
diff --git a/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp b/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp
index 08bfd27a..8900c799 100644
--- a/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp
+++ b/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;
}
}
}
diff --git a/source/armarx/navigation/algorithms/astar/NavigationCostMap.h b/source/armarx/navigation/algorithms/astar/NavigationCostMap.h
index d6bacdcf..d03750d5 100644
--- a/source/armarx/navigation/algorithms/astar/NavigationCostMap.h
+++ b/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;
};
diff --git a/source/armarx/navigation/algorithms/astar/util.cpp b/source/armarx/navigation/algorithms/astar/util.cpp
new file mode 100644
index 00000000..abc91b03
--- /dev/null
+++ b/source/armarx/navigation/algorithms/astar/util.cpp
@@ -0,0 +1,37 @@
+#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
diff --git a/source/armarx/navigation/algorithms/astar/util.h b/source/armarx/navigation/algorithms/astar/util.h
new file mode 100644
index 00000000..335c9c6d
--- /dev/null
+++ b/source/armarx/navigation/algorithms/astar/util.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx::navigation
+{
+
+ void dumpToFile(const Eigen::MatrixXf& grid);
+
+}
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 3ea06048..e61ad3ea 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/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;
}
--
GitLab