diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index 23422ec563c43b6534270afe79d03b6f9703f167..abdba073df9b7bfeba96db61f69724ba2dbb5a1e 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 0000000000000000000000000000000000000000..91f0b6a694ba5d9cd99522635fd123bb42de65ae
--- /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 031170efbaa9ec93beac9793f508b3578ef1330f..6580d25aa5600813ef76e81b1f61066a71873968 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 4c91b8330c4f53b4303db407abe53c9fa8f2b5f1..531412d217bd0af029b37cd55c3db09ec0f641d3 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 c0900376727e7d1ab8bfb192669e903b15c5f598..2246f92bf8693706512c2ec28cebbdd00bf483ab 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 63b61b6f9b54346494a9c58db5e8e15b7805f82d..c1b2869ffa7a8b3884de5d37dd782a132ebd7c3c 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 358efefcb01c7c722d9f278bbea662e01dd7d99e..ef57566f2844cf8c3cee3a4e441e6dedde20506d 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 857ad24282a527fc40fe436a9c1a7703a41d7c04..f008cccd43c3626b78ab5e980ae3b2da792eacab 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;