From 8e5faf80a2498729d285e0ead3cce5bbb507de38 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 28 Sep 2022 10:09:15 +0200
Subject: [PATCH] making spfa algorithms accessible via aron and spfa params
struct
---
.../navigation/algorithms/CMakeLists.txt | 1 +
.../ShortestPathFasterAlgorithmParams.xml | 23 ++++++++++++++++
.../algorithms/aron_conversions.cpp | 26 +++++++++++++++++--
.../navigation/algorithms/aron_conversions.h | 10 +++++++
.../armarx/navigation/global_planning/SPFA.h | 3 +++
.../global_planning/aron/SPFAParams.xml | 5 ++++
.../global_planning/aron_conversions.cpp | 4 +++
.../navigation/simulation/SimulatedHuman.cpp | 2 ++
8 files changed, 72 insertions(+), 2 deletions(-)
create mode 100644 source/armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml
diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index 23422ec5..abdba073 100644
--- a/source/armarx/navigation/algorithms/CMakeLists.txt
+++ b/source/armarx/navigation/algorithms/CMakeLists.txt
@@ -1,6 +1,7 @@
armarx_add_aron_library(algorithms_aron
ARON_FILES
aron/Costmap.xml
+ aron/ShortestPathFasterAlgorithmParams.xml
)
armarx_add_library(algorithms
diff --git a/source/armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml b/source/armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml
new file mode 100644
index 00000000..91f0b6a6
--- /dev/null
+++ b/source/armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+ <Object name='armarx::navigation::algorithms::arondto::ShortestPathFasterAlgorithmParams'>
+ <ObjectChild key='obstacleDistanceCosts'>
+ <bool />
+ </ObjectChild>
+ <ObjectChild key='obstacleMaxDistance'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='obstacleDistanceWeight'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='obstacleCostExponent'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/algorithms/aron_conversions.cpp b/source/armarx/navigation/algorithms/aron_conversions.cpp
index 031170ef..6580d25a 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.cpp
+++ b/source/armarx/navigation/algorithms/aron_conversions.cpp
@@ -2,10 +2,11 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.aron.generated.h>
#include <armarx/navigation/conversions/eigen.h>
@@ -70,7 +71,7 @@ namespace armarx::navigation::algorithms
ARMARX_DEBUG << "Converting grid";
const auto gridNavigator =
aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("grid"));
-
+
ARMARX_CHECK_NOT_NULL(gridNavigator);
Costmap::Grid grid =
@@ -95,5 +96,26 @@ namespace armarx::navigation::algorithms
return {grid, parameters, sceneBounds, mask};
}
+ void
+ toAron(arondto::ShortestPathFasterAlgorithmParams& dto,
+ const spfa::ShortestPathFasterAlgorithm::Parameters& bo)
+ {
+ dto.obstacleDistanceCosts = bo.obstacleDistanceCosts;
+ dto.obstacleMaxDistance = bo.obstacleMaxDistance;
+ dto.obstacleDistanceWeight = bo.obstacleDistanceWeight;
+ dto.obstacleCostExponent = bo.obstacleCostExponent;
+ }
+
+
+ void
+ fromAron(const arondto::ShortestPathFasterAlgorithmParams& dto,
+ spfa::ShortestPathFasterAlgorithm::Parameters& bo)
+ {
+ bo.obstacleDistanceCosts = dto.obstacleDistanceCosts;
+ bo.obstacleMaxDistance = dto.obstacleMaxDistance;
+ bo.obstacleDistanceWeight = dto.obstacleDistanceWeight;
+ bo.obstacleCostExponent = dto.obstacleCostExponent;
+ }
+
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/aron_conversions.h b/source/armarx/navigation/algorithms/aron_conversions.h
index 4c91b833..531412d2 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.h
+++ b/source/armarx/navigation/algorithms/aron_conversions.h
@@ -22,6 +22,7 @@
#pragma once
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include "armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h"
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
@@ -43,5 +44,14 @@ namespace armarx::navigation::algorithms
// Costmap does not provide a default c'tor
Costmap fromAron(const armem::wm::EntityInstance& entityInstance);
+ namespace arondto
+ {
+ class ShortestPathFasterAlgorithmParams;
+ }
+
+ void toAron(arondto::ShortestPathFasterAlgorithmParams& dto, const spfa::ShortestPathFasterAlgorithm::Parameters& bo);
+
+ void fromAron(const arondto::ShortestPathFasterAlgorithmParams& dto, spfa::ShortestPathFasterAlgorithm::Parameters& bo);
+
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/global_planning/SPFA.h b/source/armarx/navigation/global_planning/SPFA.h
index c0900376..2246f92b 100644
--- a/source/armarx/navigation/global_planning/SPFA.h
+++ b/source/armarx/navigation/global_planning/SPFA.h
@@ -24,6 +24,8 @@
#include <VirtualRobot/MathTools.h>
#include "GlobalPlanner.h"
+#include "armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h"
+#include "armarx/navigation/global_planning/core.h"
#include <armarx/navigation/core/types.h>
namespace armarx::navigation::global_planning
@@ -45,6 +47,7 @@ namespace armarx::navigation::global_planning
float resampleDistance{-1};
+ algorithms::spfa::ShortestPathFasterAlgorithm::Parameters algo;
Algorithms algorithm() const override;
diff --git a/source/armarx/navigation/global_planning/aron/SPFAParams.xml b/source/armarx/navigation/global_planning/aron/SPFAParams.xml
index 63b61b6f..c1b2869f 100644
--- a/source/armarx/navigation/global_planning/aron/SPFAParams.xml
+++ b/source/armarx/navigation/global_planning/aron/SPFAParams.xml
@@ -2,6 +2,7 @@
<AronTypeDefinition>
<AronIncludes>
<Include include="armarx/navigation/global_planning/aron/GlobalPlannerParams.xml" />
+ <Include include="armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.xml" />
</AronIncludes>
<GenerateTypes>
@@ -15,6 +16,10 @@
<float />
</ObjectChild>
+ <ObjectChild key='algo'>
+ <armarx::navigation::algorithms::arondto::ShortestPathFasterAlgorithmParams/>
+ </ObjectChild>
+
</Object>
diff --git a/source/armarx/navigation/global_planning/aron_conversions.cpp b/source/armarx/navigation/global_planning/aron_conversions.cpp
index 358efefc..ef57566f 100644
--- a/source/armarx/navigation/global_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/global_planning/aron_conversions.cpp
@@ -4,6 +4,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/Point2Point.h>
@@ -69,6 +70,7 @@ namespace armarx::navigation::global_planning::aron_conv
aron::toAron(dto.linearVelocity, bo.linearVelocity);
aron::toAron(dto.resampleDistance, bo.resampleDistance);
+ algorithms::toAron(dto.algo, bo.algo);
}
void
@@ -79,6 +81,8 @@ namespace armarx::navigation::global_planning::aron_conv
aron::fromAron(dto.linearVelocity, bo.linearVelocity);
aron::fromAron(dto.resampleDistance, bo.resampleDistance);
+
+ algorithms::fromAron(dto.algo, bo.algo);
}
} // namespace armarx::navigation::global_planning::aron_conv
diff --git a/source/armarx/navigation/simulation/SimulatedHuman.cpp b/source/armarx/navigation/simulation/SimulatedHuman.cpp
index 857ad242..f008cccd 100644
--- a/source/armarx/navigation/simulation/SimulatedHuman.cpp
+++ b/source/armarx/navigation/simulation/SimulatedHuman.cpp
@@ -26,6 +26,7 @@
#include "ArmarXCore/core/logging/Logging.h"
#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h"
#include "armarx/navigation/algorithms/util.h"
#include "armarx/navigation/conversions/eigen.h"
#include "armarx/navigation/core/StaticScene.h"
@@ -103,6 +104,7 @@ namespace armarx::navigation::human::simulation
{
global_planning::SPFA::Params params;
params.linearVelocity = this->params_.maxLinearVelocity;
+ params.algo.obstacleDistanceWeight = 1.1F;
core::Scene scene;
--
GitLab