From 479546569c594b321671e9062727894a22f4dc35 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 10 Jan 2022 20:55:41 +0100
Subject: [PATCH 01/11] costmap stuff
---
CMakeLists.txt | 1 +
.../navigation/algorithms/CMakeLists.txt | 8 +-
.../armarx/navigation/algorithms/Costmap.cpp | 67 +++++
source/armarx/navigation/algorithms/Costmap.h | 63 +++++
.../navigation/algorithms/CostmapBuilder.cpp | 211 +++++++++++++++
.../navigation/algorithms/CostmapBuilder.h | 56 ++++
.../algorithms/NavigationCostMap.cpp | 243 ------------------
.../navigation/algorithms/NavigationCostMap.h | 92 -------
.../algorithms/astar/AStarPlanner.cpp | 2 +-
.../spfa/ShortestPathFasterAlgorithm.cpp | 6 +-
.../spfa/ShortestPathFasterAlgorithm.h | 6 +-
.../algorithms/test/algorithms_spfa_test.cpp | 45 +++-
source/armarx/navigation/algorithms/types.h | 35 +++
source/armarx/navigation/algorithms/util.cpp | 55 ++++
source/armarx/navigation/algorithms/util.h | 34 +++
.../components/Navigator/Navigator.cpp | 31 ++-
source/armarx/navigation/core/StaticScene.h | 2 +
17 files changed, 593 insertions(+), 364 deletions(-)
create mode 100644 source/armarx/navigation/algorithms/Costmap.cpp
create mode 100644 source/armarx/navigation/algorithms/Costmap.h
create mode 100644 source/armarx/navigation/algorithms/CostmapBuilder.cpp
create mode 100644 source/armarx/navigation/algorithms/CostmapBuilder.h
delete mode 100644 source/armarx/navigation/algorithms/NavigationCostMap.cpp
delete mode 100644 source/armarx/navigation/algorithms/NavigationCostMap.h
create mode 100644 source/armarx/navigation/algorithms/types.h
create mode 100644 source/armarx/navigation/algorithms/util.cpp
create mode 100644 source/armarx/navigation/algorithms/util.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1fdc9b99..9420d8af 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,6 +46,7 @@ armarx_find_package(PUBLIC OpenCV QUIET) # Required as RobotAPI is a legacy pro
# )
# FetchContent_MakeAvailable(inotify_cpp)
+add_definitions("-Werror=init-self -Werror=uninitialized")
add_subdirectory(source)
diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index d9ff3588..4b408d87 100644
--- a/source/armarx/navigation/algorithms/CMakeLists.txt
+++ b/source/armarx/navigation/algorithms/CMakeLists.txt
@@ -9,7 +9,9 @@ armarx_add_library(algorithms
OpenCV
SOURCES
./algorithms.cpp
- ./NavigationCostMap.cpp
+ ./Costmap.cpp
+ ./CostmapBuilder.cpp
+ ./util.cpp
# A*
./astar/AStarPlanner.cpp
./astar/Node.cpp
@@ -22,7 +24,9 @@ armarx_add_library(algorithms
./smoothing/CircularPathSmoothing.cpp
HEADERS
./algorithms.h
- ./NavigationCostMap.h
+ ./Costmap.h
+ ./CostmapBuilder.h
+ ./util.h
# A*
./astar/AStarPlanner.h
./astar/Node.h
diff --git a/source/armarx/navigation/algorithms/Costmap.cpp b/source/armarx/navigation/algorithms/Costmap.cpp
new file mode 100644
index 00000000..74bc8aa2
--- /dev/null
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -0,0 +1,67 @@
+#include "Costmap.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/detail/intersects/interface.hpp>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#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>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "armarx/navigation/algorithms/types.h"
+#include "armarx/navigation/algorithms/util.h"
+#include <armarx/navigation/conversions/eigen.h>
+
+
+namespace armarx::navigation::algorithms
+{
+
+ Costmap::Position
+ Costmap::toPosition(const Costmap::Index& index) const
+ {
+ Eigen::Vector2f pos;
+ pos.x() = sceneBounds.min.x() + index.x() * parameters.cellSize + parameters.cellSize / 2;
+ pos.y() = sceneBounds.min.y() + index.y() * parameters.cellSize + parameters.cellSize / 2;
+
+ return pos;
+ }
+
+ Costmap::Vertex
+ Costmap::toVertex(const Eigen::Vector2f& v) const
+ {
+ const float vX =
+ (v.x() - parameters.cellSize / 2 - sceneBounds.min.x()) / parameters.cellSize;
+ const float vY =
+ (v.y() - parameters.cellSize / 2 - sceneBounds.min.y()) / parameters.cellSize;
+
+ ARMARX_CHECK(vX >= 0.F);
+ ARMARX_CHECK(vY >= 0.F);
+
+ ARMARX_CHECK(vX <= (grid.rows() - 1));
+ ARMARX_CHECK(vY <= (grid.cols() - 1));
+
+ return Vertex{.index = Index{static_cast<int>(vX), static_cast<int>(vY)}, .position = v};
+ }
+
+ bool
+ Costmap::isInCollision(const Position& p) const
+ {
+ const auto v = toVertex(p);
+ return grid(v.index.x(), v.index.y()) == 0.F;
+ }
+
+
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
new file mode 100644
index 00000000..b8e8a4cc
--- /dev/null
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <armarx/navigation/algorithms/types.h>
+
+namespace armarx::navigation::algorithms
+{
+
+ class Costmap
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ friend class CostmapBuilder;
+
+ struct Parameters
+ {
+ // if set to false, distance to obstacles will be computed and not only a binary collision check
+ bool binaryGrid{false};
+
+ /// How big each cell is in the uniform grid.
+ float cellSize = 100.f;
+ };
+
+
+ using Index = Eigen::Array2i;
+ using Position = Eigen::Vector2f;
+ using Grid = Eigen::MatrixXf;
+
+
+ Costmap(const Grid& grid, const Parameters& parameters, const SceneBounds& sceneBounds) :
+ grid(grid), sceneBounds(sceneBounds), parameters(parameters)
+ {
+ }
+
+ struct Vertex
+ {
+ Index index; // row corresponds to y; column corresponds to x
+ Position position;
+ };
+
+ Position toPosition(const Index& index) const;
+ Vertex toVertex(const Position& v) const;
+
+ const Grid&
+ getGrid() const
+ {
+ return grid;
+ }
+
+ bool isInCollision(const Position& p) const;
+
+ private:
+ Grid grid;
+
+ const SceneBounds sceneBounds;
+
+ const Parameters parameters;
+ };
+
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.cpp b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
new file mode 100644
index 00000000..50980e0a
--- /dev/null
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
@@ -0,0 +1,211 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CostmapBuilder.h"
+
+#include <cstddef>
+
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/Robot.h>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/util/CPPUtility/trace.h"
+
+#include "armarx/navigation/algorithms/types.h"
+#include "armarx/navigation/algorithms/util.h"
+#include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/core/basic_types.h"
+
+namespace armarx::navigation::algorithms
+{
+
+
+ CostmapBuilder::CostmapBuilder(const VirtualRobot::RobotPtr& robot,
+ const VirtualRobot::SceneObjectSetPtr& obstacles,
+ const Costmap::Parameters& parameters,
+ const std::string& robotCollisonModelName) :
+ robot(robot),
+ obstacles(obstacles),
+ parameters(parameters),
+ robotCollisionModelName(robotCollisonModelName)
+ {
+ ARMARX_CHECK_NOT_NULL(robot) << "Robot must be set";
+ ARMARX_CHECK_NOT_NULL(obstacles);
+ ARMARX_CHECK(robot->hasRobotNode(robotCollisionModelName));
+ }
+
+ Costmap
+ CostmapBuilder::create()
+ {
+
+ const auto sceneBounds = computeSceneBounds(obstacles);
+ const auto grid = createUniformGrid(sceneBounds, parameters);
+
+ ARMARX_INFO << "Creating grid";
+ Costmap costmap(grid, parameters, sceneBounds);
+
+ ARMARX_INFO << "Filling grid";
+ fillGridCosts(costmap);
+ ARMARX_INFO << "Filled grid";
+
+ return costmap;
+ }
+
+
+ Eigen::MatrixXf
+ CostmapBuilder::createUniformGrid(const SceneBounds& sceneBounds,
+ const Costmap::Parameters& parameters)
+ {
+ ARMARX_TRACE;
+
+ ARMARX_INFO << "Scene bounds are " << sceneBounds.min << " and " << sceneBounds.max;
+
+ //+1 for explicit rounding up
+ size_t c_x = (sceneBounds.max.x() - sceneBounds.min.x()) / parameters.cellSize + 1;
+ size_t c_y = (sceneBounds.max.y() - sceneBounds.min.y()) / parameters.cellSize + 1;
+
+ ARMARX_INFO << "Grid size: " << c_y << ", " << c_x;
+
+ ARMARX_INFO << "Resetting grid";
+ return Eigen::MatrixXf(c_x, c_y);
+ }
+
+
+ float
+ CostmapBuilder::computeCost(const Costmap::Position& position,
+ VirtualRobot::RobotPtr& collisionRobot,
+ const VirtualRobot::CollisionModelPtr& robotCollisionModel)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(collisionRobot);
+ ARMARX_CHECK_NOT_NULL(robotCollisionModel);
+ ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
+ ARMARX_CHECK_NOT_NULL(obstacles);
+
+ const core::Pose globalPose(Eigen::Translation3f(conv::to3D(position)));
+ collisionRobot->setGlobalPose(globalPose.matrix());
+
+ const float distance =
+ VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
+ robotCollisionModel, obstacles);
+
+ return distance;
+
+
+ // Eigen::Vector3f P1;
+ // Eigen::Vector3f P2;
+ // int id1, id2;
+
+ // float minDistance = std::numeric_limits<float>::max();
+
+ // // TODO omp...
+ // for (size_t i = 0; i < obstacles->getSize(); i++)
+ // {
+ // // cheap collision check
+ // // VirtualRobot::BoundingBox obstacleBbox =
+ // // obstacles->getSceneObject(i)->getCollisionModel()->getBoundingBox(true);
+ // // if (not intersects(robotBbox, obstacleBbox))
+ // // {
+ // // continue;
+ // // }
+
+ // // precise collision check
+ // const float dist =
+ // VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
+ // robotCollisionModel,
+ // obstacles->getSceneObject(i)->getCollisionModel(),
+ // P1,
+ // P2,
+ // &id1,
+ // &id2);
+
+ // // check if objects collide
+ // if ((dist <= parameters.cellSize / 2) or
+ // VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
+ // robotCollisionModel, obstacles->getSceneObject(i)->getCollisionModel()))
+ // {
+ // minDistance = 0;
+ // break;
+ // }
+
+ // minDistance = std::min(minDistance, dist);
+ // }
+ // // return n->position.x() >= sceneBounds.min.x() && n->position.x() <= sceneBounds.max.x() &&
+ // // n->position.y() >= sceneBounds.min.y() && n->position.y() <= sceneBounds.max.y();
+
+ // return minDistance;
+ }
+
+
+ void
+ CostmapBuilder::fillGridCosts(Costmap& costmap)
+ {
+
+ VirtualRobot::RobotPtr collisionRobot;
+ VirtualRobot::CollisionModelPtr robotCollisionModel;
+
+ const std::size_t c_x = costmap.grid.rows();
+ const std::size_t c_y = costmap.grid.cols();
+
+ robot->setUpdateVisualization(false);
+
+#pragma omp parallel for schedule(static) private(collisionRobot,robotCollisionModel) default(shared)
+ for (unsigned int x = 0; x < c_x; x++)
+ {
+ // initialize if needed
+ if (collisionRobot == nullptr)
+ {
+#pragma omp critical
+ {
+ ARMARX_DEBUG << "Copying robot";
+ ARMARX_CHECK_NOT_NULL(robot);
+ collisionRobot = robot->clone("collision_robot_" + std::to_string(omp_get_thread_num()));
+ collisionRobot->setUpdateVisualization(false);
+ ARMARX_DEBUG << "Copying done";
+ }
+
+ ARMARX_CHECK_NOT_NULL(collisionRobot);
+ ARMARX_CHECK(collisionRobot->hasRobotNode(robotCollisionModelName));
+
+ ARMARX_CHECK(collisionRobot);
+ const auto collisionRobotNode =
+ collisionRobot->getRobotNode(robotCollisionModelName);
+ ARMARX_CHECK_NOT_NULL(collisionRobotNode);
+
+ robotCollisionModel = collisionRobotNode->getCollisionModel();
+ }
+
+ ARMARX_CHECK_NOT_NULL(collisionRobot);
+ ARMARX_CHECK_NOT_NULL(robotCollisionModel);
+
+ for (unsigned int y = 0; y < c_y; y++)
+ {
+ const Costmap::Index index{x, y};
+ const Costmap::Position position = costmap.toPosition(index);
+
+ costmap.grid(x, y) = computeCost(position, collisionRobot, robotCollisionModel);
+ }
+ }
+ }
+
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.h b/source/armarx/navigation/algorithms/CostmapBuilder.h
new file mode 100644
index 00000000..674e1615
--- /dev/null
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.h
@@ -0,0 +1,56 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <armarx/navigation/algorithms/Costmap.h>
+
+namespace armarx::navigation::algorithms
+{
+
+ class CostmapBuilder
+ {
+ public:
+ CostmapBuilder(const VirtualRobot::RobotPtr& robot,
+ const VirtualRobot::SceneObjectSetPtr& obstacles,
+ const Costmap::Parameters& parameters,
+ const std::string& robotCollisonModelName);
+
+ Costmap create();
+
+ private:
+ Eigen::MatrixXf createUniformGrid(const SceneBounds& sceneBounds,
+ const Costmap::Parameters& parameters);
+
+ float computeCost(const Costmap::Position& position,
+ VirtualRobot::RobotPtr& collisionRobot,
+ const VirtualRobot::CollisionModelPtr& robotCollisionModel);
+
+ void fillGridCosts(Costmap& costmap);
+
+ const VirtualRobot::RobotPtr robot;
+ const VirtualRobot::SceneObjectSetPtr obstacles;
+ const Costmap::Parameters parameters;
+ const std::string robotCollisionModelName;
+ };
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/NavigationCostMap.cpp b/source/armarx/navigation/algorithms/NavigationCostMap.cpp
deleted file mode 100644
index 9d994ecb..00000000
--- a/source/armarx/navigation/algorithms/NavigationCostMap.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-#include "NavigationCostMap.h"
-
-#include <algorithm>
-#include <cmath>
-#include <limits>
-
-#include <boost/geometry.hpp>
-#include <boost/geometry/algorithms/detail/intersects/interface.hpp>
-
-#include <Eigen/Geometry>
-
-#include <IceUtil/Time.h>
-
-#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>
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <armarx/navigation/conversions/eigen.h>
-
-namespace bg = boost::geometry;
-
-namespace armarx::navigation::algorithm
-{
-
- NavigationCostMap::NavigationCostMap(VirtualRobot::RobotPtr robot,
- VirtualRobot::SceneObjectSetPtr obstacles,
- const Parameters& parameters) :
- robot(robot), obstacles(obstacles), parameters(parameters)
- {
- }
-
- NavigationCostMap::Position
- NavigationCostMap::toPosition(const NavigationCostMap::Index& index) const
- {
- Eigen::Vector2f pos;
- // TODO check if x and y must be switched
- pos.x() = sceneBoundsMin.x() + index.x() * parameters.cellSize + parameters.cellSize / 2;
- pos.y() = sceneBoundsMin.y() + index.y() * parameters.cellSize + parameters.cellSize / 2;
-
- return pos;
- }
-
- void
- NavigationCostMap::createUniformGrid()
- {
- ARMARX_TRACE;
-
- 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_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);
-
- ARMARX_INFO << "done.";
- }
-
-
- float
- NavigationCostMap::computeCost(const NavigationCostMap::Position& position)
- {
- ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(robotCollisionModel);
- ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
- ARMARX_CHECK_NOT_NULL(obstacles);
-
- 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());
-
- // Unused.
- if (false)
- {
- VirtualRobot::BoundingBox robotBbox = robotCollisionModel->getBoundingBox(true);
- (void) robotBbox;
- }
-
- Eigen::Vector3f P1;
- Eigen::Vector3f P2;
- int id1, id2;
-
- float minDistance = std::numeric_limits<float>::max();
-
- // TODO omp...
- for (size_t i = 0; i < obstacles->getSize(); i++)
- {
- // cheap collision check
- // VirtualRobot::BoundingBox obstacleBbox =
- // obstacles->getSceneObject(i)->getCollisionModel()->getBoundingBox(true);
- // if (not intersects(robotBbox, obstacleBbox))
- // {
- // continue;
- // }
-
- // precise collision check
- const float dist =
- VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
- robotCollisionModel,
- obstacles->getSceneObject(i)->getCollisionModel(),
- P1,
- P2,
- &id1,
- &id2);
-
- // check if objects collide
- if ((dist <= parameters.cellSize / 2) or
- VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
- robotCollisionModel, obstacles->getSceneObject(i)->getCollisionModel()))
- {
- minDistance = 0;
- break;
- }
-
- minDistance = std::min(minDistance, dist);
- }
- // return n->position.x() >= sceneBoundsMin.x() && n->position.x() <= sceneBoundsMax.x() &&
- // n->position.y() >= sceneBoundsMin.y() && n->position.y() <= sceneBoundsMax.y();
-
- return minDistance;
- }
-
- NavigationCostMap::Vertex
- NavigationCostMap::toVertex(const Eigen::Vector2f& v) const
- {
- const float v_x = (v.x() - parameters.cellSize / 2 - sceneBoundsMin.x()) / parameters.cellSize;
- const float v_y = (v.y() - parameters.cellSize / 2 - sceneBoundsMin.y()) / parameters.cellSize;
-
- ARMARX_CHECK(v_x >= 0.F);
- ARMARX_CHECK(v_y >= 0.F);
-
- ARMARX_CHECK(v_x <= (c_x - 1));
- ARMARX_CHECK(v_y <= (c_y - 1));
-
- return Vertex{.index = Index{static_cast<int>(v_x), static_cast<int>(v_y)}, .position = v};
- }
-
- bool NavigationCostMap::isInCollision(const Position& p) const
- {
- const auto v = toVertex(p);
-
- return grid.value()(v.index.x(), v.index.y()) == 0.F;
- }
-
-
- void
- NavigationCostMap::createCostmap()
- {
- ARMARX_TRACE;
- std::vector<Eigen::Vector2f> result;
-
- 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())
- // << "No collision model for robot node " << robotColModelName;
-
- // robotCollisionModel = robot->getRobotNode(robotColModelName)->getCollisionModel()->clone();
- robotCollisionModel->setGlobalPose(robot->getGlobalPose());
-
- // static auto cylinder = VirtualRobot::ObjectIO::loadObstacle("/home/fabi/cylinder.xml");
- // ARMARX_CHECK_NOT_NULL(cylinder);
- // cylinder->setGlobalPose(robot->getGlobalPose());
-
- // robotCollisionModel = cylinder->getCollisionModel();
-
- ARMARX_CHECK_NOT_NULL(robotCollisionModel);
-
- ARMARX_INFO << "Creating grid";
- createUniformGrid();
-
- ARMARX_INFO << "Filling grid";
- fillGridCosts();
-
- ARMARX_INFO << "Filled grid";
- }
-
- void
- NavigationCostMap::setRobotCollisionModel(const VirtualRobot::CollisionModelPtr& collisionModel)
- {
- robotCollisionModel = collisionModel;
- }
-
- void
- NavigationCostMap::fillGridCosts()
- {
- int i = 0;
-
- for (unsigned int x = 0; x < c_x; x++)
- {
- for (unsigned int y = 0; y < c_y; y++)
- {
- const Index index{x, y};
- const Position position = toPosition(index);
-
- grid.value()(x, y) = computeCost(position);
-
- i++;
-
- ARMARX_INFO << i << "/" << c_x * c_y;
- }
- }
- }
-
-} // namespace armarx::navigation::algorithm::astar
diff --git a/source/armarx/navigation/algorithms/NavigationCostMap.h b/source/armarx/navigation/algorithms/NavigationCostMap.h
deleted file mode 100644
index 75328ef4..00000000
--- a/source/armarx/navigation/algorithms/NavigationCostMap.h
+++ /dev/null
@@ -1,92 +0,0 @@
-#pragma once
-
-#include <cstddef>
-
-#include <Eigen/Core>
-
-#include <VirtualRobot/CollisionDetection/CollisionModel.h>
-
-
-namespace armarx::navigation::algorithm
-{
-
- /**
- * The A* planner
- */
- class NavigationCostMap
- {
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- struct Parameters
- {
- // if set to false, distance to obstacles will be computed and not only a binary collision check
- bool binaryGrid{false};
-
- /// How big each cell is in the uniform grid.
- float cellSize = 100.f;
- };
-
-
- using Index = Eigen::Array2i;
- using Position = Eigen::Vector2f;
- using Grid = Eigen::MatrixXf;
-
- NavigationCostMap(VirtualRobot::RobotPtr robot,
- VirtualRobot::SceneObjectSetPtr obstacles,
- const Parameters& parameters);
-
- NavigationCostMap(const Grid& grid,
- const Parameters& parameters,
- const Eigen::Vector2f& sceneBoundsMin) :
- grid(grid), sceneBoundsMin(sceneBoundsMin), parameters(parameters)
- {
- }
-
- void createCostmap();
-
- void setRobotCollisionModel(const VirtualRobot::CollisionModelPtr& collisionModel);
-
-
- struct Vertex
- {
- Index index; // row corresponds to y; column corresponds to x
- Position position;
- };
-
- Position toPosition(const Index& index) const;
-
- const Grid&
- getGrid() const
- {
- return grid.value();
- }
-
- Vertex toVertex(const Position& v) const;
-
- bool isInCollision(const Position& p) const;
-
-
- private:
- void createUniformGrid();
- float computeCost(const NavigationCostMap::Position& position);
-
- void fillGridCosts();
-
- private:
- std::optional<Grid> grid;
-
- size_t c_x = 0;
- size_t c_y = 0;
-
- VirtualRobot::RobotPtr robot;
- VirtualRobot::SceneObjectSetPtr obstacles;
-
- VirtualRobot::CollisionModelPtr robotCollisionModel;
-
- Eigen::Vector2f sceneBoundsMin;
- Eigen::Vector2f sceneBoundsMax;
-
- const Parameters parameters;
- };
-} // namespace armarx::navigation::algorithm
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 3bb7aad4..5452be0d 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -116,7 +116,7 @@ namespace armarx::navigation::algorithm::astar
pos.x() = sceneBoundsMin.x() + c * cellSize + cellSize / 2;
pos.y() = sceneBoundsMin.y() + r * cellSize + cellSize / 2;
- const float obstacleDistance = computeObstacleDistance(pos, robot);
+ const float obstacleDistance = computeObstacleDistance(pos, collisionRobot);
grid[r][c] = std::make_shared<Node>(pos, obstacleDistance);
}
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
index 8fd47283..72b981ec 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
@@ -51,7 +51,7 @@ namespace armarx::navigation::algorithms::spfa
}
ShortestPathFasterAlgorithm::ShortestPathFasterAlgorithm(
- const algorithm::NavigationCostMap& grid,
+ const Costmap& grid,
const Parameters& params) :
grid(grid), params(params)
{
@@ -64,7 +64,7 @@ namespace armarx::navigation::algorithms::spfa
ARMARX_CHECK(not grid.isInCollision(start)) << "Start is in collision";
ARMARX_CHECK(not grid.isInCollision(goal)) << "Goal is in collision";
- const algorithm::NavigationCostMap::Vertex startVertex = grid.toVertex(start);
+ const Costmap::Vertex startVertex = grid.toVertex(start);
const auto goalVertex = Eigen::Vector2i{grid.toVertex(goal).index};
ARMARX_INFO << "Planning from " << startVertex.index << " to " << goalVertex;
@@ -252,7 +252,7 @@ namespace armarx::navigation::algorithms::spfa
{
continue;
}
-
+
output_parents.at(row).at(col) = unravel(parents[u], num_cols);
}
}
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
index d0c22c09..de25ace9 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
@@ -25,7 +25,7 @@
#include <Eigen/Core>
-#include <armarx/navigation/algorithms/NavigationCostMap.h>
+#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::algorithms::spfa
@@ -39,7 +39,7 @@ namespace armarx::navigation::algorithms::spfa
bool obstacleDistanceCosts{true};
};
- ShortestPathFasterAlgorithm(const algorithm::NavigationCostMap& grid,
+ ShortestPathFasterAlgorithm(const Costmap& grid,
const Parameters& params);
struct Result
@@ -65,7 +65,7 @@ namespace armarx::navigation::algorithms::spfa
private:
- const algorithm::NavigationCostMap grid;
+ const Costmap grid;
const Parameters params;
};
diff --git a/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp b/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
index 97c57f6b..bea64e74 100644
--- a/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
+++ b/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
@@ -42,7 +42,8 @@
#include "ArmarXCore/core/system/ArmarXDataPath.h"
#include <ArmarXCore/core/logging/Logging.h>
-#include "armarx/navigation/algorithms/NavigationCostMap.h"
+#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/algorithms/types.h"
#include <armarx/navigation/Test.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
@@ -51,15 +52,19 @@ BOOST_AUTO_TEST_CASE(testSPFA)
const int rows = 100;
const int cols = 200;
+ const int cellSize = 100;
+
Eigen::Vector2i start{50, 130};
Eigen::Vector2i goal{50, 100};
Eigen::MatrixXf obstacleDistances(rows, cols);
obstacleDistances.setOnes();
- const armarx::navigation::algorithm::NavigationCostMap::Parameters params;
- const armarx::navigation::algorithm::NavigationCostMap costmap(
- obstacleDistances, params, Eigen::Vector2f::Zero());
+ const armarx::navigation::algorithms::SceneBounds sceneBounds{
+ .min = Eigen::Vector2f::Zero(), .max = Eigen::Vector2f{rows * cellSize, cols * cellSize}};
+
+ const armarx::navigation::algorithms::Costmap::Parameters params;
+ const armarx::navigation::algorithms::Costmap costmap(obstacleDistances, params, sceneBounds);
ARMARX_INFO << "computing ...";
@@ -110,9 +115,11 @@ BOOST_AUTO_TEST_CASE(testSPFAPlan)
Eigen::MatrixXf obstacleDistances(rows, cols);
obstacleDistances.setOnes();
- const armarx::navigation::algorithm::NavigationCostMap::Parameters params{.cellSize = cellSize};
- const armarx::navigation::algorithm::NavigationCostMap costmap(
- obstacleDistances, params, Eigen::Vector2f::Zero());
+ const armarx::navigation::algorithms::SceneBounds sceneBounds{
+ .min = Eigen::Vector2f::Zero(), .max = Eigen::Vector2f{rows * cellSize, cols * cellSize}};
+
+ const armarx::navigation::algorithms::Costmap::Parameters params{.cellSize = cellSize};
+ const armarx::navigation::algorithms::Costmap costmap(obstacleDistances, params, sceneBounds);
ARMARX_INFO << "computing ...";
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams{
@@ -152,9 +159,15 @@ BOOST_AUTO_TEST_CASE(testGrid)
BOOST_REQUIRE_GT(cellSize, 0);
std::vector<float> sceneBoundsMinV(j["scene_bounds"]["min"]);
+ std::vector<float> sceneBoundsMaxV(j["scene_bounds"]["max"]);
BOOST_REQUIRE_EQUAL(sceneBoundsMinV.size(), 2);
+ BOOST_REQUIRE_EQUAL(sceneBoundsMaxV.size(), 2);
const Eigen::Vector2f sceneBoundsMin{sceneBoundsMinV.at(0), sceneBoundsMinV.at(1)};
+ const Eigen::Vector2f sceneBoundsMax{sceneBoundsMaxV.at(0), sceneBoundsMaxV.at(1)};
+
+ const armarx::navigation::algorithms::SceneBounds sceneBounds{.min = sceneBoundsMin,
+ .max = sceneBoundsMax};
BOOST_REQUIRE(std::filesystem::exists(testExrFilename));
@@ -163,9 +176,9 @@ BOOST_AUTO_TEST_CASE(testGrid)
Eigen::MatrixXf obstacleDistances;
cv::cv2eigen(mat, obstacleDistances);
- const armarx::navigation::algorithm::NavigationCostMap::Parameters params{.cellSize = cellSize};
- const armarx::navigation::algorithm::NavigationCostMap costmap(
- obstacleDistances, params, sceneBoundsMin);
+
+ const armarx::navigation::algorithms::Costmap::Parameters params{.cellSize = cellSize};
+ const armarx::navigation::algorithms::Costmap costmap(obstacleDistances, params, sceneBounds);
Eigen::Vector2f start{1500, 0};
@@ -200,9 +213,15 @@ BOOST_AUTO_TEST_CASE(testSPFAPlanWObstacleDistance)
BOOST_REQUIRE_GT(cellSize, 0);
std::vector<float> sceneBoundsMinV(j["scene_bounds"]["min"]);
+ std::vector<float> sceneBoundsMaxV(j["scene_bounds"]["max"]);
BOOST_REQUIRE_EQUAL(sceneBoundsMinV.size(), 2);
+ BOOST_REQUIRE_EQUAL(sceneBoundsMaxV.size(), 2);
const Eigen::Vector2f sceneBoundsMin{sceneBoundsMinV.at(0), sceneBoundsMinV.at(1)};
+ const Eigen::Vector2f sceneBoundsMax{sceneBoundsMaxV.at(0), sceneBoundsMaxV.at(1)};
+
+ const armarx::navigation::algorithms::SceneBounds sceneBounds{.min = sceneBoundsMin,
+ .max = sceneBoundsMax};
BOOST_REQUIRE(std::filesystem::exists(testExrFilename));
@@ -212,9 +231,8 @@ BOOST_AUTO_TEST_CASE(testSPFAPlanWObstacleDistance)
cv::cv2eigen(mat, obstacleDistances);
obstacleDistances = obstacleDistances.transpose();
- const armarx::navigation::algorithm::NavigationCostMap::Parameters params{.cellSize = cellSize};
- const armarx::navigation::algorithm::NavigationCostMap costmap(
- obstacleDistances, params, sceneBoundsMin);
+ const armarx::navigation::algorithms::Costmap::Parameters params{.cellSize = cellSize};
+ const armarx::navigation::algorithms::Costmap costmap(obstacleDistances, params, sceneBounds);
ARMARX_INFO << "computing ...";
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams{
@@ -228,5 +246,4 @@ BOOST_AUTO_TEST_CASE(testSPFAPlanWObstacleDistance)
ARMARX_INFO << "done.";
BOOST_CHECK(not path.empty());
-
}
diff --git a/source/armarx/navigation/algorithms/types.h b/source/armarx/navigation/algorithms/types.h
new file mode 100644
index 00000000..4e685859
--- /dev/null
+++ b/source/armarx/navigation/algorithms/types.h
@@ -0,0 +1,35 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx::navigation::algorithms
+{
+
+ struct SceneBounds
+ {
+ Eigen::Vector2f min = Eigen::Vector2f::Zero();
+ Eigen::Vector2f max = Eigen::Vector2f::Zero();
+ };
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/util.cpp b/source/armarx/navigation/algorithms/util.cpp
new file mode 100644
index 00000000..53a3aafa
--- /dev/null
+++ b/source/armarx/navigation/algorithms/util.cpp
@@ -0,0 +1,55 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "util.h"
+
+#include <VirtualRobot/BoundingBox.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+namespace armarx::navigation::algorithms
+{
+
+
+ SceneBounds
+ computeSceneBounds(const VirtualRobot::SceneObjectSetPtr& obstacles)
+ {
+ SceneBounds bounds;
+
+ ARMARX_CHECK_NOT_NULL(obstacles);
+ ARMARX_CHECK(not obstacles->getCollisionModels().empty());
+
+ for (size_t i = 0; i < obstacles->getCollisionModels().size(); i++)
+ {
+ VirtualRobot::BoundingBox bb = obstacles->getCollisionModels()[i]->getBoundingBox();
+ bounds.min.x() = std::min(bb.getMin().x(), bounds.min.x());
+ bounds.min.y() = std::min(bb.getMin().y(), bounds.min.y());
+ bounds.max.x() = std::max(bb.getMax().x(), bounds.max.x());
+ bounds.max.y() = std::max(bb.getMax().y(), bounds.max.y());
+ }
+
+ return bounds;
+ }
+
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/util.h b/source/armarx/navigation/algorithms/util.h
new file mode 100644
index 00000000..add037fc
--- /dev/null
+++ b/source/armarx/navigation/algorithms/util.h
@@ -0,0 +1,34 @@
+
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <armarx/navigation/algorithms/types.h>
+
+namespace armarx::navigation::algorithms
+{
+
+ SceneBounds computeSceneBounds(const VirtualRobot::SceneObjectSetPtr& obstacles);
+
+} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index fb665e71..8a36f658 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -56,7 +56,8 @@
#include "RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h"
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <armarx/navigation/algorithms/NavigationCostMap.h>
+#include "armarx/navigation/algorithms/CostmapBuilder.h"
+#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/astar/util.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/PathBuilder.h>
@@ -444,7 +445,25 @@ namespace armarx::navigation::components
ARMARX_TRACE;
const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
- core::StaticScene scene{.objects = util::asSceneObjects(objectPoses)};
+ const auto objects = util::asSceneObjects(objectPoses);
+
+ ARMARX_INFO << "Creating costmap";
+
+ algorithms::CostmapBuilder costmapBuilder(
+ getRobot(),
+ objects,
+ algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ "Platform-navigation-colmodel");
+
+ const auto costmap = costmapBuilder.create();
+
+ ARMARX_INFO << "Done";
+
+ ARMARX_TRACE;
+ const auto grid = costmap.getGrid();
+
+ core::StaticScene scene{.objects = objects,
+ .costmap = std::make_unique<algorithms::Costmap>(costmap)};
ARMARX_INFO << "The object scene consists of " << scene.objects->getSize() << " objects";
@@ -482,13 +501,13 @@ namespace armarx::navigation::components
ARMARX_INFO << "Creating costmap";
- algorithm::NavigationCostMap costmap(
+ algorithms::CostmapBuilder costmapBuilder(
getRobot(),
scene.objects,
- algorithm::NavigationCostMap::Parameters{.binaryGrid = false,
- .cellSize = 100});
+ algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ "Platform-navigation-colmodel");
- costmap.createCostmap();
+ const auto costmap = costmapBuilder.create();
ARMARX_INFO << "Done";
diff --git a/source/armarx/navigation/core/StaticScene.h b/source/armarx/navigation/core/StaticScene.h
index 4d05a2fe..f70a2451 100644
--- a/source/armarx/navigation/core/StaticScene.h
+++ b/source/armarx/navigation/core/StaticScene.h
@@ -25,6 +25,7 @@
#include <memory>
#include <VirtualRobot/VirtualRobot.h>
+#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::core
{
@@ -32,6 +33,7 @@ namespace armarx::navigation::core
struct StaticScene
{
VirtualRobot::SceneObjectSetPtr objects;
+ std::unique_ptr<algorithms::Costmap> costmap;
};
using StaticScenePtr = std::shared_ptr<StaticScene>;
--
GitLab
From 264c8d27601d8f40ea930e672f389a3a357c26cb Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 10 Jan 2022 21:54:48 +0100
Subject: [PATCH 02/11] a star planner now using gridmap
---
.../navigation/algorithms/CostmapBuilder.cpp | 11 +-
.../algorithms/astar/AStarPlanner.cpp | 141 ++++--------------
.../algorithms/astar/AStarPlanner.h | 17 +--
.../components/Navigator/Navigator.cpp | 12 +-
.../navigation/global_planning/AStar.cpp | 10 +-
5 files changed, 51 insertions(+), 140 deletions(-)
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.cpp b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
index 50980e0a..2e9b9b1f 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.cpp
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
@@ -63,7 +63,8 @@ namespace armarx::navigation::algorithms
ARMARX_INFO << "Creating grid";
Costmap costmap(grid, parameters, sceneBounds);
- ARMARX_INFO << "Filling grid";
+ ARMARX_INFO << "Filling grid with size (" << costmap.getGrid().rows() << "/"
+ << costmap.getGrid().cols() << ")";
fillGridCosts(costmap);
ARMARX_INFO << "Filled grid";
@@ -83,7 +84,7 @@ namespace armarx::navigation::algorithms
size_t c_x = (sceneBounds.max.x() - sceneBounds.min.x()) / parameters.cellSize + 1;
size_t c_y = (sceneBounds.max.y() - sceneBounds.min.y()) / parameters.cellSize + 1;
- ARMARX_INFO << "Grid size: " << c_y << ", " << c_x;
+ ARMARX_INFO << "Grid size: " << c_x << ", " << c_y;
ARMARX_INFO << "Resetting grid";
return Eigen::MatrixXf(c_x, c_y);
@@ -168,7 +169,8 @@ namespace armarx::navigation::algorithms
robot->setUpdateVisualization(false);
-#pragma omp parallel for schedule(static) private(collisionRobot,robotCollisionModel) default(shared)
+#pragma omp parallel for schedule(static) private(collisionRobot, \
+ robotCollisionModel) default(shared)
for (unsigned int x = 0; x < c_x; x++)
{
// initialize if needed
@@ -178,7 +180,8 @@ namespace armarx::navigation::algorithms
{
ARMARX_DEBUG << "Copying robot";
ARMARX_CHECK_NOT_NULL(robot);
- collisionRobot = robot->clone("collision_robot_" + std::to_string(omp_get_thread_num()));
+ collisionRobot =
+ robot->clone("collision_robot_" + std::to_string(omp_get_thread_num()));
collisionRobot->setUpdateVisualization(false);
ARMARX_DEBUG << "Copying done";
}
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 5452be0d..2b4a2d4e 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -43,12 +43,8 @@ namespace armarx::navigation::algorithm::astar
return bg::intersects(box1, box2);
}
- AStarPlanner::AStarPlanner(VirtualRobot::RobotPtr robot,
- VirtualRobot::SceneObjectSetPtr obstacles,
- float cellSize) :
- Planner2D(robot, obstacles), cellSize(cellSize)
+ AStarPlanner::AStarPlanner(const algorithms::Costmap& costmap) : costmap(costmap)
{
- ARMARX_CHECK_GREATER_EQUAL(static_cast<int>(obstacles->getSize()), 0);
}
float
@@ -57,72 +53,34 @@ namespace armarx::navigation::algorithm::astar
return (1.F - obstacleDistanceWeightFactor) * (n1->position - n2->position).norm();
}
- float
- AStarPlanner::computeObstacleDistance(const Eigen::Vector2f& pos,
- VirtualRobot::RobotPtr& robot) const
- {
-
- core::Pose global_T_robot = core::Pose::Identity();
- global_T_robot.translation().head<2>() = pos;
- robot->setGlobalPose(global_T_robot.matrix());
-
- const auto robotCollisionModel =
- robot->getRobotNode(robotColModelName)->getCollisionModel();
-
- const float distance =
- VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
- robotCollisionModel, obstacles);
-
- return distance;
- }
-
void
AStarPlanner::createUniformGrid()
{
ARMARX_TRACE;
- ARMARX_VERBOSE << "Scene bounds are " << sceneBoundsMin << " and " << sceneBoundsMax;
-
- //+1 for explicit rounding up
- cols = (sceneBoundsMax.x() - sceneBoundsMin.x()) / cellSize + 1;
- rows = (sceneBoundsMax.y() - sceneBoundsMin.y()) / cellSize + 1;
-
- ARMARX_VERBOSE << "Grid size: " << rows << ", " << cols;
+ const size_t rows = costmap.getGrid().rows();
+ const size_t cols = costmap.getGrid().cols();
// create the grid with the correct size
+ ARMARX_INFO << "Creating grid";
for (size_t r = 0; r < rows; r++)
{
grid.push_back(std::vector<NodePtr>(cols));
}
- ARMARX_INFO << "Using " << omp_get_num_threads() << " threads to build grid";
-
- VirtualRobot::RobotPtr collisionRobot;
-
-#pragma omp parallel for schedule(static) private(collisionRobot) default(shared)
+ // intitializing grid
for (size_t r = 0; r < rows; r++)
{
- // initialize if needed
- if (collisionRobot == nullptr)
- {
- collisionRobot =
- robot->clone("collision_robot_" + std::to_string(omp_get_thread_num()));
- collisionRobot->setUpdateVisualization(false);
- }
-
for (size_t c = 0; c < cols; c++)
{
- Eigen::Vector2f pos;
- pos.x() = sceneBoundsMin.x() + c * cellSize + cellSize / 2;
- pos.y() = sceneBoundsMin.y() + r * cellSize + cellSize / 2;
-
- const float obstacleDistance = computeObstacleDistance(pos, collisionRobot);
-
+ const auto pos = costmap.toPosition({r, c});
+ const float obstacleDistance = costmap.getGrid()(r, c);
+
grid[r][c] = std::make_shared<Node>(pos, obstacleDistance);
}
}
- ARMARX_INFO << "Finished initialization of grid";
+ ARMARX_INFO << "Creating graph";
// Init successors
for (size_t r = 0; r < rows; r++)
@@ -140,24 +98,26 @@ namespace armarx::navigation::algorithm::astar
{
for (int nC = -1; nC <= 1; nC++)
{
- const int neighborIndexX = c + nC;
- const int neighborIndexY = r + nR;
- if (neighborIndexX < 0 || neighborIndexY < 0 || (nR == 0 && nC == 0))
+ const int neighborIndexC = c + nC;
+ const int neighborIndexR = r + nR;
+ if (neighborIndexC < 0 || neighborIndexR < 0 || (nR == 0 && nC == 0))
{
continue;
}
- if (neighborIndexY >= static_cast<int>(rows) ||
- neighborIndexX >= static_cast<int>(cols))
+ if (neighborIndexR >= static_cast<int>(rows) ||
+ neighborIndexC >= static_cast<int>(cols))
{
continue;
}
- grid[neighborIndexY][neighborIndexX]->successors.push_back(candidate);
+ grid[neighborIndexR][neighborIndexC]->successors.push_back(candidate);
}
}
}
}
+
+ ARMARX_INFO << "Done.";
}
bool
@@ -165,51 +125,24 @@ namespace armarx::navigation::algorithm::astar
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(n);
- ARMARX_CHECK_NOT_NULL(robotCollisionModel);
- ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
- ARMARX_CHECK_NOT_NULL(obstacles);
-
- const core::Pose globalPose(Eigen::Translation3f(conv::to3D(n->position))); //*
- //Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
-
- robotCollisionModel->setGlobalPose(globalPose.matrix());
-
- VirtualRobot::BoundingBox robotBbox = robotCollisionModel->getBoundingBox(true);
- Eigen::Vector3f P1, P2;
- for (size_t i = 0; i < obstacles->getSize(); i++)
- {
- // cheap collision check
- VirtualRobot::BoundingBox obstacleBbox =
- obstacles->getSceneObject(i)->getCollisionModel()->getBoundingBox(true);
- if (not intersects(robotBbox, obstacleBbox))
- {
- continue;
- }
-
- // precise collision check
- bool collision =
- VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
- robotCollisionModel, obstacles->getSceneObject(i)->getCollisionModel());
-
- if (collision)
- {
- return false;
- }
- }
- return n->position.x() >= sceneBoundsMin.x() && n->position.x() <= sceneBoundsMax.x() &&
- n->position.y() >= sceneBoundsMin.y() && n->position.y() <= sceneBoundsMax.y();
+ return not costmap.isInCollision(n->position);
}
NodePtr
AStarPlanner::closestNode(const Eigen::Vector2f& v)
{
- float r = (v.y() - cellSize / 2 - sceneBoundsMin.y()) / cellSize;
- float c = (v.x() - cellSize / 2 - sceneBoundsMin.x()) / cellSize;
+ const auto vertex = costmap.toVertex(v);
+
+ const int r = vertex.index.x();
+ const int c = vertex.index.y();
ARMARX_CHECK_GREATER_EQUAL(r, 0.F);
ARMARX_CHECK_GREATER_EQUAL(c, 0.F);
+ const size_t rows = costmap.getGrid().rows();
+ const size_t cols = costmap.getGrid().cols();
+
ARMARX_CHECK_LESS_EQUAL(r, rows - 1);
ARMARX_CHECK_LESS_EQUAL(c, cols - 1);
@@ -223,23 +156,6 @@ namespace armarx::navigation::algorithm::astar
grid.clear();
std::vector<Eigen::Vector2f> result;
- ARMARX_CHECK_NOT_NULL(robot);
-
- // ARMARX_CHECK(robot->hasRobotNode(robotColModelName)) << "No robot node " << robotColModelName;
- // ARMARX_CHECK(robot->getRobotNode(robotColModelName)->getCollisionModel())
- // << "No collision model for robot node " << robotColModelName;
-
- // robotCollisionModel = robot->getRobotNode(robotColModelName)->getCollisionModel()->clone();
- ARMARX_CHECK_NOT_NULL(robotCollisionModel);
- robotCollisionModel->setGlobalPose(robot->getGlobalPose());
-
- // static auto cylinder = VirtualRobot::ObjectIO::loadObstacle("/home/fabi/cylinder.xml");
- // ARMARX_CHECK_NOT_NULL(cylinder);
- // cylinder->setGlobalPose(robot->getGlobalPose());
-
- // robotCollisionModel = cylinder->getCollisionModel();
-
-
createUniformGrid();
ARMARX_DEBUG << "Setting start node for position " << start;
NodePtr nodeStart = closestNode(start);
@@ -315,13 +231,6 @@ namespace armarx::navigation::algorithm::astar
return result;
}
- void
- AStarPlanner::setRobotCollisionModel(const std::string& collisionModelNodeName)
- {
- robotColModelName = collisionModelNodeName;
- robotCollisionModel = robot->getRobotNode(collisionModelNodeName)->getCollisionModel();
- }
-
float
AStarPlanner::costs(const NodePtr& n1, const NodePtr& n2) const
{
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.h b/source/armarx/navigation/algorithms/astar/AStarPlanner.h
index 5737f628..9f6e7496 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.h
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.h
@@ -6,6 +6,7 @@
#include "Node.h"
#include "Planner2D.h"
+#include "armarx/navigation/algorithms/Costmap.h"
namespace armarx::navigation::algorithm::astar
{
@@ -13,18 +14,14 @@ namespace armarx::navigation::algorithm::astar
/**
* The A* planner
*/
- class AStarPlanner : public Planner2D
+ class AStarPlanner //: public Planner2D
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- AStarPlanner(VirtualRobot::RobotPtr robot,
- VirtualRobot::SceneObjectSetPtr obstacles = {},
- float cellSize = 100.f);
+ AStarPlanner(const algorithms::Costmap& costmap);
- std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal) override;
-
- void setRobotCollisionModel(const std::string& collisionModelNodeName);
+ std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal) /* override */;
private:
float heuristic(const NodePtr& n1, const NodePtr& n2);
@@ -36,14 +33,10 @@ namespace armarx::navigation::algorithm::astar
float computeObstacleDistance(const Eigen::Vector2f& pos, VirtualRobot::RobotPtr& robot) const;
private:
- /// How big each cell is in the uniform grid.
- float cellSize;
+ const algorithms::Costmap costmap;
std::vector<std::vector<NodePtr>> grid;
- size_t cols = 0;
- size_t rows = 0;
-
const float obstacleDistanceWeightFactor = 0.5;
const float maxObstacleDistance = 1500; // [mm]
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 8a36f658..14430f5e 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -31,6 +31,8 @@
#include <utility>
#include <Eigen/Geometry>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/imgcodecs.hpp>
#include <Ice/Current.h>
@@ -216,7 +218,15 @@ namespace armarx::navigation::components
{
std::lock_guard g{scene.staticSceneMtx};
- scene.staticScene = staticScene();
+
+ if (not scene.staticScene.has_value())
+ {
+ scene.staticScene = staticScene();
+
+ cv::Mat1f mat;
+ cv::eigen2cv(scene.staticScene->costmap->getGrid(), mat);
+ cv::imwrite("/tmp/grid.exr", mat);
+ }
}
// TODO dynamic scene
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 641492e1..16ca1777 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -159,13 +159,9 @@ namespace armarx::navigation::global_planning
ARMARX_TRACE;
std::lock_guard g{scene.staticSceneMtx};
- algorithm::astar::AStarPlanner planner(scene.robot, scene.staticScene->objects, 100.F);
- // planner.setRobotColModel("Platform-colmodel");
-
- // planner.setRobotColModel("Platform-colmodel");
- //planner.setRobotCollisionModel(robotCollisionModel());
- planner.setRobotCollisionModel("Platform-navigation-colmodel");
-
+ // FIXME check if costmap is available
+ algorithm::astar::AStarPlanner planner(*scene.staticScene->costmap);
+
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
const auto timeStart = IceUtil::Time::now();
--
GitLab
From 2444e56b80d8e3e8f6f18a5f45faa7d1a34b8d4f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 Jan 2022 13:54:53 +0100
Subject: [PATCH 03/11] spfa wrapper
---
source/armarx/navigation/algorithms/Costmap.h | 5 +
.../algorithms/astar/AStarPlanner.cpp | 4 +-
.../spfa/ShortestPathFasterAlgorithm.cpp | 41 ++--
.../spfa/ShortestPathFasterAlgorithm.h | 5 +-
.../components/Navigator/Navigator.cpp | 74 ++++++
.../components/Navigator/Navigator.h | 1 +
.../factories/GlobalPlannerFactory.cpp | 9 +-
.../navigation/global_planning/CMakeLists.txt | 3 +
.../navigation/global_planning/SPFA.cpp | 231 ++++++++++++++++++
.../armarx/navigation/global_planning/SPFA.h | 71 ++++++
.../global_planning/aron/SPFAParams.xml | 25 ++
.../global_planning/aron_conversions.cpp | 23 ++
.../global_planning/aron_conversions.h | 5 +
.../armarx/navigation/global_planning/core.h | 1 +
14 files changed, 477 insertions(+), 21 deletions(-)
create mode 100644 source/armarx/navigation/global_planning/SPFA.cpp
create mode 100644 source/armarx/navigation/global_planning/SPFA.h
create mode 100644 source/armarx/navigation/global_planning/aron/SPFAParams.xml
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index b8e8a4cc..ffd4edee 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -51,6 +51,11 @@ namespace armarx::navigation::algorithms
bool isInCollision(const Position& p) const;
+ const Parameters& params() const noexcept
+ {
+ return parameters;
+ }
+
private:
Grid grid;
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 2b4a2d4e..bddc5157 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -140,8 +140,8 @@ namespace armarx::navigation::algorithm::astar
ARMARX_CHECK_GREATER_EQUAL(r, 0.F);
ARMARX_CHECK_GREATER_EQUAL(c, 0.F);
- const size_t rows = costmap.getGrid().rows();
- const size_t cols = costmap.getGrid().cols();
+ const int rows = static_cast<int>(costmap.getGrid().rows());
+ const int cols = static_cast<int>(costmap.getGrid().cols());
ARMARX_CHECK_LESS_EQUAL(r, rows - 1);
ARMARX_CHECK_LESS_EQUAL(c, cols - 1);
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
index 72b981ec..8ba7681d 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
@@ -28,6 +28,8 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
+#include "armarx/navigation/algorithms/Costmap.h"
+
namespace armarx::navigation::algorithms::spfa
{
@@ -50,26 +52,26 @@ namespace armarx::navigation::algorithms::spfa
return p;
}
- ShortestPathFasterAlgorithm::ShortestPathFasterAlgorithm(
- const Costmap& grid,
- const Parameters& params) :
- grid(grid), params(params)
+ ShortestPathFasterAlgorithm::ShortestPathFasterAlgorithm(const Costmap& grid,
+ const Parameters& params) :
+ costmap(grid), params(params)
{
- ARMARX_INFO << "Grid with size (" << grid.getGrid().rows() << ", " << grid.getGrid().cols() << ").";
+ ARMARX_INFO << "Grid with size (" << grid.getGrid().rows() << ", " << grid.getGrid().cols()
+ << ").";
}
std::vector<Eigen::Vector2f>
ShortestPathFasterAlgorithm::plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal)
{
- ARMARX_CHECK(not grid.isInCollision(start)) << "Start is in collision";
- ARMARX_CHECK(not grid.isInCollision(goal)) << "Goal is in collision";
+ ARMARX_CHECK(not costmap.isInCollision(start)) << "Start is in collision";
+ ARMARX_CHECK(not costmap.isInCollision(goal)) << "Goal is in collision";
- const Costmap::Vertex startVertex = grid.toVertex(start);
- const auto goalVertex = Eigen::Vector2i{grid.toVertex(goal).index};
+ const Costmap::Vertex startVertex = costmap.toVertex(start);
+ const auto goalVertex = Eigen::Vector2i{costmap.toVertex(goal).index};
ARMARX_INFO << "Planning from " << startVertex.index << " to " << goalVertex;
- const Result result = spfa(grid.getGrid(), Eigen::Vector2i{startVertex.index});
+ const Result result = spfa(costmap.getGrid(), Eigen::Vector2i{startVertex.index});
const auto isStart = [&startVertex](const Eigen::Vector2i& v)
{ return (v.array() == startVertex.index.array()).all(); };
@@ -101,7 +103,7 @@ namespace armarx::navigation::algorithms::spfa
break;
}
- path.push_back(grid.toPosition(parent.array()));
+ path.push_back(costmap.toPosition(parent.array()));
pt = &parent;
}
@@ -115,6 +117,12 @@ namespace armarx::navigation::algorithms::spfa
return path;
}
+ ShortestPathFasterAlgorithm::Result
+ ShortestPathFasterAlgorithm::spfa(const Eigen::Vector2f& start)
+ {
+ return spfa(costmap.getGrid(), costmap.toVertex(start).index);
+ }
+
ShortestPathFasterAlgorithm::Result
ShortestPathFasterAlgorithm::spfa(const Eigen::MatrixXf& inputMap,
@@ -177,14 +185,15 @@ namespace armarx::navigation::algorithms::spfa
continue;
- const float obstacleDistance1 = inputMap(row, col);
- const float obstacleDistance2 = inputMap(ip, jp);
+ const float obstacleDistance1 = std::min(inputMap(row, col), 1000.F);
+ const float obstacleDistance2 = std::min(inputMap(ip, jp), 1000.F);
const float travelCost = dir_lengths[k];
- const float obstacleDistanceCost = obstacleDistance1 - obstacleDistance2;
+ // const float obstacleDistanceCost = std::max(obstacleDistance1 - obstacleDistance2, 0.F) / costmap.params().cellSize;
+ const float targetDistanceCost = 3* (1000.F - obstacleDistance2) / 1000.F;
const float edgeCost = params.obstacleDistanceCosts
- ? travelCost + obstacleDistanceCost
+ ? travelCost + targetDistanceCost
: travelCost;
int e = ravel(v, edge_counts[v], max_edges_per_vert);
@@ -248,7 +257,7 @@ namespace armarx::navigation::algorithms::spfa
int u = ravel(row, col, num_cols);
output_dists(row, col) = (dists[u] < inf - eps) * dists[u];
- if(parents[u] == -1) // no parent
+ if (parents[u] == -1) // no parent
{
continue;
}
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
index de25ace9..85909558 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
@@ -53,6 +53,9 @@ namespace armarx::navigation::algorithms::spfa
const Eigen::Vector2f& goal);
+
+ Result spfa(const Eigen::Vector2f& start);
+
// protected:
/**
* @brief Implementation highly inspired by github.com:jimmyyhwu/spatial-action-maps
@@ -65,7 +68,7 @@ namespace armarx::navigation::algorithms::spfa
private:
- const Costmap grid;
+ const Costmap costmap;
const Parameters params;
};
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 14430f5e..db4c14da 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -26,20 +26,25 @@
#include <algorithm>
#include <cmath>
#include <cstddef>
+#include <cstdint>
#include <iterator>
#include <memory>
#include <utility>
#include <Eigen/Geometry>
+
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgcodecs.hpp>
#include <Ice/Current.h>
+#include <SimoxUtility/color/ColorMap.h>
+#include <SimoxUtility/color/cmaps/colormaps.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
+#include "ArmarXCore/core/Component.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -50,8 +55,10 @@
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+#include "RobotAPI/components/ArViz/Client/Client.h"
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
+#include "RobotAPI/interface/ArViz/Elements.h"
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem_vision/OccupancyGridHelper.h"
@@ -59,6 +66,9 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include "armarx/navigation/algorithms/CostmapBuilder.h"
+#include "armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h"
+#include "armarx/navigation/algorithms/util.h"
+#include "armarx/navigation/conversions/eigen.h"
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/astar/util.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
@@ -323,6 +333,7 @@ namespace armarx::navigation::components
<< "Navigator config for caller `" << callerId << "` not registered!";
updateContext();
+ visualizeSPFA();
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
// navigators.at(callerId).moveTo()
@@ -449,6 +460,47 @@ namespace armarx::navigation::components
return def;
}
+ void
+ visualize(const algorithms::Costmap& costmap, viz::Client& arviz, const std::string& name)
+ {
+ const auto cmap = simox::color::cmaps::viridis();
+ const float vmax = costmap.getGrid().array().maxCoeff();
+
+ const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
+ {
+ const auto color = cmap.at(distance, 0.F, vmax);
+ return {color.a, color.r, color.g, color.b};
+ };
+
+ auto layer = arviz.layer("costmap_" + name);
+
+ const std::int64_t cols = costmap.getGrid().cols();
+ const std::int64_t rows = costmap.getGrid().rows();
+
+ auto mesh = viz::Mesh("");
+
+ std::vector<std::vector<Eigen::Vector3f>> vertices;
+ std::vector<std::vector<viz::data::Color>> colors;
+
+ for (int r = 0; r < rows; r++)
+ {
+ auto& verticesRow = vertices.emplace_back(cols);
+ auto& colorsRow = colors.emplace_back(cols);
+
+ for (int c = 0; c < cols; c++)
+ {
+ verticesRow.at(c) = conv::to3D(costmap.toPosition({r, c}));
+ colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
+ }
+ }
+
+ mesh.grid2D(vertices, colors);
+
+ layer.add(mesh);
+
+ arviz.commit(layer);
+ }
+
core::StaticScene
components::Navigator::staticScene()
{
@@ -477,6 +529,10 @@ namespace armarx::navigation::components
ARMARX_INFO << "The object scene consists of " << scene.objects->getSize() << " objects";
+
+ visualize(costmap, arviz, "distance");
+
+
const armem::vision::occupancy_grid::client::Reader::Result result =
occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
.providerName = "CartographerMappingAndLocalization",
@@ -538,6 +594,24 @@ namespace armarx::navigation::components
return scene;
}
+ void components::Navigator::visualizeSPFA()
+ {
+ algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams{.obstacleDistanceCosts =
+ true};
+
+ ARMARX_INFO << "spfa...";
+ algorithms::spfa::ShortestPathFasterAlgorithm spfa(*scene.staticScene->costmap, spfaParams);
+ const auto result123 = spfa.spfa(getRobot()->getGlobalPosition().head<2>());
+ ARMARX_INFO << "spfa done...";
+
+ const auto sceneBounds = algorithms::computeSceneBounds(scene.staticScene->objects);
+
+ ARMARX_INFO << "Costmap SPFA";
+ algorithms::Costmap cm(result123.distances, algorithms::Costmap::Parameters{}, sceneBounds);
+ visualize(cm, arviz, "spfa");
+ ARMARX_INFO << "Done.";
+ }
+
VirtualRobot::RobotPtr
components::Navigator::getRobot()
{
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 2d7411d8..45785e2e 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -161,6 +161,7 @@ namespace armarx::navigation::components
server::Navigator* activeNavigator();
private:
+ void visualizeSPFA();
// TODO update context periodically
// TODO: Remove dependency to PlatformUnit. This component is generally
diff --git a/source/armarx/navigation/factories/GlobalPlannerFactory.cpp b/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
index 757b56e8..98a07a52 100644
--- a/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
+++ b/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
@@ -5,6 +5,7 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
+#include "armarx/navigation/global_planning/SPFA.h"
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/Point2Point.h>
@@ -38,8 +39,12 @@ namespace armarx::navigation::fac
switch (algo)
{
case global_planning::Algorithms::AStar:
- globalPlanner = std::make_shared<global_planning::AStar>(
- global_planning::AStarParams::FromAron(algoParams), ctx);
+ // globalPlanner = std::make_shared<global_planning::AStar>(
+ // global_planning::AStarParams::FromAron(algoParams), ctx);
+ // break;
+ case global_planning::Algorithms::SPFA:
+ globalPlanner = std::make_shared<global_planning::SPFA>(
+ global_planning::SPFAParams::FromAron(algoParams), ctx);
break;
case global_planning::Algorithms::Point2Point:
globalPlanner = std::make_shared<global_planning::Point2Point>(
diff --git a/source/armarx/navigation/global_planning/CMakeLists.txt b/source/armarx/navigation/global_planning/CMakeLists.txt
index 7100ea90..b3b248d6 100644
--- a/source/armarx/navigation/global_planning/CMakeLists.txt
+++ b/source/armarx/navigation/global_planning/CMakeLists.txt
@@ -3,6 +3,7 @@ armarx_add_aron_library(global_planning_aron
aron/GlobalPlannerParams.xml
aron/Point2PointParams.xml
aron/AStarParams.xml
+ aron/SPFAParams.xml
)
armarx_add_library(global_planning
@@ -20,12 +21,14 @@ armarx_add_library(global_planning
SOURCES
./GlobalPlanner.cpp
./AStar.cpp
+ ./SPFA.cpp
./Point2Point.cpp
./aron_conversions.cpp
./optimization/OrientationOptimizer.cpp
HEADERS
./GlobalPlanner.h
./AStar.h
+ ./SPFA.h
./Point2Point.h
./aron_conversions.h
./optimization/OrientationOptimizer.h
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
new file mode 100644
index 00000000..65efe579
--- /dev/null
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -0,0 +1,231 @@
+#include "SPFA.h"
+
+#include <algorithm>
+#include <mutex>
+#include <optional>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
+
+#include "armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h"
+#include <armarx/navigation/algorithms/smoothing/ChainApproximation.h>
+#include <armarx/navigation/algorithms/smoothing/CircularPathSmoothing.h>
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/core/constants.h>
+#include <armarx/navigation/global_planning/GlobalPlanner.h>
+#include <armarx/navigation/global_planning/aron/SPFAParams.aron.generated.h>
+#include <armarx/navigation/global_planning/aron_conversions.h>
+#include <armarx/navigation/global_planning/core.h>
+#include <armarx/navigation/global_planning/optimization/OrientationOptimizer.h>
+
+
+namespace armarx::navigation::global_planning
+{
+
+ // SPFAParams
+
+ Algorithms
+ SPFAParams::algorithm() const
+ {
+ return Algorithms::SPFA;
+ }
+
+ aron::data::DictPtr
+ SPFAParams::toAron() const
+ {
+ arondto::SPFAParams dto;
+
+ SPFAParams bo;
+ aron_conv::toAron(dto, bo);
+
+ return dto.toAron();
+ }
+
+ SPFAParams
+ SPFAParams::FromAron(const aron::data::DictPtr& dict)
+ {
+ ARMARX_CHECK_NOT_NULL(dict);
+
+ // ARMARX_DEBUG << dict->getAllKeysAsString();
+
+ arondto::SPFAParams dto;
+ dto.fromAron(dict);
+
+ SPFAParams bo;
+ aron_conv::fromAron(dto, bo);
+
+ return bo;
+ }
+
+ // SPFA
+
+ SPFA::SPFA(const SPFAParams& params, const core::Scene& ctx) :
+ GlobalPlanner(ctx), params(params)
+ {
+ }
+
+
+
+ std::optional<GlobalPlannerResult>
+ SPFA::plan(const core::Pose& goal)
+ {
+ const core::Pose start(scene.robot->getGlobalPose());
+ return plan(start, goal);
+ }
+
+ std::optional<GlobalPlannerResult>
+ SPFA::plan(const core::Pose& start, const core::Pose& goal)
+ {
+ ARMARX_TRACE;
+
+ std::lock_guard g{scene.staticSceneMtx};
+ // FIXME check if costmap is available
+
+ algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams;
+ algorithms::spfa::ShortestPathFasterAlgorithm planner(*scene.staticScene->costmap, spfaParams);
+
+ const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
+
+ const auto timeStart = IceUtil::Time::now();
+
+ std::vector<Eigen::Vector2f> plan;
+ try
+ {
+ plan = planner.plan(conv::to2D(start.translation()), goalPos);
+ }
+ catch (...)
+ {
+ ARMARX_INFO << "Could not plan collision-free path from"
+ << "(" << start.translation().x() << "," << start.translation().y() << ")"
+ << " to "
+ << "(" << goal.translation().x() << "," << goal.translation().y() << ")"
+ << " due to exception " << GetHandledExceptionString();
+
+ return std::nullopt;
+ }
+ const auto timeEnd = IceUtil::Time::now();
+
+ ARMARX_IMPORTANT << "A* planning took " << (timeEnd - timeStart).toMilliSeconds() << " ms";
+ ARMARX_IMPORTANT << "Path contains " << plan.size() << " points";
+
+ if (plan.size() < 2) // failure
+ {
+ ARMARX_INFO << "Could not plan collision-free path from"
+ << "(" << start.translation().x() << "," << start.translation().y() << ")"
+ << " to "
+ << "(" << goal.translation().x() << "," << goal.translation().y() << ")";
+ return std::nullopt;
+ }
+
+ ARMARX_DEBUG << "The plan consists of the following positions:";
+ for (const auto& position : plan)
+ {
+ ARMARX_DEBUG << position;
+ }
+
+ const core::Pose startPose(Eigen::Translation3f(conv::to3D(plan.front())));
+ const core::Pose goalPose(Eigen::Translation3f(conv::to3D(plan.back())));
+
+ const auto plan3d = conv::to3D(plan);
+
+ std::vector<core::Position> wpts;
+ for (size_t i = 1; i < (plan3d.size() - 1); i++)
+ {
+ wpts.push_back(plan3d.at(i));
+ }
+
+ ARMARX_TRACE;
+ auto smoothPlan = postProcessPath(plan);
+ ARMARX_IMPORTANT << "Smooth path contains " << smoothPlan.size() << " points";
+
+ ARMARX_TRACE;
+ // we need to strip the first and the last points from the plan as they encode the start and goal position
+ smoothPlan.erase(smoothPlan.begin());
+ smoothPlan.pop_back();
+
+ ARMARX_TRACE;
+ auto trajectory =
+ core::Trajectory::FromPath(start, conv::to3D(smoothPlan), goal, params.linearVelocity);
+
+ // TODO(fabian.reister): resampling of trajectory
+
+ // FIXME param
+ std::optional<core::Trajectory> resampledTrajectory;
+
+ try
+ {
+ resampledTrajectory = trajectory.resample(params.resampleDistance);
+ }
+ catch (...)
+ {
+ ARMARX_INFO << "Caught exception during resampling: " << GetHandledExceptionString();
+ // return std::nullopt;
+ resampledTrajectory = trajectory;
+ }
+
+ ARMARX_IMPORTANT << "Resampled trajectory contains " << resampledTrajectory->points().size()
+ << " points";
+
+ resampledTrajectory->setMaxVelocity(params.linearVelocity);
+
+
+ // ARMARX_CHECK(resampledTrajectory.hasMaxDistanceBetweenWaypoints(params.resampleDistance));
+
+ if (resampledTrajectory->points().size() == 2)
+ {
+ ARMARX_INFO << "Only start and goal provided. Not optimizing orientation";
+ ARMARX_TRACE;
+ return GlobalPlannerResult{.trajectory = resampledTrajectory.value()};
+ }
+
+ ARMARX_TRACE;
+ OrientationOptimizer optimizer(resampledTrajectory.value(), OrientationOptimizer::Params{});
+ const auto result = optimizer.optimize();
+
+ if (not result)
+ {
+ ARMARX_ERROR << "Optimizer failure";
+ return std::nullopt;
+ }
+
+ // TODO circular path smoothing should be done now
+
+ algorithm::CircularPathSmoothing smoothing;
+ auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
+ smoothTrajectory.setMaxVelocity(params.linearVelocity);
+
+ ARMARX_TRACE;
+ return GlobalPlannerResult{.trajectory = result.trajectory.value()};
+ }
+
+
+ std::vector<Eigen::Vector2f>
+ SPFA::postProcessPath(const std::vector<Eigen::Vector2f>& path)
+ {
+ /// chain approximation
+ algorithm::ChainApproximation approx(
+ path, algorithm::ChainApproximation::Params{.distanceTh = 200.F});
+ approx.approximate();
+ const auto p = approx.approximatedChain();
+
+ // visualizePath(p, "approximated", simox::Color::green());
+
+ // algo::CircularPathSmoothing smoothing;
+ // const auto points = smoothing.smooth(p);
+
+ return p;
+ }
+
+
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/SPFA.h b/source/armarx/navigation/global_planning/SPFA.h
new file mode 100644
index 00000000..b3af4fc1
--- /dev/null
+++ b/source/armarx/navigation/global_planning/SPFA.h
@@ -0,0 +1,71 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "GlobalPlanner.h"
+#include <armarx/navigation/core/types.h>
+
+namespace armarx::navigation::global_planning
+{
+
+ /**
+ * @brief Parameters for AStar
+ *
+ */
+ struct SPFAParams : public GlobalPlannerParams
+ {
+ float linearVelocity{500};
+ float resampleDistance{500};
+
+ Algorithms algorithm() const override;
+ aron::data::DictPtr toAron() const override;
+
+ static SPFAParams FromAron(const aron::data::DictPtr& dict);
+ };
+
+ /**
+ * @class AStar
+ * @ingroup Library-GlobalPlanner
+ *
+ * Implements the A* algorithm
+ */
+ class SPFA : public GlobalPlanner
+ {
+ public:
+ using Params = SPFAParams;
+
+ SPFA(const Params& params, const core::Scene& ctx);
+ ~SPFA() override = default;
+
+ std::optional<GlobalPlannerResult> plan(const core::Pose& goal) override;
+ std::optional<GlobalPlannerResult> plan(const core::Pose& start,
+ const core::Pose& goal) override;
+
+ protected:
+ std::vector<Eigen::Vector2f> postProcessPath(const std::vector<Eigen::Vector2f>& path);
+
+ private:
+ Params params;
+ };
+
+} // namespace armarx::navigation::global_planning
diff --git a/source/armarx/navigation/global_planning/aron/SPFAParams.xml b/source/armarx/navigation/global_planning/aron/SPFAParams.xml
new file mode 100644
index 00000000..b1a34a4d
--- /dev/null
+++ b/source/armarx/navigation/global_planning/aron/SPFAParams.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<armarx/navigation/global_planning/aron/GlobalPlannerParams.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="GlobalPlannerParams.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::global_planning::arondto::SPFAParams'>
+ <!-- <Object name='armarx::navigation::global_planning::arondto::SPFAParams' extends="armarx::navigation::global_planning::arondto::GlobalPlannerParams"> -->
+ <ObjectChild key='linearVelocity'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='resampleDistance'>
+ <float />
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/global_planning/aron_conversions.cpp b/source/armarx/navigation/global_planning/aron_conversions.cpp
index 84ec219a..358efefc 100644
--- a/source/armarx/navigation/global_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/global_planning/aron_conversions.cpp
@@ -7,9 +7,11 @@
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/Point2Point.h>
+#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/global_planning/aron/AStarParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron/GlobalPlannerParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron/Point2PointParams.aron.generated.h>
+#include <armarx/navigation/global_planning/aron/SPFAParams.aron.generated.h>
namespace armarx::navigation::global_planning::aron_conv
{
@@ -58,4 +60,25 @@ namespace armarx::navigation::global_planning::aron_conv
aron::fromAron(dto.linearVelocity, bo.linearVelocity);
aron::fromAron(dto.resampleDistance, bo.resampleDistance);
}
+
+ void
+ toAron(arondto::SPFAParams& dto, const SPFAParams& bo)
+ {
+ // toAron(static_cast<arondto::GlobalPlannerParams&>(dto),
+ // static_cast<const GlobalPlannerParams&>(bo));
+
+ aron::toAron(dto.linearVelocity, bo.linearVelocity);
+ aron::toAron(dto.resampleDistance, bo.resampleDistance);
+ }
+
+ void
+ fromAron(const arondto::SPFAParams& dto, SPFAParams& bo)
+ {
+ // fromAron(static_cast<const arondto::GlobalPlannerParams&>(dto),
+ // static_cast<GlobalPlannerParams&>(bo));
+
+ aron::fromAron(dto.linearVelocity, bo.linearVelocity);
+ aron::fromAron(dto.resampleDistance, bo.resampleDistance);
+ }
+
} // namespace armarx::navigation::global_planning::aron_conv
diff --git a/source/armarx/navigation/global_planning/aron_conversions.h b/source/armarx/navigation/global_planning/aron_conversions.h
index a27a6b02..d5b14e9d 100644
--- a/source/armarx/navigation/global_planning/aron_conversions.h
+++ b/source/armarx/navigation/global_planning/aron_conversions.h
@@ -5,12 +5,14 @@ namespace armarx::navigation::global_planning
struct GlobalPlannerParams;
struct Point2PointParams;
struct AStarParams;
+ struct SPFAParams;
namespace arondto
{
struct GlobalPlannerParams;
struct Point2PointParams;
struct AStarParams;
+ struct SPFAParams;
} // namespace arondto
@@ -27,4 +29,7 @@ namespace armarx::navigation::global_planning::aron_conv
void toAron(arondto::AStarParams& dto, const AStarParams& bo);
void fromAron(const arondto::AStarParams& dto, AStarParams& bo);
+ void toAron(arondto::SPFAParams& dto, const SPFAParams& bo);
+ void fromAron(const arondto::SPFAParams& dto, SPFAParams& bo);
+
} // namespace armarx::navigation::global_planning::aron_conv
diff --git a/source/armarx/navigation/global_planning/core.h b/source/armarx/navigation/global_planning/core.h
index 792e7b6f..1e21cbc2 100644
--- a/source/armarx/navigation/global_planning/core.h
+++ b/source/armarx/navigation/global_planning/core.h
@@ -38,6 +38,7 @@ namespace armarx::navigation::global_planning
enum class Algorithms
{
AStar, ///< see AStar
+ SPFA,
Point2Point ///< see Point2Point
};
--
GitLab
From d49c2a85d8a8e69ee2c400929c6e7080b0d75860 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 Jan 2022 14:55:55 +0100
Subject: [PATCH 04/11] formatting files
---
source/armarx/navigation/algorithms/Costmap.cpp | 1 -
source/armarx/navigation/algorithms/Costmap.h | 3 ++-
source/armarx/navigation/algorithms/CostmapBuilder.h | 3 ++-
.../navigation/algorithms/astar/AStarPlanner.cpp | 2 +-
.../armarx/navigation/algorithms/astar/AStarPlanner.h | 7 +++++--
source/armarx/navigation/algorithms/astar/Planner2D.h | 3 ++-
.../algorithms/spfa/ShortestPathFasterAlgorithm.cpp | 8 +++-----
.../algorithms/spfa/ShortestPathFasterAlgorithm.h | 4 +---
.../armarx/navigation/client/NavigationStackConfig.cpp | 4 ++--
.../navigation/client/services/MemorySubscriber.h | 2 +-
.../components/GraphImportExport/GraphImportExport.cpp | 2 +-
.../components/NavigationMemory/NavigationMemory.cpp | 2 +-
.../navigation/components/NavigationMemory/Visu.cpp | 2 +-
.../navigation/components/Navigator/Navigator.cpp | 9 +++++----
source/armarx/navigation/core/StaticScene.h | 1 +
source/armarx/navigation/core/Trajectory.cpp | 8 ++++----
source/armarx/navigation/core/json_conversions.cpp | 8 +++++---
source/armarx/navigation/core/json_conversions.h | 2 +-
source/armarx/navigation/core/test/coreTest.cpp | 4 ++--
source/armarx/navigation/core/types.h | 2 +-
.../navigation/factories/GlobalPlannerFactory.cpp | 9 +++------
.../armarx/navigation/factories/GlobalPlannerFactory.h | 4 ++--
.../navigation/factories/LocalPlannerFactory.cpp | 9 +++------
.../navigation/factories/NavigationStackFactory.cpp | 6 ++----
.../navigation/factories/NavigationStackFactory.h | 3 +--
.../navigation/factories/SafetyControllerFactory.cpp | 9 +++------
.../navigation/factories/SafetyControllerFactory.h | 4 ++--
.../factories/TrajectoryControllerFactory.cpp | 9 +++------
.../navigation/factories/TrajectoryControllerFactory.h | 4 ++--
.../armarx/navigation/factories/test/factoriesTest.cpp | 4 ++--
source/armarx/navigation/global_planning/AStar.cpp | 2 +-
source/armarx/navigation/global_planning/SPFA.cpp | 6 +++---
source/armarx/navigation/global_planning/core.h | 2 +-
.../navigation/global_planning/optimization/math.h | 2 +-
.../LocationGraphEditor/WidgetController.cpp | 2 +-
.../gui-plugins/LocationGraphEditor/WidgetController.h | 2 +-
.../armarx/navigation/memory/client/graph/Reader.cpp | 2 +-
source/armarx/navigation/memory/client/graph/Reader.h | 2 +-
.../memory/client/parameterization/Writer.cpp | 7 +++----
.../navigation/memory/client/parameterization/Writer.h | 3 +--
.../navigation/safety_control/LaserBasedProximity.h | 3 +--
.../server/introspection/ArvizIntrospector.h | 3 +--
.../server/introspection/MemoryIntrospector.h | 4 ++--
.../navigation/server/monitoring/GoalReachedMonitor.h | 2 +-
.../parameterization/MemoryParameterizationService.cpp | 3 +--
source/armarx/navigation/server/test/serverTest.cpp | 10 +++++-----
.../trajectory_control/TrajectoryFollowingController.h | 3 +--
source/armarx/navigation/util/util.cpp | 5 +++--
source/armarx/navigation/util/util.h | 5 +++--
49 files changed, 96 insertions(+), 110 deletions(-)
diff --git a/source/armarx/navigation/algorithms/Costmap.cpp b/source/armarx/navigation/algorithms/Costmap.cpp
index 74bc8aa2..67fde2b8 100644
--- a/source/armarx/navigation/algorithms/Costmap.cpp
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -63,5 +63,4 @@ namespace armarx::navigation::algorithms
}
-
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index ffd4edee..503ca9fb 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -51,7 +51,8 @@ namespace armarx::navigation::algorithms
bool isInCollision(const Position& p) const;
- const Parameters& params() const noexcept
+ const Parameters&
+ params() const noexcept
{
return parameters;
}
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.h b/source/armarx/navigation/algorithms/CostmapBuilder.h
index 674e1615..565ad526 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.h
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.h
@@ -22,6 +22,7 @@
#pragma once
#include <VirtualRobot/VirtualRobot.h>
+
#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::algorithms
@@ -40,7 +41,7 @@ namespace armarx::navigation::algorithms
private:
Eigen::MatrixXf createUniformGrid(const SceneBounds& sceneBounds,
const Costmap::Parameters& parameters);
-
+
float computeCost(const Costmap::Position& position,
VirtualRobot::RobotPtr& collisionRobot,
const VirtualRobot::CollisionModelPtr& robotCollisionModel);
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index bddc5157..10d338ec 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -75,7 +75,7 @@ namespace armarx::navigation::algorithm::astar
{
const auto pos = costmap.toPosition({r, c});
const float obstacleDistance = costmap.getGrid()(r, c);
-
+
grid[r][c] = std::make_shared<Node>(pos, obstacleDistance);
}
}
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.h b/source/armarx/navigation/algorithms/astar/AStarPlanner.h
index 9f6e7496..87256085 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.h
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.h
@@ -21,7 +21,8 @@ namespace armarx::navigation::algorithm::astar
AStarPlanner(const algorithms::Costmap& costmap);
- std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal) /* override */;
+ std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start,
+ const Eigen::Vector2f& goal) /* override */;
private:
float heuristic(const NodePtr& n1, const NodePtr& n2);
@@ -31,7 +32,9 @@ namespace armarx::navigation::algorithm::astar
float costs(const NodePtr& a, const NodePtr& b) const;
- float computeObstacleDistance(const Eigen::Vector2f& pos, VirtualRobot::RobotPtr& robot) const;
+ float computeObstacleDistance(const Eigen::Vector2f& pos,
+ VirtualRobot::RobotPtr& robot) const;
+
private:
const algorithms::Costmap costmap;
diff --git a/source/armarx/navigation/algorithms/astar/Planner2D.h b/source/armarx/navigation/algorithms/astar/Planner2D.h
index 5fb3b7e6..cd7b0540 100644
--- a/source/armarx/navigation/algorithms/astar/Planner2D.h
+++ b/source/armarx/navigation/algorithms/astar/Planner2D.h
@@ -26,7 +26,8 @@ namespace armarx::navigation::algorithm::astar
virtual ~Planner2D() = default;
// planners implement this method
- virtual std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal) = 0;
+ virtual std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start,
+ const Eigen::Vector2f& goal) = 0;
/// Update obstacles
void setObstacles(VirtualRobot::SceneObjectSetPtr obstacles);
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
index 8ba7681d..692eec77 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
@@ -190,11 +190,10 @@ namespace armarx::navigation::algorithms::spfa
const float travelCost = dir_lengths[k];
// const float obstacleDistanceCost = std::max(obstacleDistance1 - obstacleDistance2, 0.F) / costmap.params().cellSize;
- const float targetDistanceCost = 3* (1000.F - obstacleDistance2) / 1000.F;
+ const float targetDistanceCost = 3 * (1000.F - obstacleDistance2) / 1000.F;
- const float edgeCost = params.obstacleDistanceCosts
- ? travelCost + targetDistanceCost
- : travelCost;
+ const float edgeCost =
+ params.obstacleDistanceCosts ? travelCost + targetDistanceCost : travelCost;
int e = ravel(v, edge_counts[v], max_edges_per_vert);
edges[e] = vp;
@@ -277,7 +276,6 @@ namespace armarx::navigation::algorithms::spfa
ARMARX_INFO << "Done.";
return Result{.distances = output_dists, .parents = output_parents};
- // return py::make_tuple(output_dists, output_parents);
}
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
index 85909558..9bfa9eec 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
@@ -39,8 +39,7 @@ namespace armarx::navigation::algorithms::spfa
bool obstacleDistanceCosts{true};
};
- ShortestPathFasterAlgorithm(const Costmap& grid,
- const Parameters& params);
+ ShortestPathFasterAlgorithm(const Costmap& grid, const Parameters& params);
struct Result
{
@@ -53,7 +52,6 @@ namespace armarx::navigation::algorithms::spfa
const Eigen::Vector2f& goal);
-
Result spfa(const Eigen::Vector2f& start);
// protected:
diff --git a/source/armarx/navigation/client/NavigationStackConfig.cpp b/source/armarx/navigation/client/NavigationStackConfig.cpp
index 6acdc7f0..bfaaf93b 100644
--- a/source/armarx/navigation/client/NavigationStackConfig.cpp
+++ b/source/armarx/navigation/client/NavigationStackConfig.cpp
@@ -10,7 +10,6 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
-
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/core/types.h>
@@ -24,7 +23,8 @@ namespace armarx::navigation::client
return element;
}
- aron::data::dto::DictPtr NavigationStackConfig::toAron() const
+ aron::data::dto::DictPtr
+ NavigationStackConfig::toAron() const
{
ARMARX_CHECK(isValid())
<< "The NavigationStackConfig is not valid as some elements are not set!";
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.h b/source/armarx/navigation/client/services/MemorySubscriber.h
index 059305dd..c7d79e14 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.h
+++ b/source/armarx/navigation/client/services/MemorySubscriber.h
@@ -41,7 +41,7 @@ namespace armarx::navigation::client
void onEntityUpdate(const armem::MemoryID& subscriptionID,
const std::vector<armem::MemoryID>& snapshotIDs);
-
+
private:
const std::string callerId;
armem::client::MemoryNameSystem& memoryNameSystem;
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index e10c6e3f..bf2bb976 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -37,9 +37,9 @@
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
+#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/Visu.h>
#include <armarx/navigation/graph/constants.h>
-#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/location/constants.h>
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index 292b497c..799c5604 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -36,10 +36,10 @@
#include "Visu.h"
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
+#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
-#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/location/constants.h>
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.cpp b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
index 328e21ce..07f364bc 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
@@ -26,8 +26,8 @@
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
-#include <armarx/navigation/graph/Visu.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
+#include <armarx/navigation/graph/Visu.h>
namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index db4c14da..13718710 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -594,11 +594,12 @@ namespace armarx::navigation::components
return scene;
}
- void components::Navigator::visualizeSPFA()
+ void
+ components::Navigator::visualizeSPFA()
{
- algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams{.obstacleDistanceCosts =
- true};
-
+ algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams{
+ .obstacleDistanceCosts = true};
+
ARMARX_INFO << "spfa...";
algorithms::spfa::ShortestPathFasterAlgorithm spfa(*scene.staticScene->costmap, spfaParams);
const auto result123 = spfa.spfa(getRobot()->getGlobalPosition().head<2>());
diff --git a/source/armarx/navigation/core/StaticScene.h b/source/armarx/navigation/core/StaticScene.h
index f70a2451..1f4ad33a 100644
--- a/source/armarx/navigation/core/StaticScene.h
+++ b/source/armarx/navigation/core/StaticScene.h
@@ -25,6 +25,7 @@
#include <memory>
#include <VirtualRobot/VirtualRobot.h>
+
#include <armarx/navigation/algorithms/Costmap.h>
namespace armarx::navigation::core
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index 9f5679a0..e7bf3ea1 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -589,11 +589,11 @@ namespace armarx::navigation::core
return 0; // FIXME
}
- bool Trajectory::isValid() const noexcept
+ bool
+ Trajectory::isValid() const noexcept
{
- const auto isValid = [](const TrajectoryPoint& pt) -> bool {
- return std::isfinite(pt.velocity) and pt.velocity < 3000;
- };
+ const auto isValid = [](const TrajectoryPoint& pt) -> bool
+ { return std::isfinite(pt.velocity) and pt.velocity < 3000; };
return std::all_of(pts.begin(), pts.end(), isValid);
}
diff --git a/source/armarx/navigation/core/json_conversions.cpp b/source/armarx/navigation/core/json_conversions.cpp
index 30b8a70f..661331fb 100644
--- a/source/armarx/navigation/core/json_conversions.cpp
+++ b/source/armarx/navigation/core/json_conversions.cpp
@@ -21,21 +21,23 @@
#include "json_conversions.h"
-#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
namespace armarx::navigation::core
{
- void arondto::to_json(nlohmann::json& j, const Edge& bo)
+ void
+ arondto::to_json(nlohmann::json& j, const Edge& bo)
{
armarx::aron::data::writer::NlohmannJSONWriter writer;
j = bo.write(writer);
}
- void arondto::from_json(const nlohmann::json& j, Edge& bo)
+ void
+ arondto::from_json(const nlohmann::json& j, Edge& bo)
{
armarx::aron::data::reader::NlohmannJSONReader reader;
bo.read<armarx::aron::data::reader::NlohmannJSONReader::InputType>(reader, j);
diff --git a/source/armarx/navigation/core/json_conversions.h b/source/armarx/navigation/core/json_conversions.h
index 57cee75d..a8fd2caa 100644
--- a/source/armarx/navigation/core/json_conversions.h
+++ b/source/armarx/navigation/core/json_conversions.h
@@ -33,4 +33,4 @@ namespace armarx::navigation::core::arondto
void from_json(const nlohmann::json& j, arondto::Edge& bo);
-} // namespace armarx::navigation::graph
+} // namespace armarx::navigation::core::arondto
diff --git a/source/armarx/navigation/core/test/coreTest.cpp b/source/armarx/navigation/core/test/coreTest.cpp
index 8e323a97..c62597ce 100644
--- a/source/armarx/navigation/core/test/coreTest.cpp
+++ b/source/armarx/navigation/core/test/coreTest.cpp
@@ -24,11 +24,11 @@
#include <algorithm>
#include <vector>
+#include <range/v3/view/zip.hpp>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <range/v3/view/zip.hpp>
-
#include <SemanticObjectRelations/Shapes/Shape.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/Trajectory.h>
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index 464966df..0e5b96fa 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -80,7 +80,7 @@ namespace armarx::navigation::core
{
mutable std::mutex staticSceneMtx;
std::optional<core::StaticScene> staticScene = std::nullopt;
-
+
mutable std::mutex dynamicSceneMtx;
std::optional<core::DynamicScene> dynamicScene = std::nullopt;
// TopologicMapPtr topologicMap;
diff --git a/source/armarx/navigation/factories/GlobalPlannerFactory.cpp b/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
index 98a07a52..f6e20580 100644
--- a/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
+++ b/source/armarx/navigation/factories/GlobalPlannerFactory.cpp
@@ -14,8 +14,7 @@
namespace armarx::navigation::fac
{
global_planning::GlobalPlannerPtr
- GlobalPlannerFactory::create(const aron::data::DictPtr& params,
- const core::Scene& ctx)
+ GlobalPlannerFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
{
namespace layer = global_planning;
@@ -25,14 +24,12 @@ namespace armarx::navigation::fac
}
// algo name
- const auto algoName =
- aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
+ const auto algoName = aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
ARMARX_CHECK_NOT_NULL(algoName);
const layer::Algorithms algo = layer::AlgorithmNames.from_name(algoName->getValue());
// algo params
- const auto algoParams =
- aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
+ const auto algoParams = aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
ARMARX_CHECK_NOT_NULL(algoParams);
global_planning::GlobalPlannerPtr globalPlanner;
diff --git a/source/armarx/navigation/factories/GlobalPlannerFactory.h b/source/armarx/navigation/factories/GlobalPlannerFactory.h
index 4ef9a823..da60c839 100644
--- a/source/armarx/navigation/factories/GlobalPlannerFactory.h
+++ b/source/armarx/navigation/factories/GlobalPlannerFactory.h
@@ -36,8 +36,8 @@ namespace armarx::navigation::fac
class GlobalPlannerFactory
{
public:
- static global_planning::GlobalPlannerPtr
- create(const aron::data::DictPtr& params, const core::Scene& ctx);
+ static global_planning::GlobalPlannerPtr create(const aron::data::DictPtr& params,
+ const core::Scene& ctx);
};
} // namespace armarx::navigation::fac
diff --git a/source/armarx/navigation/factories/LocalPlannerFactory.cpp b/source/armarx/navigation/factories/LocalPlannerFactory.cpp
index e386f55d..aed7f192 100644
--- a/source/armarx/navigation/factories/LocalPlannerFactory.cpp
+++ b/source/armarx/navigation/factories/LocalPlannerFactory.cpp
@@ -11,8 +11,7 @@
namespace armarx::navigation::fac
{
loc_plan::LocalPlannerPtr
- LocalPlannerFactory::create(const aron::data::DictPtr& params,
- const core::Scene& ctx)
+ LocalPlannerFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
{
namespace layer = loc_plan;
@@ -22,14 +21,12 @@ namespace armarx::navigation::fac
}
// algo name
- const auto algoName =
- aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
+ const auto algoName = aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
ARMARX_CHECK_NOT_NULL(algoName);
const layer::Algorithms algo = layer::AlgorithmNames.from_name(algoName->getValue());
// algo params
- const auto algoParams =
- aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
+ const auto algoParams = aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
ARMARX_CHECK_NOT_NULL(algoParams);
loc_plan::LocalPlannerPtr localPlanner;
diff --git a/source/armarx/navigation/factories/NavigationStackFactory.cpp b/source/armarx/navigation/factories/NavigationStackFactory.cpp
index e6c90a5c..665bfecb 100644
--- a/source/armarx/navigation/factories/NavigationStackFactory.cpp
+++ b/source/armarx/navigation/factories/NavigationStackFactory.cpp
@@ -14,13 +14,11 @@
namespace armarx::navigation::fac
{
server::NavigationStack
- NavigationStackFactory::create(const aron::data::DictPtr& params,
- const core::Scene& ctx)
+ NavigationStackFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
{
using aron::data::Dict;
- const auto getElementOrNull =
- [¶ms](const core::StackLayer& layer) -> Dict::PointerType
+ const auto getElementOrNull = [¶ms](const core::StackLayer& layer) -> Dict::PointerType
{
const std::string key = core::StackLayerNames.to_name(layer);
diff --git a/source/armarx/navigation/factories/NavigationStackFactory.h b/source/armarx/navigation/factories/NavigationStackFactory.h
index f0ee4e19..abd6be41 100644
--- a/source/armarx/navigation/factories/NavigationStackFactory.h
+++ b/source/armarx/navigation/factories/NavigationStackFactory.h
@@ -22,11 +22,10 @@
#pragma once
-#include <armarx/navigation/server/NavigationStack.h>
-
#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
#include <armarx/navigation/core/fwd.h>
+#include <armarx/navigation/server/NavigationStack.h>
namespace armarx::navigation::fac
{
diff --git a/source/armarx/navigation/factories/SafetyControllerFactory.cpp b/source/armarx/navigation/factories/SafetyControllerFactory.cpp
index 8dec9fb3..a4173c42 100644
--- a/source/armarx/navigation/factories/SafetyControllerFactory.cpp
+++ b/source/armarx/navigation/factories/SafetyControllerFactory.cpp
@@ -13,8 +13,7 @@
namespace armarx::navigation::fac
{
safe_ctrl::SafetyControllerPtr
- SafetyControllerFactory::create(const aron::data::DictPtr& params,
- const core::Scene& ctx)
+ SafetyControllerFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
{
namespace layer = safe_ctrl;
@@ -24,14 +23,12 @@ namespace armarx::navigation::fac
}
// algo name
- const auto algoName =
- aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
+ const auto algoName = aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
ARMARX_CHECK_NOT_NULL(algoName);
const layer::Algorithms algo = layer::AlgorithmNames.from_name(algoName->getValue());
// algo params
- const auto algoParams =
- aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
+ const auto algoParams = aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
ARMARX_CHECK_NOT_NULL(algoParams);
safe_ctrl::SafetyControllerPtr controller;
diff --git a/source/armarx/navigation/factories/SafetyControllerFactory.h b/source/armarx/navigation/factories/SafetyControllerFactory.h
index 023a7624..02c427c9 100644
--- a/source/armarx/navigation/factories/SafetyControllerFactory.h
+++ b/source/armarx/navigation/factories/SafetyControllerFactory.h
@@ -37,8 +37,8 @@ namespace armarx::navigation::fac
class SafetyControllerFactory
{
public:
- static safe_ctrl::SafetyControllerPtr
- create(const aron::data::DictPtr& params, const core::Scene& ctx);
+ static safe_ctrl::SafetyControllerPtr create(const aron::data::DictPtr& params,
+ const core::Scene& ctx);
protected:
private:
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
index c30fe546..277d277d 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.cpp
@@ -12,8 +12,7 @@
namespace armarx::navigation::fac
{
traj_ctrl::TrajectoryControllerPtr
- TrajectoryControllerFactory::create(const aron::data::DictPtr& params,
- const core::Scene& ctx)
+ TrajectoryControllerFactory::create(const aron::data::DictPtr& params, const core::Scene& ctx)
{
namespace layer = traj_ctrl;
@@ -23,14 +22,12 @@ namespace armarx::navigation::fac
}
// algo name
- const auto algoName =
- aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
+ const auto algoName = aron::data::String::DynamicCast(params->getElement(core::NAME_KEY));
ARMARX_CHECK_NOT_NULL(algoName);
const layer::Algorithms algo = layer::AlgorithmNames.from_name(algoName->getValue());
// algo params
- const auto algoParams =
- aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
+ const auto algoParams = aron::data::Dict::DynamicCast(params->getElement(core::PARAMS_KEY));
ARMARX_CHECK_NOT_NULL(algoParams);
traj_ctrl::TrajectoryControllerPtr controller;
diff --git a/source/armarx/navigation/factories/TrajectoryControllerFactory.h b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
index eb603d5f..16328e1c 100644
--- a/source/armarx/navigation/factories/TrajectoryControllerFactory.h
+++ b/source/armarx/navigation/factories/TrajectoryControllerFactory.h
@@ -38,8 +38,8 @@ namespace armarx::navigation::fac
class TrajectoryControllerFactory
{
public:
- static traj_ctrl::TrajectoryControllerPtr
- create(const aron::data::DictPtr& params, const core::Scene& ctx);
+ static traj_ctrl::TrajectoryControllerPtr create(const aron::data::DictPtr& params,
+ const core::Scene& ctx);
protected:
private:
diff --git a/source/armarx/navigation/factories/test/factoriesTest.cpp b/source/armarx/navigation/factories/test/factoriesTest.cpp
index adb15278..2e7d7fdb 100644
--- a/source/armarx/navigation/factories/test/factoriesTest.cpp
+++ b/source/armarx/navigation/factories/test/factoriesTest.cpp
@@ -66,8 +66,8 @@ BOOST_AUTO_TEST_CASE(testStackCreation)
BOOST_CHECK(navStack.globalPlanner.get() != nullptr);
BOOST_CHECK(navStack.trajectoryControl.get() != nullptr);
- BOOST_CHECK(dynamic_cast<armarx::navigation::global_planning::AStar*>(navStack.globalPlanner.get()) !=
- nullptr);
+ BOOST_CHECK(dynamic_cast<armarx::navigation::global_planning::AStar*>(
+ navStack.globalPlanner.get()) != nullptr);
BOOST_CHECK(dynamic_cast<armarx::navigation::traj_ctrl::TrajectoryFollowingController*>(
navStack.trajectoryControl.get()) != nullptr);
}
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 16ca1777..dde26d2f 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -161,7 +161,7 @@ namespace armarx::navigation::global_planning
std::lock_guard g{scene.staticSceneMtx};
// FIXME check if costmap is available
algorithm::astar::AStarPlanner planner(*scene.staticScene->costmap);
-
+
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
const auto timeStart = IceUtil::Time::now();
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
index 65efe579..26363016 100644
--- a/source/armarx/navigation/global_planning/SPFA.cpp
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -75,7 +75,6 @@ namespace armarx::navigation::global_planning
{
}
-
std::optional<GlobalPlannerResult>
SPFA::plan(const core::Pose& goal)
@@ -93,8 +92,9 @@ namespace armarx::navigation::global_planning
// FIXME check if costmap is available
algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams;
- algorithms::spfa::ShortestPathFasterAlgorithm planner(*scene.staticScene->costmap, spfaParams);
-
+ algorithms::spfa::ShortestPathFasterAlgorithm planner(*scene.staticScene->costmap,
+ spfaParams);
+
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
const auto timeStart = IceUtil::Time::now();
diff --git a/source/armarx/navigation/global_planning/core.h b/source/armarx/navigation/global_planning/core.h
index 1e21cbc2..f8b6eaf7 100644
--- a/source/armarx/navigation/global_planning/core.h
+++ b/source/armarx/navigation/global_planning/core.h
@@ -37,7 +37,7 @@ namespace armarx::navigation::global_planning
*/
enum class Algorithms
{
- AStar, ///< see AStar
+ AStar, ///< see AStar
SPFA,
Point2Point ///< see Point2Point
};
diff --git a/source/armarx/navigation/global_planning/optimization/math.h b/source/armarx/navigation/global_planning/optimization/math.h
index 2007edb3..c8854e1f 100644
--- a/source/armarx/navigation/global_planning/optimization/math.h
+++ b/source/armarx/navigation/global_planning/optimization/math.h
@@ -61,7 +61,7 @@ namespace armarx::navigation::global_planning::optimization
1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b), 2);
// prevent division by 0
- if(denomSquared < 0.001)
+ if (denomSquared < 0.001)
{
return 0.;
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
index b4487009..894b527d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.cpp
@@ -41,9 +41,9 @@
#include "widgets/graph_scene/Widget.h"
#include "widgets/utils.h"
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
+#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
-#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/location/constants.h>
// Qt headers
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.h
index f06f0bde..080b9617 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/WidgetController.h
@@ -40,8 +40,8 @@
#include "Visu.h"
#include "widgets/graph_scene.h"
#include <armarx/navigation/core/Graph.h>
-#include <armarx/navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
class QDialog;
diff --git a/source/armarx/navigation/memory/client/graph/Reader.cpp b/source/armarx/navigation/memory/client/graph/Reader.cpp
index 7d3513a1..98049272 100644
--- a/source/armarx/navigation/memory/client/graph/Reader.cpp
+++ b/source/armarx/navigation/memory/client/graph/Reader.cpp
@@ -14,8 +14,8 @@
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
-#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
+#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
namespace armarx::navigation::mem::client::graph
diff --git a/source/armarx/navigation/memory/client/graph/Reader.h b/source/armarx/navigation/memory/client/graph/Reader.h
index 9a65ed9f..a61bc7c6 100644
--- a/source/armarx/navigation/memory/client/graph/Reader.h
+++ b/source/armarx/navigation/memory/client/graph/Reader.h
@@ -24,8 +24,8 @@
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
#include <armarx/navigation/core/Graph.h>
-#include <armarx/navigation/core/types.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
+#include <armarx/navigation/core/types.h>
namespace armarx::navigation::mem::client::graph
{
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.cpp b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
index d40f300f..b1b8610d 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.cpp
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
@@ -8,10 +8,9 @@
namespace armarx::navigation::mem::client::param
{
bool
- Writer::store(
- const std::unordered_map<core::StackLayer, aron::data::DictPtr>& stack,
- const std::string& clientID,
- const core::TimestampUs& timestamp)
+ Writer::store(const std::unordered_map<core::StackLayer, aron::data::DictPtr>& stack,
+ const std::string& clientID,
+ const core::TimestampUs& timestamp)
{
ARMARX_CHECK(not stack.empty());
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.h b/source/armarx/navigation/memory/client/parameterization/Writer.h
index d48c3e80..eda0e83e 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.h
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.h
@@ -36,8 +36,7 @@ namespace armarx::navigation::mem::client::param
using armem::client::util::SimpleWriterBase::SimpleWriterBase;
- bool store(const std::unordered_map<core::StackLayer,
- aron::data::DictPtr>& stack,
+ bool store(const std::unordered_map<core::StackLayer, aron::data::DictPtr>& stack,
const std::string& clientID,
const core::TimestampUs& timestamp);
diff --git a/source/armarx/navigation/safety_control/LaserBasedProximity.h b/source/armarx/navigation/safety_control/LaserBasedProximity.h
index 94108ade..1f1cf519 100644
--- a/source/armarx/navigation/safety_control/LaserBasedProximity.h
+++ b/source/armarx/navigation/safety_control/LaserBasedProximity.h
@@ -33,8 +33,7 @@ namespace armarx::navigation::safe_ctrl
Algorithms algorithm() const override;
aron::data::DictPtr toAron() const override;
- static LaserBasedProximityParams
- FromAron(const aron::data::DictPtr& dict);
+ static LaserBasedProximityParams FromAron(const aron::data::DictPtr& dict);
};
class LaserBasedProximity : virtual public SafetyController
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index 1cd9cf12..39d45649 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -59,7 +59,7 @@ namespace armarx::navigation::server
void onSafetyControllerResult(const safe_ctrl::SafetyControllerResult& result) override;
void onGoal(const core::Pose& goal) override;
-
+
void success();
void failure();
@@ -68,7 +68,6 @@ namespace armarx::navigation::server
void onGlobalGraph(const core::Graph& graph) override;
-
// // Non-API
ArvizIntrospector(ArvizIntrospector&& other) noexcept;
ArvizIntrospector& operator=(ArvizIntrospector&&) noexcept;
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index c153b521..18ecf769 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -46,8 +46,8 @@ namespace armarx::navigation::server
void onGoal(const core::Pose& goal) override;
- void success() override {};
- void failure() override {};
+ void success() override{};
+ void failure() override{};
void
onGlobalShortestPath(const std::vector<core::Pose>& path) override
diff --git a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
index 9382cbae..34f9e49d 100644
--- a/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
+++ b/source/armarx/navigation/server/monitoring/GoalReachedMonitor.h
@@ -33,7 +33,7 @@ namespace armarx::navigation::server
struct GoalReachedMonitorConfig
{
- float posTh{70.F}; // [mm]
+ float posTh{70.F}; // [mm]
float oriTh{VirtualRobot::MathTools::deg2rad(5.F)}; // [rad]
float linearVelTh{100.F}; // [mm/s]
diff --git a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
index 94bbd51b..68067ad9 100644
--- a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
+++ b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
@@ -19,8 +19,7 @@ namespace armarx::navigation::server
using aron::data::Dict;
- const auto getElementOrNull =
- [¶ms](const core::StackLayer& layer) -> Dict::PointerType
+ const auto getElementOrNull = [¶ms](const core::StackLayer& layer) -> Dict::PointerType
{
const std::string key = core::StackLayerNames.to_name(layer);
diff --git a/source/armarx/navigation/server/test/serverTest.cpp b/source/armarx/navigation/server/test/serverTest.cpp
index 7bcfcc6d..5fb6ce92 100644
--- a/source/armarx/navigation/server/test/serverTest.cpp
+++ b/source/armarx/navigation/server/test/serverTest.cpp
@@ -55,11 +55,11 @@ BOOST_AUTO_TEST_CASE(testNavigator)
scene.robot->setGlobalPose(Eigen::Matrix4f::Identity(), false);
// TODO create static shared ctor
- server::NavigationStack stack{
- .globalPlanner =
- std::make_shared<global_planning::Point2Point>(global_planning::Point2PointParams(), scene),
- .trajectoryControl = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
- traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
+ server::NavigationStack stack{.globalPlanner = std::make_shared<global_planning::Point2Point>(
+ global_planning::Point2PointParams(), scene),
+ .trajectoryControl =
+ std::make_shared<traj_ctrl::TrajectoryFollowingController>(
+ traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
// Executor.
server::DummyExecutor executor{scene.robot, server::DummyExecutor::Params()};
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
index 064de2fc..a9a8a94c 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
@@ -40,8 +40,7 @@ namespace armarx::navigation::traj_ctrl
Algorithms algorithm() const override;
aron::data::DictPtr toAron() const override;
- static TrajectoryFollowingControllerParams
- FromAron(const aron::data::DictPtr& dict);
+ static TrajectoryFollowingControllerParams FromAron(const aron::data::DictPtr& dict);
};
class TrajectoryFollowingController : virtual public TrajectoryController
diff --git a/source/armarx/navigation/util/util.cpp b/source/armarx/navigation/util/util.cpp
index ba409ab3..3438ce67 100644
--- a/source/armarx/navigation/util/util.cpp
+++ b/source/armarx/navigation/util/util.cpp
@@ -37,9 +37,9 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::navigation::util
{
@@ -63,7 +63,8 @@ namespace armarx::navigation::util
}
VirtualRobot::SceneObjectSetPtr
- asSceneObjects(const armem::OccupancyGrid& occupancyGrid, const OccupancyGridHelper::Params& params)
+ asSceneObjects(const armem::OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params)
{
const OccupancyGridHelper ocHelper(occupancyGrid, params);
const auto obstacles = ocHelper.obstacles();
diff --git a/source/armarx/navigation/util/util.h b/source/armarx/navigation/util/util.h
index c15487f1..805b7f7c 100644
--- a/source/armarx/navigation/util/util.h
+++ b/source/armarx/navigation/util/util.h
@@ -25,13 +25,14 @@
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
namespace armarx::navigation::util
{
VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
- VirtualRobot::SceneObjectSetPtr asSceneObjects(const armem::OccupancyGrid& occupancyGrid, const OccupancyGridHelper::Params& params);
+ VirtualRobot::SceneObjectSetPtr asSceneObjects(const armem::OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params);
} // namespace armarx::navigation::util
--
GitLab
From 9d4053129e8e0c9f5fbe39b8aa9fbafb18c26bf5 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 Jan 2022 18:56:42 +0100
Subject: [PATCH 05/11] util stuff
---
source/armarx/navigation/algorithms/Costmap.h | 6 +++++
.../navigation/algorithms/CostmapBuilder.cpp | 2 ++
.../navigation/algorithms/CostmapBuilder.h | 2 +-
.../spfa/ShortestPathFasterAlgorithm.h | 2 +-
source/armarx/navigation/algorithms/util.cpp | 23 +++++++++++++++++++
source/armarx/navigation/algorithms/util.h | 10 +++++++-
6 files changed, 42 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 503ca9fb..730a7175 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -57,6 +57,12 @@ namespace armarx::navigation::algorithms
return parameters;
}
+ const SceneBounds&
+ getSceneBounds() const noexcept
+ {
+ return sceneBounds;
+ }
+
private:
Grid grid;
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.cpp b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
index 2e9b9b1f..9c40a93d 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.cpp
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
@@ -195,6 +195,8 @@ namespace armarx::navigation::algorithms
ARMARX_CHECK_NOT_NULL(collisionRobotNode);
robotCollisionModel = collisionRobotNode->getCollisionModel();
+ ARMARX_CHECK(robotCollisionModel) << "Collision model not available. "
+ "Make sure that you load the robot correctly!";
}
ARMARX_CHECK_NOT_NULL(collisionRobot);
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.h b/source/armarx/navigation/algorithms/CostmapBuilder.h
index 565ad526..ff6ef9d2 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.h
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.h
@@ -36,7 +36,7 @@ namespace armarx::navigation::algorithms
const Costmap::Parameters& parameters,
const std::string& robotCollisonModelName);
- Costmap create();
+ [[nodiscard]] Costmap create();
private:
Eigen::MatrixXf createUniformGrid(const SceneBounds& sceneBounds,
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
index 9bfa9eec..65a458c3 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
@@ -39,7 +39,7 @@ namespace armarx::navigation::algorithms::spfa
bool obstacleDistanceCosts{true};
};
- ShortestPathFasterAlgorithm(const Costmap& grid, const Parameters& params);
+ ShortestPathFasterAlgorithm(const Costmap& costmap, const Parameters& params);
struct Result
{
diff --git a/source/armarx/navigation/algorithms/util.cpp b/source/armarx/navigation/algorithms/util.cpp
index 53a3aafa..843d40eb 100644
--- a/source/armarx/navigation/algorithms/util.cpp
+++ b/source/armarx/navigation/algorithms/util.cpp
@@ -24,6 +24,7 @@
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/Workspace/WorkspaceGrid.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
@@ -52,4 +53,26 @@ namespace armarx::navigation::algorithms
}
+ armarx::navigation::algorithms::SceneBounds
+ toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends)
+ {
+ return {.min = Eigen::Vector2f{extends.minX, extends.minY},
+ .max = Eigen::Vector2f{extends.maxX, extends.maxY}};
+ }
+
+ armarx::navigation::algorithms::Costmap
+ toCostmap(const VirtualRobot::WorkspaceGrid& workspaceGrid)
+ {
+ const armarx::navigation::algorithms::Costmap::Grid grid(workspaceGrid.getCells().x,
+ workspaceGrid.getCells().y);
+ const armarx::navigation::algorithms::SceneBounds sceneBounds =
+ toSceneBounds(workspaceGrid.getExtends());
+
+ const armarx::navigation::algorithms::Costmap::Parameters parameters{
+ .binaryGrid = false, .cellSize = workspaceGrid.getDiscretizeSize()};
+
+ return {grid, parameters, sceneBounds};
+ }
+
+
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/util.h b/source/armarx/navigation/algorithms/util.h
index add037fc..cee91e0c 100644
--- a/source/armarx/navigation/algorithms/util.h
+++ b/source/armarx/navigation/algorithms/util.h
@@ -23,12 +23,20 @@
#pragma once
#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Workspace/WorkspaceGrid.h>
+#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/types.h>
+
namespace armarx::navigation::algorithms
{
-
SceneBounds computeSceneBounds(const VirtualRobot::SceneObjectSetPtr& obstacles);
+ armarx::navigation::algorithms::SceneBounds
+ toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends);
+
+ armarx::navigation::algorithms::Costmap
+ toCostmap(const VirtualRobot::WorkspaceGrid& workspaceGrid);
+
} // namespace armarx::navigation::algorithms
--
GitLab
From eefd681bdf9adc57e19752a84f993a3b064241ee Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 Jan 2022 21:41:35 +0100
Subject: [PATCH 06/11] clang-tidy: ignore
cppcoreguidelines-narrowing-conversions
---
.clang-tidy | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/.clang-tidy b/.clang-tidy
index 6e80b902..2a651563 100644
--- a/.clang-tidy
+++ b/.clang-tidy
@@ -26,7 +26,8 @@ Checks: '
-cppcoreguidelines-pro-bounds-array-to-pointer-decay,
-cppcoreguidelines-pro-type-member-init,
-cppcoreguidelines-special-member-functions,
- -cppcoreguidelines-owning-memory
+ -cppcoreguidelines-owning-memory,
+ -cppcoreguidelines-narrowing-conversions
'
HeaderFilterRegex: '^.*(source|include).*$'
CheckOptions:
--
GitLab
From b364d60ae0ebac183b8f715b7894efbb0912598d Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 Jan 2022 21:41:58 +0100
Subject: [PATCH 07/11] replacing magic numbers
---
.../algorithms/spfa/ShortestPathFasterAlgorithm.cpp | 6 +++---
.../algorithms/spfa/ShortestPathFasterAlgorithm.h | 7 +++++--
2 files changed, 8 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
index 692eec77..c7ad4ea4 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
@@ -185,12 +185,12 @@ namespace armarx::navigation::algorithms::spfa
continue;
- const float obstacleDistance1 = std::min(inputMap(row, col), 1000.F);
- const float obstacleDistance2 = std::min(inputMap(ip, jp), 1000.F);
+ // const float obstacleDistance1 = std::min(inputMap(row, col), 1000.F);
+ const float obstacleDistance2 = std::min(inputMap(ip, jp), params.obstacleMaxDistance);
const float travelCost = dir_lengths[k];
// const float obstacleDistanceCost = std::max(obstacleDistance1 - obstacleDistance2, 0.F) / costmap.params().cellSize;
- const float targetDistanceCost = 3 * (1000.F - obstacleDistance2) / 1000.F;
+ const float targetDistanceCost = params.obstacleDistanceWeight * (params.obstacleMaxDistance - obstacleDistance2) / params.obstacleMaxDistance;
const float edgeCost =
params.obstacleDistanceCosts ? travelCost + targetDistanceCost : travelCost;
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
index 65a458c3..810783e4 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h
@@ -36,7 +36,10 @@ namespace armarx::navigation::algorithms::spfa
public:
struct Parameters
{
- bool obstacleDistanceCosts{true};
+ bool obstacleDistanceCosts = true;
+ float obstacleMaxDistance = 1000.F;
+
+ float obstacleDistanceWeight = 3.F;
};
ShortestPathFasterAlgorithm(const Costmap& costmap, const Parameters& params);
@@ -54,7 +57,7 @@ namespace armarx::navigation::algorithms::spfa
Result spfa(const Eigen::Vector2f& start);
- // protected:
+ // protected:
/**
* @brief Implementation highly inspired by github.com:jimmyyhwu/spatial-action-maps
*
--
GitLab
From 1e9a6a4cc75f75f81e2e9dd69fbef5c080c69c3c Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 Jan 2022 21:42:24 +0100
Subject: [PATCH 08/11] cmake: using range-v3::range-v3 target instead of
range-v3 only
---
CMakeLists.txt | 30 ++++++++++++++-----
.../GraphImportExport/CMakeLists.txt | 7 -----
source/armarx/navigation/core/CMakeLists.txt | 4 +--
.../navigation/global_planning/CMakeLists.txt | 2 +-
.../navigation/local_planning/CMakeLists.txt | 9 ------
.../armarx/navigation/server/CMakeLists.txt | 2 +-
6 files changed, 26 insertions(+), 28 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9420d8af..25e2b850 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.18)
# default settings
set(ARMARX_ENABLE_DEPENDENCY_VERSION_CHECK_DEFAULT FALSE)
@@ -11,17 +11,19 @@ armarx_enable_modern_cmake_project()
armarx_project(navigation NAMESPACE armarx)
# Specify each ArmarX Package dependency with the following macro
-armarx_find_package(PUBLIC ArmarXGui)
+
+# - required
armarx_find_package(PUBLIC RobotAPI REQUIRED)
+
+# - optional
+armarx_find_package(PUBLIC ArmarXGui)
armarx_find_package(PUBLIC MemoryX QUIET)
armarx_find_package(PUBLIC VisionX QUIET)
-add_subdirectory(etc)
-add_subdirectory(external)
-
-# Required dependencies
+# System dependencies
+# - required
-# Optional dependencies
+# - optional
armarx_find_package(PUBLIC OpenMP QUIET)
armarx_find_package(PUBLIC Ceres QUIET)
armarx_find_package(PUBLIC VTK QUIET)
@@ -29,6 +31,14 @@ armarx_find_package(PUBLIC SemanticObjectRelations QUIET)
armarx_find_package(PUBLIC OpenCV QUIET) # Required as RobotAPI is a legacy project.
+add_subdirectory(etc)
+
+set(RANGES_VERBOSE_BUILD OFF)
+set(RANGES_RELEASE_BUILD ON)
+set(RANGES_CXX_STD 17)
+add_subdirectory(external)
+
+
#include(FetchContent)
#FetchContent_Declare(
@@ -46,7 +56,11 @@ armarx_find_package(PUBLIC OpenCV QUIET) # Required as RobotAPI is a legacy pro
# )
# FetchContent_MakeAvailable(inotify_cpp)
-add_definitions("-Werror=init-self -Werror=uninitialized")
+add_definitions(-Werror=init-self)
+add_definitions(-Werror=uninitialized)
+add_definitions(-Werror=missing-field-initializers)
+add_definitions(-Werror=reorder)
+add_definitions(-Werror=narrowing)
add_subdirectory(source)
diff --git a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
index ec6c529d..123769e2 100644
--- a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
+++ b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
@@ -1,10 +1,3 @@
-#find_package(MemoryX QUIET)
-#armarx_build_if(MemoryX_FOUND "MemoryX not available")
-#if(MemoryX_FOUND)
-# target_include_directories(${LIB_NAME} PUBLIC ${MemoryX_INCLUDE_DIRS})
-#endif()
-
-
armarx_add_component(GraphImportExport
DEPENDENCIES
# ArmarXCore
diff --git a/source/armarx/navigation/core/CMakeLists.txt b/source/armarx/navigation/core/CMakeLists.txt
index ec1598c6..ca477220 100644
--- a/source/armarx/navigation/core/CMakeLists.txt
+++ b/source/armarx/navigation/core/CMakeLists.txt
@@ -45,7 +45,7 @@ armarx_add_library(core
Simox::VirtualRobot
armarx_navigation::core_aron
PRIVATE
- range-v3
+ range-v3::range-v3
)
@@ -57,5 +57,5 @@ armarx_add_test(core_test
ArmarXCore
armarx_navigation::core
PRIVATE
- range-v3
+ range-v3::range-v3
)
diff --git a/source/armarx/navigation/global_planning/CMakeLists.txt b/source/armarx/navigation/global_planning/CMakeLists.txt
index b3b248d6..ca9fcf74 100644
--- a/source/armarx/navigation/global_planning/CMakeLists.txt
+++ b/source/armarx/navigation/global_planning/CMakeLists.txt
@@ -17,7 +17,7 @@ armarx_add_library(global_planning
armarx_navigation::algorithms
armarx_navigation::global_planning_aron
PRIVATE
- range-v3
+ range-v3::range-v3
SOURCES
./GlobalPlanner.cpp
./AStar.cpp
diff --git a/source/armarx/navigation/local_planning/CMakeLists.txt b/source/armarx/navigation/local_planning/CMakeLists.txt
index 5894b6af..78a91fc0 100644
--- a/source/armarx/navigation/local_planning/CMakeLists.txt
+++ b/source/armarx/navigation/local_planning/CMakeLists.txt
@@ -9,12 +9,3 @@ armarx_add_library(local_planning
HEADERS ./LocalPlanner.h
./TimedElasticBands.h
)
-
-
-# find_package(MyLib QUIET) armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND) for multiple
-# libraries write: if(X_FOUND AND Y_FOUND).... if(MyLib_FOUND)
-# target_include_directories(local_planning PUBLIC ${MyLib_INCLUDE_DIRS})
-# endif()
-
-# add unit tests add_subdirectory(test)
diff --git a/source/armarx/navigation/server/CMakeLists.txt b/source/armarx/navigation/server/CMakeLists.txt
index 51cd9bae..b9f7a261 100644
--- a/source/armarx/navigation/server/CMakeLists.txt
+++ b/source/armarx/navigation/server/CMakeLists.txt
@@ -47,7 +47,7 @@ armarx_add_library(server
armarx_navigation::safety_control
armarx_navigation::memory
PRIVATE
- range-v3
+ range-v3::range-v3
)
armarx_add_test(server_test
--
GitLab
From 0a5f98fcea70fdc29a630defc30c5b07d4662972 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 12 Jan 2022 09:19:02 +0100
Subject: [PATCH 09/11] implementing merging of costmaps / grids
---
.../armarx/navigation/algorithms/Costmap.cpp | 33 +++++
source/armarx/navigation/algorithms/Costmap.h | 32 ++++-
.../navigation/algorithms/CostmapBuilder.cpp | 5 +-
.../navigation/algorithms/CostmapBuilder.h | 6 +-
source/armarx/navigation/algorithms/util.cpp | 113 ++++++++++++++++--
source/armarx/navigation/algorithms/util.h | 15 ++-
6 files changed, 185 insertions(+), 19 deletions(-)
diff --git a/source/armarx/navigation/algorithms/Costmap.cpp b/source/armarx/navigation/algorithms/Costmap.cpp
index 67fde2b8..3cb10124 100644
--- a/source/armarx/navigation/algorithms/Costmap.cpp
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -62,5 +62,38 @@ namespace armarx::navigation::algorithms
return grid(v.index.x(), v.index.y()) == 0.F;
}
+ bool
+ Costmap::add(const Costmap& other)
+ {
+ const auto startIdx = toVertex(other.sceneBounds.min);
+
+ // merge other grid into this one => use max costs
+
+ // TODO if one of both matrices has mask, it has to be considered.
+ ARMARX_TRACE;
+ grid.block(startIdx.index.x(), startIdx.index.y(), other.grid.rows(), other.grid.cols()) =
+ grid.block(startIdx.index.x(), startIdx.index.y(), other.grid.rows(), other.grid.cols())
+ .cwiseMax(other.grid);
+
+ if(other.mask.has_value())
+ {
+ if(not mask.has_value())
+ {
+ mask = Mask(grid.rows(), grid.cols());
+ mask->setOnes();
+ }
+
+ // union of masks
+ ARMARX_TRACE;
+ mask = mask->cwiseMin(other.mask->value());
+
+ // apply mask to grid => all invalid points will be 0
+ ARMARX_TRACE;
+ grid = grid.cwiseProduct(mask->cast<float>());
+ }
+
+ return true;
+ }
+
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 730a7175..4e94697d 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -1,6 +1,8 @@
#pragma once
#include <Eigen/Core>
+#include <Eigen/src/Core/util/Constants.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include <armarx/navigation/algorithms/types.h>
@@ -28,10 +30,20 @@ namespace armarx::navigation::algorithms
using Position = Eigen::Vector2f;
using Grid = Eigen::MatrixXf;
+ // if 0, the cell is invalid
+ using Mask = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>;
- Costmap(const Grid& grid, const Parameters& parameters, const SceneBounds& sceneBounds) :
- grid(grid), sceneBounds(sceneBounds), parameters(parameters)
+ Costmap(const Grid& grid,
+ const Parameters& parameters,
+ const SceneBounds& sceneBounds,
+ const std::optional<Mask>& mask = std::nullopt) :
+ grid(grid), mask(mask), sceneBounds(sceneBounds), parameters(parameters)
{
+ if(mask.has_value())
+ {
+ ARMARX_CHECK_EQUAL(grid.rows(), mask->rows());
+ ARMARX_CHECK_EQUAL(grid.cols(), mask->cols());
+ }
}
struct Vertex
@@ -57,14 +69,28 @@ namespace armarx::navigation::algorithms
return parameters;
}
- const SceneBounds&
+ const SceneBounds&
getSceneBounds() const noexcept
{
return sceneBounds;
}
+ Index
+ minIndex() const noexcept
+ {
+ Grid::Index row = 0;
+ Grid::Index col = 0;
+
+ getGrid().minCoeff(&row, &col);
+
+ return Index{row, col};
+ }
+
+ bool add(const Costmap& other);
+
private:
Grid grid;
+ std::optional<Mask> mask;
const SceneBounds sceneBounds;
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.cpp b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
index 9c40a93d..4fe02e87 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.cpp
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
@@ -87,7 +87,10 @@ namespace armarx::navigation::algorithms
ARMARX_INFO << "Grid size: " << c_x << ", " << c_y;
ARMARX_INFO << "Resetting grid";
- return Eigen::MatrixXf(c_x, c_y);
+ Eigen::MatrixXf grid(c_x, c_y);
+ grid.setZero();
+
+ return grid;
}
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.h b/source/armarx/navigation/algorithms/CostmapBuilder.h
index ff6ef9d2..3e1e4237 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.h
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.h
@@ -38,10 +38,10 @@ namespace armarx::navigation::algorithms
[[nodiscard]] Costmap create();
- private:
- Eigen::MatrixXf createUniformGrid(const SceneBounds& sceneBounds,
- const Costmap::Parameters& parameters);
+ static Eigen::MatrixXf createUniformGrid(const SceneBounds& sceneBounds,
+ const Costmap::Parameters& parameters);
+ private:
float computeCost(const Costmap::Position& position,
VirtualRobot::RobotPtr& collisionRobot,
const VirtualRobot::CollisionModelPtr& robotCollisionModel);
diff --git a/source/armarx/navigation/algorithms/util.cpp b/source/armarx/navigation/algorithms/util.cpp
index 843d40eb..a991a3d3 100644
--- a/source/armarx/navigation/algorithms/util.cpp
+++ b/source/armarx/navigation/algorithms/util.cpp
@@ -21,6 +21,13 @@
#include "util.h"
+#include <algorithm>
+#include <iterator>
+
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include <SimoxUtility/algorithm/apply.hpp>
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/SceneObjectSet.h>
@@ -28,6 +35,10 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/algorithms/CostmapBuilder.h"
+#include "armarx/navigation/algorithms/types.h"
+
namespace armarx::navigation::algorithms
{
@@ -53,26 +64,112 @@ namespace armarx::navigation::algorithms
}
- armarx::navigation::algorithms::SceneBounds
+ SceneBounds
toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends)
{
return {.min = Eigen::Vector2f{extends.minX, extends.minY},
.max = Eigen::Vector2f{extends.maxX, extends.maxY}};
}
- armarx::navigation::algorithms::Costmap
+ SceneBounds
+ merge(const std::vector<SceneBounds>& sceneBounds)
+ {
+ SceneBounds bounds;
+
+ const auto expandBounds = [&bounds](const SceneBounds& sb)
+ {
+ bounds.min.x() = std::min(bounds.min.x(), sb.min.x());
+ bounds.min.y() = std::min(bounds.min.y(), sb.min.y());
+
+ bounds.max.x() = std::max(bounds.max.x(), sb.max.x());
+ bounds.max.y() = std::max(bounds.max.y(), sb.max.y());
+
+ return bounds;
+ };
+
+ std::for_each(sceneBounds.begin(), sceneBounds.end(), expandBounds);
+
+ return bounds;
+ }
+
+
+ Costmap
toCostmap(const VirtualRobot::WorkspaceGrid& workspaceGrid)
{
- const armarx::navigation::algorithms::Costmap::Grid grid(workspaceGrid.getCells().x,
- workspaceGrid.getCells().y);
- const armarx::navigation::algorithms::SceneBounds sceneBounds =
- toSceneBounds(workspaceGrid.getExtends());
+ const Costmap::Grid grid(workspaceGrid.getCells().x, workspaceGrid.getCells().y);
+ const SceneBounds sceneBounds = toSceneBounds(workspaceGrid.getExtends());
- const armarx::navigation::algorithms::Costmap::Parameters parameters{
- .binaryGrid = false, .cellSize = workspaceGrid.getDiscretizeSize()};
+ const Costmap::Parameters parameters{.binaryGrid = false,
+ .cellSize = workspaceGrid.getDiscretizeSize()};
return {grid, parameters, sceneBounds};
}
+ Costmap
+ mergeUnaligned(const std::vector<Costmap>& costmaps,
+ const std::vector<float>& weights,
+ float cellSize)
+ {
+ ARMARX_CHECK_EQUAL(costmaps.size(), weights.size());
+
+ // if unset, use the smallest cell size
+ if (cellSize < 0)
+ {
+ const auto compCellSize = [](const Costmap& a, const Costmap& b) -> bool
+ { return a.params().cellSize < b.params().cellSize; };
+
+ cellSize =
+ std::min_element(costmaps.begin(), costmaps.end(), compCellSize)->params().cellSize;
+ }
+
+ // scale each costmap
+ std::vector<Costmap> scaledCostmaps = simox::alg::apply(
+ costmaps,
+ [&cellSize](const Costmap& costmap) -> Costmap { return scaleCostmap(costmap, cellSize); });
+
+ // merge costmaps into one
+
+ // - combine scene bounds
+ std::vector<SceneBounds> sceneBoundsAll = simox::alg::apply(
+ scaledCostmaps, [](const Costmap& costmap) { return costmap.getSceneBounds(); });
+
+ const SceneBounds sceneBounds = merge(sceneBoundsAll);
+
+ // - create grid
+ const auto largeGrid = CostmapBuilder::createUniformGrid(
+ sceneBounds, Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize});
+
+ Costmap largeCostmap(
+ largeGrid, Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize}, sceneBounds);
+
+ // - add all costmaps to this one
+ std::for_each(scaledCostmaps.begin(),
+ scaledCostmaps.end(),
+ [&largeCostmap](const Costmap& costmap) { largeCostmap.add(costmap); });
+
+ // return merged grid
+ return largeCostmap;
+ }
+
+ Costmap
+ scaleCostmap(const Costmap& costmap, float cellSize)
+ {
+ const float scale = cellSize / costmap.params().cellSize;
+
+ cv::Mat src;
+ cv::eigen2cv(costmap.getGrid(), src);
+
+ cv::Mat dst;
+ cv::resize(src, dst, cv::Size{0, 0}, scale, scale);
+
+ Eigen::MatrixXf scaledGrid;
+ cv::cv2eigen(dst, scaledGrid);
+
+ return {scaledGrid,
+ Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize},
+ costmap.getSceneBounds()};
+ }
+
+
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/util.h b/source/armarx/navigation/algorithms/util.h
index cee91e0c..1a4d1d0e 100644
--- a/source/armarx/navigation/algorithms/util.h
+++ b/source/armarx/navigation/algorithms/util.h
@@ -33,10 +33,17 @@ namespace armarx::navigation::algorithms
{
SceneBounds computeSceneBounds(const VirtualRobot::SceneObjectSetPtr& obstacles);
- armarx::navigation::algorithms::SceneBounds
- toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends);
+ SceneBounds toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends);
- armarx::navigation::algorithms::Costmap
- toCostmap(const VirtualRobot::WorkspaceGrid& workspaceGrid);
+ SceneBounds merge(const std::vector<SceneBounds>& sceneBounds);
+
+
+ Costmap toCostmap(const VirtualRobot::WorkspaceGrid& workspaceGrid);
+
+ Costmap mergeUnaligned(const std::vector<Costmap>& costmaps,
+ const std::vector<float>& weights,
+ float cellSize = -1);
+
+ Costmap scaleCostmap(const Costmap& costmap, float cellSize);
} // namespace armarx::navigation::algorithms
--
GitLab
From bc9d6b28a00d0b788c91beaada2cada4f89a6d2f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 12 Jan 2022 16:23:51 +0100
Subject: [PATCH 10/11] navigation simulation scenario update
---
.../HandUnitDynamicSimulationApp.LeftHand.cfg | 23 +++++++++++++++++++
...HandUnitDynamicSimulationApp.RightHand.cfg | 23 +++++++++++++++++++
.../config/LongtermMemory.cfg | 16 -------------
.../config/PriorKnowledge.cfg | 8 -------
.../config/WorkingMemory.cfg | 8 -------
5 files changed, 46 insertions(+), 32 deletions(-)
diff --git a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
index 18e2d2db..ddfb9cce 100644
--- a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
+++ b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
@@ -76,6 +76,14 @@ ArmarX.ApplicationName = LeftHandUnitApp
# ArmarX.EnableProfiling = false
+# ArmarX.HandUnitDynamicSimulation.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HandUnitDynamicSimulation.ArVizTopicName = ArVizTopic
+
+
# ArmarX.HandUnitDynamicSimulation.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
@@ -131,6 +139,13 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = LeftHandUnit
# ArmarX.HandUnitDynamicSimulation.RobotStateComponentName = RobotStateComponent
+# ArmarX.HandUnitDynamicSimulation.Side: Name of the hand (left, right)
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.HandUnitDynamicSimulation.Side = ::_NOT_SET_::
+
+
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName: Name of the simulator proxy to use.
# Attributes:
# - Default: Simulator
@@ -139,6 +154,14 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = LeftHandUnit
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName = Simulator
+# ArmarX.HandUnitDynamicSimulation.WorkingMemoryName: Name of the working memory that should be used
+# Attributes:
+# - Default: WorkingMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HandUnitDynamicSimulation.WorkingMemoryName = WorkingMemory
+
+
# ArmarX.HandUnitDynamicSimulation.inheritFrom: No Description
# Attributes:
# - Default: RobotConfig
diff --git a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
index 85cc4345..cf952e6f 100644
--- a/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
+++ b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.RightHand.cfg
@@ -76,6 +76,14 @@ ArmarX.ApplicationName = RightHandUnitApp
# ArmarX.EnableProfiling = false
+# ArmarX.HandUnitDynamicSimulation.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HandUnitDynamicSimulation.ArVizTopicName = ArVizTopic
+
+
# ArmarX.HandUnitDynamicSimulation.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
@@ -131,6 +139,13 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = RightHandUnit
# ArmarX.HandUnitDynamicSimulation.RobotStateComponentName = RobotStateComponent
+# ArmarX.HandUnitDynamicSimulation.Side: Name of the hand (left, right)
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.HandUnitDynamicSimulation.Side = ::_NOT_SET_::
+
+
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName: Name of the simulator proxy to use.
# Attributes:
# - Default: Simulator
@@ -139,6 +154,14 @@ ArmarX.HandUnitDynamicSimulation.ObjectName = RightHandUnit
# ArmarX.HandUnitDynamicSimulation.SimulatorProxyName = Simulator
+# ArmarX.HandUnitDynamicSimulation.WorkingMemoryName: Name of the working memory that should be used
+# Attributes:
+# - Default: WorkingMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HandUnitDynamicSimulation.WorkingMemoryName = WorkingMemory
+
+
# ArmarX.HandUnitDynamicSimulation.inheritFrom: No Description
# Attributes:
# - Default: RobotConfig
diff --git a/scenarios/NavigationSimulation/config/LongtermMemory.cfg b/scenarios/NavigationSimulation/config/LongtermMemory.cfg
index 0eae3f1a..e323230d 100644
--- a/scenarios/NavigationSimulation/config/LongtermMemory.cfg
+++ b/scenarios/NavigationSimulation/config/LongtermMemory.cfg
@@ -100,14 +100,6 @@
MemoryX.LongtermMemory.ClassCollections = memdb.Longterm_Objects
-# MemoryX.LongtermMemory.CommonStorageName: Name of common storage.
-# Attributes:
-# - Default: CommonStorage
-# - Case sensitivity: yes
-# - Required: no
-# MemoryX.LongtermMemory.CommonStorageName = CommonStorage
-
-
# MemoryX.LongtermMemory.DatabaseName: Mongo database to store LTM data
# Attributes:
# - Case sensitivity: yes
@@ -173,14 +165,6 @@ MemoryX.LongtermMemory.DatabaseName = CeBITdb
# MemoryX.LongtermMemory.PredictionDataCollection = ltm_predictiondata
-# MemoryX.LongtermMemory.PriorKnowledgeName: Name of prior knowledge.
-# Attributes:
-# - Default: PriorKnowledge
-# - Case sensitivity: yes
-# - Required: no
-# MemoryX.LongtermMemory.PriorKnowledgeName = PriorKnowledge
-
-
# MemoryX.LongtermMemory.ProfilerDataCollection: Mongo collection for storing Profiler data
# Attributes:
# - Default: ltm_profilerdata
diff --git a/scenarios/NavigationSimulation/config/PriorKnowledge.cfg b/scenarios/NavigationSimulation/config/PriorKnowledge.cfg
index d2a8fe85..c44a2b83 100644
--- a/scenarios/NavigationSimulation/config/PriorKnowledge.cfg
+++ b/scenarios/NavigationSimulation/config/PriorKnowledge.cfg
@@ -99,14 +99,6 @@
MemoryX.PriorKnowledge.ClassCollections = CeBITdb.Prior_CeBIT
-# MemoryX.PriorKnowledge.CommonStorageName: Name of common storage.
-# Attributes:
-# - Default: CommonStorage
-# - Case sensitivity: yes
-# - Required: no
-# MemoryX.PriorKnowledge.CommonStorageName = CommonStorage
-
-
# MemoryX.PriorKnowledge.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
diff --git a/scenarios/NavigationSimulation/config/WorkingMemory.cfg b/scenarios/NavigationSimulation/config/WorkingMemory.cfg
index a6115c9f..f33e4b8b 100644
--- a/scenarios/NavigationSimulation/config/WorkingMemory.cfg
+++ b/scenarios/NavigationSimulation/config/WorkingMemory.cfg
@@ -272,14 +272,6 @@ MemoryX.ObjectLocalizationMemoryUpdater.WorkingMemoryName = "WorkingMemory"
# MemoryX.Verbosity = Info
-# MemoryX.WorkingMemory.CommonStorageName: Name of common storage.
-# Attributes:
-# - Default: CommonStorage
-# - Case sensitivity: yes
-# - Required: no
-# MemoryX.WorkingMemory.CommonStorageName = CommonStorage
-
-
# MemoryX.WorkingMemory.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
--
GitLab
From 927ae4f29a3a4d3fb99a39611375b5b64429a8e4 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 12 Jan 2022 16:24:26 +0100
Subject: [PATCH 11/11] using mask; fix: robot only with collision model (visu
model sucks -> segfaults ...)
---
.../armarx/navigation/algorithms/Costmap.cpp | 90 +++++++++++++++++--
source/armarx/navigation/algorithms/Costmap.h | 41 ++-------
.../spfa/ShortestPathFasterAlgorithm.cpp | 16 +++-
.../components/Navigator/Navigator.cpp | 2 +-
4 files changed, 101 insertions(+), 48 deletions(-)
diff --git a/source/armarx/navigation/algorithms/Costmap.cpp b/source/armarx/navigation/algorithms/Costmap.cpp
index 3cb10124..bf9cad97 100644
--- a/source/armarx/navigation/algorithms/Costmap.cpp
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -7,6 +7,7 @@
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/detail/intersects/interface.hpp>
+#include <Eigen/Core>
#include <Eigen/Geometry>
#include <IceUtil/Time.h>
@@ -28,6 +29,19 @@
namespace armarx::navigation::algorithms
{
+ Costmap::Costmap(const Grid& grid,
+ const Parameters& parameters,
+ const SceneBounds& sceneBounds,
+ const std::optional<Mask>& mask) :
+ grid(grid), mask(mask), sceneBounds(sceneBounds), parameters(parameters)
+ {
+ if (mask.has_value())
+ {
+ ARMARX_CHECK_EQUAL(grid.rows(), mask->rows());
+ ARMARX_CHECK_EQUAL(grid.cols(), mask->cols());
+ }
+ }
+
Costmap::Position
Costmap::toPosition(const Costmap::Index& index) const
{
@@ -46,13 +60,75 @@ namespace armarx::navigation::algorithms
const float vY =
(v.y() - parameters.cellSize / 2 - sceneBounds.min.y()) / parameters.cellSize;
- ARMARX_CHECK(vX >= 0.F);
- ARMARX_CHECK(vY >= 0.F);
+ const int iX = std::round(vX);
+ const int iY = std::round(vY);
+
+ const int iXSan = std::clamp<int>(iX, 0, grid.rows() - 1);
+ const int iYSan = std::clamp<int>(iY, 0, grid.cols() - 1);
+
+ // FIXME accept one cell off
+
+ // ARMARX_CHECK_GREATER(iX, 0);
+ // ARMARX_CHECK_GREATER(iY, 0);
+
+ // ARMARX_CHECK_LESS_EQUAL(iX, grid.rows() - 1);
+ // ARMARX_CHECK_LESS_EQUAL(iY, grid.cols() - 1);
+
+ return Vertex{.index = Index{iXSan, iYSan}, .position = v};
+ }
+
+ const SceneBounds&
+ Costmap::getSceneBounds() const noexcept
+ {
+ return sceneBounds;
+ }
+
+ Costmap::Index
+ Costmap::minIndex() const noexcept
+ {
+ // index of the min element
+ Grid::Index row = 0;
+ Grid::Index col = 0;
+
+ // value of the min element
+ float minVal = std::numeric_limits<float>::max();
+
+ for (int r = 0; r < grid.rows(); r++)
+ {
+ for (int c = 0; c < grid.cols(); c++)
+ {
+ if (mask.has_value())
+ {
+ if (not mask.value()(r, c)) // skip invalid cells
+ {
+ continue;
+ }
+
+ const float currentVal = grid(r, c);
+ if (currentVal >= minVal)
+ {
+ minVal = currentVal;
+ row = r;
+ col = c;
+ }
+ }
+ }
+ }
+
- ARMARX_CHECK(vX <= (grid.rows() - 1));
- ARMARX_CHECK(vY <= (grid.cols() - 1));
+ return Index{row, col};
+ }
- return Vertex{.index = Index{static_cast<int>(vX), static_cast<int>(vY)}, .position = v};
+ const Costmap::Parameters&
+ Costmap::params() const noexcept
+ {
+ return parameters;
+ }
+
+ const Costmap::Grid&
+ Costmap::getGrid() const
+ {
+ return grid;
}
bool
@@ -75,9 +151,9 @@ namespace armarx::navigation::algorithms
grid.block(startIdx.index.x(), startIdx.index.y(), other.grid.rows(), other.grid.cols())
.cwiseMax(other.grid);
- if(other.mask.has_value())
+ if (other.mask.has_value())
{
- if(not mask.has_value())
+ if (not mask.has_value())
{
mask = Mask(grid.rows(), grid.cols());
mask->setOnes();
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 4e94697d..d35e7781 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -1,8 +1,6 @@
#pragma once
#include <Eigen/Core>
-#include <Eigen/src/Core/util/Constants.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include <armarx/navigation/algorithms/types.h>
@@ -36,15 +34,7 @@ namespace armarx::navigation::algorithms
Costmap(const Grid& grid,
const Parameters& parameters,
const SceneBounds& sceneBounds,
- const std::optional<Mask>& mask = std::nullopt) :
- grid(grid), mask(mask), sceneBounds(sceneBounds), parameters(parameters)
- {
- if(mask.has_value())
- {
- ARMARX_CHECK_EQUAL(grid.rows(), mask->rows());
- ARMARX_CHECK_EQUAL(grid.cols(), mask->cols());
- }
- }
+ const std::optional<Mask>& mask = std::nullopt);
struct Vertex
{
@@ -55,36 +45,15 @@ namespace armarx::navigation::algorithms
Position toPosition(const Index& index) const;
Vertex toVertex(const Position& v) const;
- const Grid&
- getGrid() const
- {
- return grid;
- }
+ const Grid& getGrid() const;
bool isInCollision(const Position& p) const;
- const Parameters&
- params() const noexcept
- {
- return parameters;
- }
-
- const SceneBounds&
- getSceneBounds() const noexcept
- {
- return sceneBounds;
- }
-
- Index
- minIndex() const noexcept
- {
- Grid::Index row = 0;
- Grid::Index col = 0;
+ const Parameters& params() const noexcept;
- getGrid().minCoeff(&row, &col);
+ const SceneBounds& getSceneBounds() const noexcept;
- return Index{row, col};
- }
+ Index minIndex() const noexcept;
bool add(const Costmap& other);
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
index c7ad4ea4..248db17d 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
@@ -22,6 +22,7 @@
#include "ShortestPathFasterAlgorithm.h"
#include <algorithm>
+#include <numeric>
#include <Eigen/Core>
@@ -128,6 +129,8 @@ namespace armarx::navigation::algorithms::spfa
ShortestPathFasterAlgorithm::spfa(const Eigen::MatrixXf& inputMap,
const Eigen::Vector2i& source) const
{
+ ARMARX_CHECK_GREATER(inputMap(source.x(), source.y()), 0.F) << "Start must not be in collision";
+
const float eps = 1e-6;
const int num_dirs = 8;
@@ -160,9 +163,8 @@ namespace armarx::navigation::algorithms::spfa
float* dists = new float[max_num_verts];
for (int i = 0; i < max_num_verts; ++i)
dists[i] = inf;
- int* parents = new int[max_num_verts]();
- for (int i = 0; i < max_num_verts; ++i)
- parents[i] = -1;
+
+ std::vector<int> parents(max_num_verts, -1);
// Build graph
ARMARX_INFO << "Build graph";
@@ -249,6 +251,8 @@ namespace armarx::navigation::algorithms::spfa
Eigen::Vector2i{-1, -1}));
}
+ int invalids = 0;
+
for (int row = 0; row < num_rows; ++row)
{
for (int col = 0; col < num_cols; ++col)
@@ -258,6 +262,7 @@ namespace armarx::navigation::algorithms::spfa
if (parents[u] == -1) // no parent
{
+ invalids++;
continue;
}
@@ -265,6 +270,10 @@ namespace armarx::navigation::algorithms::spfa
}
}
+
+ ARMARX_INFO << "Fraction of invalid cells: (" << invalids << "/" << parents.size() << ")";
+
+
// Free memory
delete[] edges;
delete[] edge_counts;
@@ -272,7 +281,6 @@ namespace armarx::navigation::algorithms::spfa
delete[] in_queue;
delete[] weights;
delete[] dists;
- delete[] parents;
ARMARX_INFO << "Done.";
return Result{.distances = output_dists, .parents = output_parents};
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 13718710..2ec54dc2 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -619,7 +619,7 @@ namespace armarx::navigation::components
ARMARX_TRACE;
auto robot = RemoteRobot::createLocalCloneFromFile(
- getRobotStateComponent(), VirtualRobot::RobotIO::RobotDescription::eFull);
+ getRobotStateComponent(), VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
// auto robot = RemoteRobot::createLocalClone(getRobotStateComponent());
ARMARX_CHECK_NOT_NULL(robot);
--
GitLab