diff --git a/.clang-tidy b/.clang-tidy
index 6e80b9026b3eb65a5417ca390eac4ccc1903e85e..2a651563d9077f067b1c59b59e2f46bc8e882ead 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:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1fdc9b992adab59ca3b1ddd7d8a4e3d9d2194381..25e2b850ce1593711b470bcb2e9262e97ac2e155 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,6 +56,11 @@ armarx_find_package(PUBLIC OpenCV QUIET) # Required as RobotAPI is a legacy pro
# )
# FetchContent_MakeAvailable(inotify_cpp)
+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/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg b/scenarios/NavigationSimulation/config/HandUnitDynamicSimulationApp.LeftHand.cfg
index 18e2d2dbc38674afde8395d91f1a6582b0a9116f..ddfb9cce396b5caf6cdd1b97581da4ea1d659130 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 85cc434517acbe8d8f2fe87dcf357b4d4f86823d..cf952e6fb103f0ce16f4a4fdcf2f86c0b5208e74 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 0eae3f1ab86c16681c70d88228002d456dcffa33..e323230d5da82a370611eec3fad6358f913ce064 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 d2a8fe85f077987cf9406710c2f49917fac04917..c44a2b83dc96d79b137906e8dd978541b343c8c2 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 a6115c9f6b1e95a8d4a73aae1097813fdc128e58..f33e4b8bc9dee1a831a65ab858d3c47cb4d09019 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
diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index d9ff35889b35886ee08ce6211cb04eb3f2404829..4b408d87b1b9381e23da99ec7846eeaa66ce41b3 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 0000000000000000000000000000000000000000..bf9cad9738bc917d868012fa2ad561ccf6dd8d7c
--- /dev/null
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -0,0 +1,175 @@
+#include "Costmap.h"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/detail/intersects/interface.hpp>
+
+#include <Eigen/Core>
+#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::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
+ {
+ 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;
+
+ 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;
+ }
+ }
+ }
+ }
+
+
+ return Index{row, col};
+ }
+
+ const Costmap::Parameters&
+ Costmap::params() const noexcept
+ {
+ return parameters;
+ }
+
+ const Costmap::Grid&
+ Costmap::getGrid() const
+ {
+ return grid;
+ }
+
+ bool
+ Costmap::isInCollision(const Position& p) const
+ {
+ const auto v = toVertex(p);
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..d35e77810c95bf7fbb0853b1def4ee44a6a7017a
--- /dev/null
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -0,0 +1,70 @@
+#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;
+
+ // 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,
+ const std::optional<Mask>& mask = std::nullopt);
+
+ 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;
+
+ bool isInCollision(const Position& p) const;
+
+ const Parameters& params() const noexcept;
+
+ const SceneBounds& getSceneBounds() const noexcept;
+
+ Index minIndex() const noexcept;
+
+ bool add(const Costmap& other);
+
+ private:
+ Grid grid;
+ std::optional<Mask> mask;
+
+ 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 0000000000000000000000000000000000000000..4fe02e875e5690dc4db9df66709f9e3dde3e3871
--- /dev/null
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
@@ -0,0 +1,219 @@
+/**
+ * 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 with size (" << costmap.getGrid().rows() << "/"
+ << costmap.getGrid().cols() << ")";
+ 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_x << ", " << c_y;
+
+ ARMARX_INFO << "Resetting grid";
+ Eigen::MatrixXf grid(c_x, c_y);
+ grid.setZero();
+
+ return grid;
+ }
+
+
+ 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(robotCollisionModel) << "Collision model not available. "
+ "Make sure that you load the robot correctly!";
+ }
+
+ 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 0000000000000000000000000000000000000000..3e1e4237cd1cf7bf92e99f08577ff3cfaeb75d17
--- /dev/null
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.h
@@ -0,0 +1,57 @@
+/**
+ * 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);
+
+ [[nodiscard]] Costmap create();
+
+ 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);
+
+ 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 9d994ecbb31171fe73130611ea04cd934f9b1003..0000000000000000000000000000000000000000
--- 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 75328ef436d6593ae05dbdef533ea34d03b682f7..0000000000000000000000000000000000000000
--- 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 3bb7aad49855ded307d14a346d1d714dc275f62b..10d338ec0945a10986903c2f4e265458ee33a13a 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, robot);
+ 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 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);
@@ -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 5737f62860491d37021af9b463f053eaa127aa96..872560850b4393e0275d6b86cf7c1b3c7688745a 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,15 @@ 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);
@@ -34,16 +32,14 @@ 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:
- /// 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/algorithms/astar/Planner2D.h b/source/armarx/navigation/algorithms/astar/Planner2D.h
index 5fb3b7e6c9e67ed38c8434f0414eec34b03f28ea..cd7b0540989ea9dd91c4aa9475d77000028c344e 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 8fd472833abe06e4adbfa992c69ab12d01aee2f7..248db17da23a3eee36ec94e9bc03a52f73d6672e 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
@@ -22,12 +22,15 @@
#include "ShortestPathFasterAlgorithm.h"
#include <algorithm>
+#include <numeric>
#include <Eigen/Core>
#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 +53,26 @@ namespace armarx::navigation::algorithms::spfa
return p;
}
- ShortestPathFasterAlgorithm::ShortestPathFasterAlgorithm(
- const algorithm::NavigationCostMap& 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 algorithm::NavigationCostMap::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 +104,7 @@ namespace armarx::navigation::algorithms::spfa
break;
}
- path.push_back(grid.toPosition(parent.array()));
+ path.push_back(costmap.toPosition(parent.array()));
pt = &parent;
}
@@ -115,11 +118,19 @@ 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,
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;
@@ -152,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";
@@ -177,15 +187,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), params.obstacleMaxDistance);
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 = params.obstacleDistanceWeight * (params.obstacleMaxDistance - obstacleDistance2) / params.obstacleMaxDistance;
- const float edgeCost = params.obstacleDistanceCosts
- ? travelCost + obstacleDistanceCost
- : travelCost;
+ const float edgeCost =
+ params.obstacleDistanceCosts ? travelCost + targetDistanceCost : travelCost;
int e = ravel(v, edge_counts[v], max_edges_per_vert);
edges[e] = vp;
@@ -241,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)
@@ -248,15 +260,20 @@ 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
{
+ invalids++;
continue;
}
-
+
output_parents.at(row).at(col) = unravel(parents[u], num_cols);
}
}
+
+ ARMARX_INFO << "Fraction of invalid cells: (" << invalids << "/" << parents.size() << ")";
+
+
// Free memory
delete[] edges;
delete[] edge_counts;
@@ -264,11 +281,9 @@ 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};
- // 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 d0c22c09d238372e9425d7c2ddde378d3e674693..810783e458d35afaebea915e615377b98fc795d5 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
@@ -36,11 +36,13 @@ namespace armarx::navigation::algorithms::spfa
public:
struct Parameters
{
- bool obstacleDistanceCosts{true};
+ bool obstacleDistanceCosts = true;
+ float obstacleMaxDistance = 1000.F;
+
+ float obstacleDistanceWeight = 3.F;
};
- ShortestPathFasterAlgorithm(const algorithm::NavigationCostMap& grid,
- const Parameters& params);
+ ShortestPathFasterAlgorithm(const Costmap& costmap, const Parameters& params);
struct Result
{
@@ -53,7 +55,9 @@ namespace armarx::navigation::algorithms::spfa
const Eigen::Vector2f& goal);
- // protected:
+ Result spfa(const Eigen::Vector2f& start);
+
+ // protected:
/**
* @brief Implementation highly inspired by github.com:jimmyyhwu/spatial-action-maps
*
@@ -65,7 +69,7 @@ namespace armarx::navigation::algorithms::spfa
private:
- const algorithm::NavigationCostMap grid;
+ const Costmap costmap;
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 97c57f6b120d55e2303b590cb23122aa6610db01..bea64e74c6330d292a870874308769ecefb62b61 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 0000000000000000000000000000000000000000..4e685859661210610a8c20060b44c9bf62b1b2dc
--- /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 0000000000000000000000000000000000000000..a991a3d3019a504cbbf315b2edeb3bb3b96fe432
--- /dev/null
+++ b/source/armarx/navigation/algorithms/util.cpp
@@ -0,0 +1,175 @@
+/**
+ * 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 <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>
+#include <VirtualRobot/Workspace/WorkspaceGrid.h>
+
+#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
+{
+
+
+ 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;
+ }
+
+
+ SceneBounds
+ toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends)
+ {
+ return {.min = Eigen::Vector2f{extends.minX, extends.minY},
+ .max = Eigen::Vector2f{extends.maxX, extends.maxY}};
+ }
+
+ 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 Costmap::Grid grid(workspaceGrid.getCells().x, workspaceGrid.getCells().y);
+ const SceneBounds sceneBounds = toSceneBounds(workspaceGrid.getExtends());
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..1a4d1d0e780e26c53adedf562d3d4123825492a7
--- /dev/null
+++ b/source/armarx/navigation/algorithms/util.h
@@ -0,0 +1,49 @@
+
+/**
+ * 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 <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);
+
+ SceneBounds toSceneBounds(const VirtualRobot::WorkspaceGrid::Extends& extends);
+
+ 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
diff --git a/source/armarx/navigation/client/NavigationStackConfig.cpp b/source/armarx/navigation/client/NavigationStackConfig.cpp
index 6acdc7f0e8aaa925ab770f14cbe421fc1bc10358..bfaaf93b57a0a72b803a366fd5940a1d391989d5 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 059305ddf91b90020a878e85dd1de24088c99d99..c7d79e1471beb197c15084ed7385d7c6092d0b40 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/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
index ec6c529d6c3b934de744e2834c3a58f06426f446..123769e24867dff0c0a6026262560197e8e514ae 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/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index e10c6e3ff2c1bab4565e3db237d49d598ccaadbe..bf2bb976f01a4ae725f4b515d3693c923b7b2ef9 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 292b497c9cfcea15a21c6cc0b8ce158d3849cb04..799c560411805e9c29a642d92dc581013727c52c 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 328e21cee7ad6e63ce00ee998ccaaadf2b67f42b..07f364bc81a7a9aa862ffe3a0f0f253321468c16 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 fb665e71ed80222cb4480fd83eb2b19505377813..2ec54dc208f4322b01f63053362c9c6471805e47 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -26,18 +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>
@@ -48,15 +55,21 @@
#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"
#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/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>
#include <armarx/navigation/client/PathBuilder.h>
@@ -215,7 +228,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
@@ -312,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()
@@ -438,16 +460,79 @@ 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()
{
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";
+
+ visualize(costmap, arviz, "distance");
+
+
const armem::vision::occupancy_grid::client::Reader::Result result =
occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
.providerName = "CartographerMappingAndLocalization",
@@ -482,13 +567,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";
@@ -509,13 +594,32 @@ 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()
{
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);
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 2d7411d8dee32e46afc81ae6221ee4203a70c001..45785e2e0cce310e5a4cead02494d1a0189675ff 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/core/CMakeLists.txt b/source/armarx/navigation/core/CMakeLists.txt
index ec1598c68ac4a430c0663a76421f6e9a9e00a855..ca477220b3479fc2e8f21bb3200c87eb89e5a215 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/core/StaticScene.h b/source/armarx/navigation/core/StaticScene.h
index 4d05a2febffa36dfacbf1443ace58c1c68915542..1f4ad33aae03bc2da57da3cec8cb45d003a205b9 100644
--- a/source/armarx/navigation/core/StaticScene.h
+++ b/source/armarx/navigation/core/StaticScene.h
@@ -26,12 +26,15 @@
#include <VirtualRobot/VirtualRobot.h>
+#include <armarx/navigation/algorithms/Costmap.h>
+
namespace armarx::navigation::core
{
struct StaticScene
{
VirtualRobot::SceneObjectSetPtr objects;
+ std::unique_ptr<algorithms::Costmap> costmap;
};
using StaticScenePtr = std::shared_ptr<StaticScene>;
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index 9f5679a0e46cff2f6655ad0b4b4952b25ea4f027..e7bf3ea1d608a7805be467e1b147f2d423074878 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 30b8a70f492e70c2730d43fab23342709d84a06a..661331fb0e427048b9001d9c1da9c9bc2f9c00a0 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 57cee75dfdffb6640c15b49053e0f35e20f7a306..a8fd2caac4355d8eea6898050bb4cccea04626de 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 8e323a973e14203da03c8e168f4253b1801f8e02..c62597ce82a30d3e460fbdbdc72b30ee263f0709 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 464966dfa62ff83484c00c7f3b88288633f6501e..0e5b96facb6dd53297fab1935525624a86b61ac3 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 757b56e84dc861a907d8b27fd1baf38e1dbc81ed..f6e20580bb27557400ebd2bcb28d3177b59d0d55 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>
@@ -13,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;
@@ -24,22 +24,24 @@ 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;
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/factories/GlobalPlannerFactory.h b/source/armarx/navigation/factories/GlobalPlannerFactory.h
index 4ef9a82393c6966cad3c4029bebcaf9f1c97092a..da60c83946c15573daa6b6a3d96f716e2a4704ff 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 e386f55d05ef1cb027667a5e63aeab33d5f87f0b..aed7f192efabc4a752ae77d2a054e1737839688e 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 e6c90a5cf1373dad0b87c13f7e161b4974e5ba70..665bfecbc2e10763d1c296624051981de18216f7 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 f0ee4e1906391595e4fbdb22442e3ad6cdbb1a1d..abd6be4143968e667ce9c9dbd1637f0f6064ee52 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 8dec9fb3dc7eb6cbb2c80a8dc9f725c817e4c804..a4173c4294c15813ceeda73e7857b05bfd502ae4 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 023a762416f59f31f7cd4b15f6a9ccc84856c295..02c427c990f3cd1b1b78693e0a90f0f3f7a8225e 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 c30fe546a39f7e4cbab26c25fb098dc65161c336..277d277deaffc3d019d93ebeb027c3451573ca0c 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 eb603d5f44483d0ff9af2b820726eab3caa7f391..16328e1c76889c6fbdefbab975baf9b4a40bb5a4 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 adb15278328d732d3b2bccda4c35885db313f027..2e7d7fdbefdf5ebc2048dc96704d14885f6ca8f8 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 641492e1027ffea1cc59757872c890c4890f09bf..dde26d2fbe5c5a2c6179570b7873ddf4d89c8a01 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -159,12 +159,8 @@ 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());
diff --git a/source/armarx/navigation/global_planning/CMakeLists.txt b/source/armarx/navigation/global_planning/CMakeLists.txt
index 7100ea9068fc250c98076cceb441e9d2ff259a57..ca9fcf745026385ef960cdcce81abfb866741985 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
@@ -16,16 +17,18 @@ 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
+ ./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 0000000000000000000000000000000000000000..26363016e3727f6dda658faa773300b01e0e6539
--- /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 0000000000000000000000000000000000000000..b3af4fc107b6c2760a0e8d9e017a0bfcf631bef4
--- /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 0000000000000000000000000000000000000000..b1a34a4d350513b1e1189a4a10a4c2c5da671ab8
--- /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 84ec219a337197fdfc1c656975ffcfe14da14047..358efefcb01c7c722d9f278bbea662e01dd7d99e 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 a27a6b0246452e9725187587a9bc7c7c82a14975..d5b14e9dbfe9070056f25468a2465e7e72491291 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 792e7b6f9f36afdc5a438e59375fd33bdca9c877..f8b6eaf7011841c296685500e27216449dda466b 100644
--- a/source/armarx/navigation/global_planning/core.h
+++ b/source/armarx/navigation/global_planning/core.h
@@ -37,7 +37,8 @@ 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 2007edb3906c9ab58c03c8caaa8cf0bc4fc5b2e5..c8854e1fc6b1213c60040380a34a5362a4a7c561 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 b4487009018c118f5d3ed49ba73d65c05aeb7cb7..894b527dcac09505145bfda3e270002a6e3325d9 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 f06f0bde91993d7b98847138de4eea9797127c9b..080b961704568478c2aa18763ab524cc89e679d3 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/local_planning/CMakeLists.txt b/source/armarx/navigation/local_planning/CMakeLists.txt
index 5894b6afaee40284153c101b3a99dc0a6bf71a57..78a91fc0ad08db6ef22a3d443781d824d7aaad44 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/memory/client/graph/Reader.cpp b/source/armarx/navigation/memory/client/graph/Reader.cpp
index 7d3513a1ce38a4d579fcc32753d598fcb8c03bf6..9804927250f74b59c7f160cc65cf7f753de9431a 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 9a65ed9f03657248c62af5b81577adfd3328e564..a61bc7c6dbafcbf24e5e36afa82e3704566b988c 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 d40f300f28d8cc787cc487d736e2f3049e11a8c0..b1b8610d6b680047078e44283b50700e068d17fd 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 d48c3e807aa8b1502e3a5669f548982b6c1cdd67..eda0e83ea2e8fd32b2e28952916f53ab858e108f 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 94108ade932d7be0c1e5eeea79b2a9ec1eb9b3d9..1f1cf51989a0d90585bf8ecc61c71df07bbef0ce 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/CMakeLists.txt b/source/armarx/navigation/server/CMakeLists.txt
index 51cd9baeb7234d53b2b7da46a3bbb4bbcd9d96e4..b9f7a261ce1b87d80a340ede8bb1c34c3c1d3ef2 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
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index 1cd9cf1265893097e3c5ba97b33c6ec0dcd74288..39d45649941b1ee1552ba9163338fbc9c7287cd2 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 c153b5218a529f8db719ad001a638e7c45f8cb0a..18ecf76906c6e5310bee7765b17a72347ee66c7a 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 9382cbae92a4108d9b453ea506ddc42e4cb76f35..34f9e49d72d2a69b3430a15397b5b09c02cb357b 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 94bbd51bbab1b4c2beb8d6f1234f45b9dab8e64d..68067ad9aed8358ec23e99ce28f3646ecb1994af 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 7bcfcc6db4599200ef5501eede53208890aa0924..5fb6ce9232b61ea5dcbfb3d0709a0c87958809dd 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 064de2fc8b7b043bea6658e6ee4f6d3f96beedc8..a9a8a94c46ed3b149d7a48f65e04660a6b5758c6 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 ba409ab3b12782d07b3bc5113b026a681c69b204..3438ce6719616a684adb532fc0a96ad58fbcc725 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 c15487f1ae194847c8a53e74fa52f389ba786dcd..805b7f7c6a1dafeac257ab432084bbe9c2d168af 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