diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8697c7078213802928bea6f60bfe68652d240d56..0c16bde85be36e2c4976a6cde6850b0fa652237e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,7 @@ armarx_project(navigation NAMESPACE armarx)
armarx_find_package(PUBLIC ArmarXGui)
armarx_find_package(PUBLIC RobotAPI REQUIRED)
armarx_find_package(PUBLIC MemoryX QUIET)
+armarx_find_package(PUBLIC VisionX QUIET)
add_subdirectory(etc)
add_subdirectory(external)
diff --git a/scenarios/NavigationSimulation/config/ObjectMemory.cfg b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
index 88bf3016ae320afc3e22aa8663eefe9051fef6ef..af2b34b517b8ade919d9834b7aa6bec6ac07b1be 100644
--- a/scenarios/NavigationSimulation/config/ObjectMemory.cfg
+++ b/scenarios/NavigationSimulation/config/ObjectMemory.cfg
@@ -213,7 +213,7 @@ ArmarX.ObjectMemory.ltm.00_enabled = false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.cls.Floor.Show = true
+ArmarX.ObjectMemory.mem.cls.Floor.Show = 0
# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
@@ -372,7 +372,7 @@ ArmarX.ObjectMemory.ltm.00_enabled = false
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = ""
# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName: Name of the Instance core segment.
diff --git a/scenarios/NavigationSimulation/config/SimulatorApp.cfg b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
index 815f7430bd8c8e21fdba45c919e7784e4389b204..065a687b123aecf5f9514d124f5660f82d89ff4e 100644
--- a/scenarios/NavigationSimulation/config/SimulatorApp.cfg
+++ b/scenarios/NavigationSimulation/config/SimulatorApp.cfg
@@ -284,12 +284,12 @@ ArmarX.Simulator.InitialRobotConfig = ArmR5_Elb1:1.57, ArmL5_Elb1:1.57
# ArmarX.Simulator.InitialRobotPose.roll_2 = 0
-# ArmarX.Simulator.InitialRobotPose.x: x component of initial robot position (mm)
+# ArmarX.Simulator.InitialRobotPose.x: Custom Property
# Attributes:
-# - Default: 0
-# - Case sensitivity: yes
+# - Default: ::NOT_DEFINED::
+# - Case sensitivity: no
# - Required: no
-ArmarX.Simulator.InitialRobotPose.x = 1300
+ArmarX.Simulator.InitialRobotPose.x = 0
# ArmarX.Simulator.InitialRobotPose.x_0: x component of initial robot position (mm)
@@ -321,7 +321,7 @@ ArmarX.Simulator.InitialRobotPose.x = 1300
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-ArmarX.Simulator.InitialRobotPose.y = -300
+ArmarX.Simulator.InitialRobotPose.y = 0
# ArmarX.Simulator.InitialRobotPose.y_0: y component of initial robot position (mm)
@@ -353,7 +353,7 @@ ArmarX.Simulator.InitialRobotPose.y = -300
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-ArmarX.Simulator.InitialRobotPose.yaw = 1.57
+# ArmarX.Simulator.InitialRobotPose.yaw = 0
# ArmarX.Simulator.InitialRobotPose.yaw_0: Initial robot pose: yaw component of RPY angles (radian)
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index 970c22d76dd86df007d9f9514607f62377437329..ba7b7dfb58a1faf3277de62ac40de8fa7cbb9e5b 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -7,5 +7,6 @@
<application name="navigator" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="navigation_memory" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="VisionMemory" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/PlatformNavigation/config/MemoryNameSystem.cfg b/scenarios/PlatformNavigation/config/MemoryNameSystem.cfg
index 7dd22218243ca4f9e67e843da8b42916f3b8568a..b8bc70a66ca7f32a628886ad1bf13e373f9750d3 100644
--- a/scenarios/PlatformNavigation/config/MemoryNameSystem.cfg
+++ b/scenarios/PlatformNavigation/config/MemoryNameSystem.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/PlatformNavigation/config/ObjectMemory.cfg b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
index 00f98d515449340628d1d3c97b062e3ed55ee469..eae3a98a41a64ec9a70ba6c5456c37501b6552b3 100644
--- a/scenarios/PlatformNavigation/config/ObjectMemory.cfg
+++ b/scenarios/PlatformNavigation/config/ObjectMemory.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/PlatformNavigation/config/RemoteGuiProviderApp.cfg b/scenarios/PlatformNavigation/config/RemoteGuiProviderApp.cfg
index 4fd690cefd94559b207493cf40e346a3e47f3b12..4b6abea40d72afd7d313ee47a9b191f3b26de30d 100644
--- a/scenarios/PlatformNavigation/config/RemoteGuiProviderApp.cfg
+++ b/scenarios/PlatformNavigation/config/RemoteGuiProviderApp.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/PlatformNavigation/config/RobotStateComponent.cfg b/scenarios/PlatformNavigation/config/RobotStateComponent.cfg
index 9c5690ce9fcfd8be7648a3cd9aacd1bfdee1ad57..22eb7df3e1c9966d56445d7c45fb12908cc00c62 100644
--- a/scenarios/PlatformNavigation/config/RobotStateComponent.cfg
+++ b/scenarios/PlatformNavigation/config/RobotStateComponent.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..1e9d06004511f601b82a565208f8292aee182c70
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -0,0 +1,239 @@
+# ==================================================================
+# VisionMemory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
+# ArmarX.VisionMemory.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.EnableProfiling = false
+
+
+# ArmarX.VisionMemory.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.VisionMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.VisionMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.ObjectName = ""
+
+
+# ArmarX.VisionMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.MemoryName = Vision
+
+
+# ArmarX.VisionMemory.mem.ltm.00_enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.00_enabled = true
+
+
+# ArmarX.VisionMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.VisionMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.VisionMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.VisionMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.tpc.sub.MemoryListener = MemoryUpdates
+
+
diff --git a/scenarios/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index b565142e13ddce514969d321bd546f67a37abc43..c51d2b49152134c46ed72c5a3801107f50051a9c 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -201,7 +201,7 @@
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.NavigationMemory.p.snapshotToLoad = PriorKnowledgeData/navigation-graphs/R003
+ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
# ArmarX.NavigationMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 525d71c323b3c009caeba5e49a7edab0165a3d84..4873c6bbc55e212ef90bafa9f8d95dfaba76c5ae 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -278,6 +278,22 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.Navigator.mem.robot_state.proprioceptionSegment = Proprioception
+# ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
+
+
+# ArmarX.Navigator.mem.vision.occupancy_grid.Memory:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.Navigator.mem.vision.occupancy_grid.Memory = Vision
+
+
# ArmarX.Navigator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
@@ -296,6 +312,14 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.Navigator.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.Navigator.p.occupancy_grid.occopied_threshold: Threshold for each cell to be considered occupied. Increase this value to reduce noise.
+# Attributes:
+# - Default: 0.550000012
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
+
+
# ArmarX.Navigator.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
# Attributes:
# - Default: MemoryUpdates
@@ -369,6 +393,6 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+ArmarX.Verbosity = Verbose
diff --git a/source/armarx/navigation/algorithms/CMakeLists.txt b/source/armarx/navigation/algorithms/CMakeLists.txt
index 5507cdeb2f34973cd052216cd079f8087617595c..295e35cf027586e1a8f049dd8af03d2585c9afd9 100644
--- a/source/armarx/navigation/algorithms/CMakeLists.txt
+++ b/source/armarx/navigation/algorithms/CMakeLists.txt
@@ -1,9 +1,13 @@
+find_package(OpenCV REQUIRED )
+
armarx_add_library(algorithms
DEPENDENCIES
ArmarXCoreInterfaces
ArmarXCore
armarx_navigation::core
armarx_navigation::conversions
+ DEPENDENCIES_LEGACY
+ OpenCV
SOURCES
./algorithms.cpp
# A*
@@ -11,6 +15,7 @@ armarx_add_library(algorithms
./astar/NavigationCostMap.cpp
./astar/Node.cpp
./astar/Planner2D.cpp
+ ./astar/util.cpp
# smoothing
./smoothing/ChainApproximation.cpp
./smoothing/CircularPathSmoothing.cpp
@@ -21,6 +26,7 @@ armarx_add_library(algorithms
./astar/NavigationCostMap.h
./astar/Node.h
./astar/Planner2D.h
+ ./astar/util.h
# smoothing
./smoothing/ChainApproximation.h
./smoothing/CircularPathSmoothing.h
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 138b60d00f8e1f6df089ac8c4f53170087f12b7e..0386c314dea9df3f0b609fe0eb022a7cf380432b 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -34,8 +34,8 @@ namespace armarx::navigation::algorithm::astar
const auto toPoint = [](const Eigen::Vector3f& pt)
{ return point_t(pt.x(), pt.y(), pt.z()); };
- box_t box1(toPoint(bb1.getMin()), toPoint(bb1.getMax()));
- box_t box2(toPoint(bb2.getMin()), toPoint(bb2.getMax()));
+ const box_t box1(toPoint(bb1.getMin()), toPoint(bb1.getMax()));
+ const box_t box2(toPoint(bb2.getMin()), toPoint(bb2.getMax()));
return bg::intersects(box1, box2);
}
@@ -45,10 +45,11 @@ namespace armarx::navigation::algorithm::astar
float cellSize) :
Planner2D(robot, obstacles), cellSize(cellSize)
{
+ ARMARX_CHECK_GREATER_EQUAL(static_cast<int>(obstacles->getSize()), 0);
}
float
- AStarPlanner::heuristic(NodePtr n1, NodePtr n2)
+ AStarPlanner::heuristic(const NodePtr& n1, const NodePtr& n2)
{
return (n1->position - n2->position).norm();
}
@@ -58,13 +59,13 @@ namespace armarx::navigation::algorithm::astar
{
ARMARX_TRACE;
- ARMARX_DEBUG << "Scene bounds are " << sceneBoundsMin << " and " << sceneBoundsMax;
+ 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_DEBUG << "Grid size: " << rows << ", " << cols;
+ ARMARX_VERBOSE << "Grid size: " << rows << ", " << cols;
for (size_t r = 0; r < rows; r++)
{
@@ -115,7 +116,7 @@ namespace armarx::navigation::algorithm::astar
}
bool
- AStarPlanner::fulfillsConstraints(NodePtr n)
+ AStarPlanner::fulfillsConstraints(const NodePtr& n)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(n);
@@ -131,7 +132,6 @@ namespace armarx::navigation::algorithm::astar
VirtualRobot::BoundingBox robotBbox = robotCollisionModel->getBoundingBox(true);
Eigen::Vector3f P1, P2;
- int id1, id2;
for (size_t i = 0; i < obstacles->getSize(); i++)
{
// cheap collision check
@@ -143,16 +143,12 @@ namespace armarx::navigation::algorithm::astar
}
// precise collision check
- float dist =
- VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
+ bool collision =
+ VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
robotCollisionModel,
- obstacles->getSceneObject(i)->getCollisionModel(),
- P1,
- P2,
- &id1,
- &id2);
+ obstacles->getSceneObject(i)->getCollisionModel());
- if (dist <= cellSize / 2)
+ if (collision)
{
return false;
}
@@ -162,22 +158,22 @@ namespace armarx::navigation::algorithm::astar
}
NodePtr
- AStarPlanner::closestNode(Eigen::Vector2f v)
+ AStarPlanner::closestNode(const Eigen::Vector2f& v)
{
float r = (v.y() - cellSize / 2 - sceneBoundsMin.y()) / cellSize;
float c = (v.x() - cellSize / 2 - sceneBoundsMin.x()) / cellSize;
- ARMARX_CHECK(r >= 0.F);
- ARMARX_CHECK(c >= 0.F);
+ ARMARX_CHECK_GREATER_EQUAL(r ,0.F);
+ ARMARX_CHECK_GREATER_EQUAL(c ,0.F);
- ARMARX_CHECK(r <= (rows - 1));
- ARMARX_CHECK(c <= (cols - 1));
+ ARMARX_CHECK_LESS_EQUAL(r, rows - 1);
+ ARMARX_CHECK_LESS_EQUAL(c, cols - 1);
return grid[static_cast<int>(r)][static_cast<int>(c)];
}
std::vector<Eigen::Vector2f>
- AStarPlanner::plan(Eigen::Vector2f start, Eigen::Vector2f goal)
+ AStarPlanner::plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal)
{
ARMARX_TRACE;
grid.clear();
@@ -210,10 +206,13 @@ namespace armarx::navigation::algorithm::astar
ARMARX_CHECK(fulfillsConstraints(nodeGoal)) << "Goal node in collision!";
std::vector<NodePtr> closedSet;
+
std::vector<NodePtr> openSet;
openSet.push_back(nodeStart);
+
std::map<NodePtr, float> gScore;
gScore[nodeStart] = 0;
+
std::map<NodePtr, float> fScore;
fScore[nodeStart] = gScore[nodeStart] + heuristic(nodeStart, nodeGoal);
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.h b/source/armarx/navigation/algorithms/astar/AStarPlanner.h
index beb5410be66cbf517863417f1ff7dcddcbf59f77..a650032a0c577d58aa1a583b7f44b0f1939998bb 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.h
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.h
@@ -22,15 +22,15 @@ namespace armarx::navigation::algorithm::astar
VirtualRobot::SceneObjectSetPtr obstacles = {},
float cellSize = 100.f);
- std::vector<Eigen::Vector2f> plan(Eigen::Vector2f start, Eigen::Vector2f goal);
+ std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal) override;
void setRobotCollisionModel(const VirtualRobot::CollisionModelPtr& collisionModel);
private:
- float heuristic(NodePtr n1, NodePtr n2);
+ float heuristic(const NodePtr& n1, const NodePtr& n2);
void createUniformGrid();
- bool fulfillsConstraints(NodePtr n);
- NodePtr closestNode(Eigen::Vector2f v);
+ bool fulfillsConstraints(const NodePtr& n);
+ NodePtr closestNode(const Eigen::Vector2f& v);
private:
/// How big each cell is in the uniform grid.
diff --git a/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp b/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp
index 08bfd27ad97f59aae72bbce55a41e31fdd9ae2c8..8900c79926c99ed66fa24396c8c83c7c4503731e 100644
--- a/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp
+++ b/source/armarx/navigation/algorithms/astar/NavigationCostMap.cpp
@@ -14,6 +14,7 @@
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -26,21 +27,6 @@ namespace bg = boost::geometry;
namespace armarx::navigation::algorithm::astar
{
- bool
- intersects(const VirtualRobot::BoundingBox& bb1, const VirtualRobot::BoundingBox& bb2)
- {
- using point_t = bg::model::point<double, 3, bg::cs::cartesian>;
- using box_t = bg::model::box<point_t>;
-
- const auto toPoint = [](const Eigen::Vector3f& pt)
- { return point_t(pt.x(), pt.y(), pt.z()); };
-
- box_t box1(toPoint(bb1.getMin()), toPoint(bb1.getMax()));
- box_t box2(toPoint(bb2.getMin()), toPoint(bb2.getMax()));
-
- return bg::intersects(box1, box2);
- }
-
NavigationCostMap::NavigationCostMap(VirtualRobot::RobotPtr robot,
VirtualRobot::SceneObjectSetPtr obstacles,
const Parameters& parameters) :
@@ -64,15 +50,32 @@ namespace armarx::navigation::algorithm::astar
{
ARMARX_TRACE;
- ARMARX_DEBUG << "Scene bounds are " << sceneBoundsMin << " and " << sceneBoundsMax;
+ if (obstacles)
+ {
+ for (size_t i = 0; i < obstacles->getCollisionModels().size(); i++)
+ {
+ VirtualRobot::BoundingBox bb = obstacles->getCollisionModels()[i]->getBoundingBox();
+ sceneBoundsMin.x() = std::min(bb.getMin().x(), sceneBoundsMin.x());
+ sceneBoundsMin.y() = std::min(bb.getMin().y(), sceneBoundsMin.y());
+ sceneBoundsMax.x() = std::max(bb.getMax().x(), sceneBoundsMax.x());
+ sceneBoundsMax.y() = std::max(bb.getMax().y(), sceneBoundsMax.y());
+ }
+ }
+
+ ARMARX_INFO << "Scene bounds are " << sceneBoundsMin << " and " << sceneBoundsMax;
//+1 for explicit rounding up
c_x = (sceneBoundsMax.x() - sceneBoundsMin.x()) / parameters.cellSize + 1;
c_y = (sceneBoundsMax.y() - sceneBoundsMin.y()) / parameters.cellSize + 1;
- ARMARX_DEBUG << "Grid size: " << c_y << ", " << c_x;
+ ARMARX_INFO << "Grid size: " << c_y << ", " << c_x;
+
+ ARMARX_INFO << "Resetting grid";
+ grid = Eigen::MatrixXf(c_x, c_y); //.array();
+ // grid = Eigen::ArrayXf(); //(c_x, c_y);
+ // grid->resize(c_x, c_y);
- grid = Eigen::ArrayXf(c_x, c_y);
+ ARMARX_INFO << "done.";
}
@@ -87,6 +90,9 @@ namespace armarx::navigation::algorithm::astar
const core::Pose globalPose = Eigen::Translation3f(conv::to3D(position)) *
Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
+ // auto colModel = robotCollisionModel->clone();
+ // colModel->setGlobalPose(globalPose.matrix());
+
robotCollisionModel->setGlobalPose(globalPose.matrix());
VirtualRobot::BoundingBox robotBbox = robotCollisionModel->getBoundingBox(true);
@@ -119,7 +125,9 @@ namespace armarx::navigation::algorithm::astar
&id2);
// check if objects collide
- if (dist <= parameters.cellSize / 2)
+ if ((dist <= parameters.cellSize / 2) or
+ VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
+ robotCollisionModel, obstacles->getSceneObject(i)->getCollisionModel()))
{
minDistance = 0;
break;
@@ -154,7 +162,19 @@ namespace armarx::navigation::algorithm::astar
ARMARX_TRACE;
std::vector<Eigen::Vector2f> result;
- ARMARX_CHECK_NOT_NULL(robot);
+ ARMARX_CHECK_NOT_NULL(robot) << "Robot must be set";
+
+
+ VirtualRobot::CoinVisualizationFactory factory;
+ const auto cylinder = factory.createCylinder(1500.F / 2, 2000.F);
+
+ VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(cylinder));
+
+
+ robotCollisionModel = collisionModel;
+
+
+ ARMARX_CHECK_NOT_NULL(robotCollisionModel) << "Collision model must be set!";
// ARMARX_CHECK(robot->hasRobotNode(robotColModelName)) << "No robot node " << robotColModelName;
// ARMARX_CHECK(robot->getRobotNode(robotColModelName)->getCollisionModel())
@@ -171,9 +191,13 @@ namespace armarx::navigation::algorithm::astar
ARMARX_CHECK_NOT_NULL(robotCollisionModel);
+ ARMARX_INFO << "Creating grid";
createUniformGrid();
+ ARMARX_INFO << "Filling grid";
fillGridCosts();
+
+ ARMARX_INFO << "Filled grid";
}
void
@@ -182,16 +206,23 @@ namespace armarx::navigation::algorithm::astar
robotCollisionModel = collisionModel;
}
- void NavigationCostMap::fillGridCosts()
+ void
+ NavigationCostMap::fillGridCosts()
{
- for (int x = 0; x < c_x; x++)
+ int i = 0;
+
+ for (unsigned int x = 0; x < c_x; x++)
{
- for (int y = 0; y < c_y; y++)
+ for (unsigned int y = 0; y < c_y; y++)
{
const Index index{x, y};
const Position position = toPosition(index);
- grid(x, y) = computeCost(position);
+ grid.value()(x, y) = computeCost(position);
+
+ i++;
+
+ ARMARX_INFO << i << "/" << c_x * c_y;
}
}
}
diff --git a/source/armarx/navigation/algorithms/astar/NavigationCostMap.h b/source/armarx/navigation/algorithms/astar/NavigationCostMap.h
index d6bacdcf88c82bf2a97c76be19d6251e33b380db..d03750d5aba8ab7ed2a3c6abb435a94e2adaefc1 100644
--- a/source/armarx/navigation/algorithms/astar/NavigationCostMap.h
+++ b/source/armarx/navigation/algorithms/astar/NavigationCostMap.h
@@ -24,7 +24,7 @@ namespace armarx::navigation::algorithm::astar
struct Parameters
{
// if set to false, distance to obstacles will be computed and not only a binary collision check
- bool binaryGrid{false};
+ bool binaryGrid{false};
/// How big each cell is in the uniform grid.
float cellSize = 100.f;
@@ -40,6 +40,7 @@ namespace armarx::navigation::algorithm::astar
using Index = Eigen::Array2i;
using Position = Eigen::Vector2f;
+ using Grid = Eigen::MatrixXf;
struct Vertex
{
@@ -47,6 +48,14 @@ namespace armarx::navigation::algorithm::astar
Position position;
};
+ Position toPosition(const Index& index) const;
+
+ const Grid&
+ getGrid() const
+ {
+ return grid.value();
+ }
+
private:
void createUniformGrid();
float computeCost(const NavigationCostMap::Position& position);
@@ -55,11 +64,7 @@ namespace armarx::navigation::algorithm::astar
void fillGridCosts();
private:
-
- Eigen::ArrayXf grid;
-
- Position toPosition(const Index& index) const;
-
+ std::optional<Grid> grid;
size_t c_x = 0;
@@ -71,8 +76,8 @@ namespace armarx::navigation::algorithm::astar
VirtualRobot::CollisionModelPtr robotCollisionModel;
- const Eigen::Vector2f sceneBoundsMin;
- const Eigen::Vector2f sceneBoundsMax;
+ Eigen::Vector2f sceneBoundsMin;
+ Eigen::Vector2f sceneBoundsMax;
const Parameters parameters;
};
diff --git a/source/armarx/navigation/algorithms/astar/Planner2D.h b/source/armarx/navigation/algorithms/astar/Planner2D.h
index c4cd26862a112fc8fc4a8cc3cfb7ccb0c415f710..5fb3b7e6c9e67ed38c8434f0414eec34b03f28ea 100644
--- a/source/armarx/navigation/algorithms/astar/Planner2D.h
+++ b/source/armarx/navigation/algorithms/astar/Planner2D.h
@@ -26,7 +26,7 @@ namespace armarx::navigation::algorithm::astar
virtual ~Planner2D() = default;
// planners implement this method
- virtual std::vector<Eigen::Vector2f> plan(Eigen::Vector2f start, 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/astar/util.cpp b/source/armarx/navigation/algorithms/astar/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..abc91b031633988a1d5271af700abe0d153c6d58
--- /dev/null
+++ b/source/armarx/navigation/algorithms/astar/util.cpp
@@ -0,0 +1,37 @@
+#include "util.h"
+
+#include <cstddef>
+
+#include <Eigen/src/Core/util/Meta.h>
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/mat.hpp>
+#include <opencv2/opencv.hpp>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+namespace armarx::navigation
+{
+ void
+ dumpToFile(const Eigen::MatrixXf& grid)
+ {
+
+ ARMARX_INFO << "Dumping to file.";
+
+ ARMARX_INFO << "Shape: " << grid.rows() << ", " << grid.cols();
+ cv::Size size(grid.rows(), grid.cols());
+
+ cv::Mat1f mat(size);
+
+ for (size_t r = 0; r < grid.rows(); r++)
+ {
+ for (size_t c = 0; c < grid.cols(); c++)
+ {
+ mat(cv::Point(r, c)) = grid(r, c);
+ }
+ }
+
+
+ cv::imwrite("/tmp/grid.exr", mat);
+ }
+} // namespace armarx::navigation
diff --git a/source/armarx/navigation/algorithms/astar/util.h b/source/armarx/navigation/algorithms/astar/util.h
new file mode 100644
index 0000000000000000000000000000000000000000..335c9c6d11813854489ffc0fc9cbb44c381f1586
--- /dev/null
+++ b/source/armarx/navigation/algorithms/astar/util.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx::navigation
+{
+
+ void dumpToFile(const Eigen::MatrixXf& grid);
+
+}
diff --git a/source/armarx/navigation/client/services/MemorySubscriber.cpp b/source/armarx/navigation/client/services/MemorySubscriber.cpp
index fc3b37fd8712efb9a125be41e97a61efc99a74d4..e287d426c8a30f8da960c3bd5d5bd6fe5db705ae 100644
--- a/source/armarx/navigation/client/services/MemorySubscriber.cpp
+++ b/source/armarx/navigation/client/services/MemorySubscriber.cpp
@@ -133,8 +133,8 @@ namespace armarx::navigation::client
.snapshots().timeRange(lastMemoryPoll, now);
// clang-format on
- ARMARX_INFO << "Polling memory events in interval "
- << "[" << lastMemoryPoll << ", " << now << "]";
+ ARMARX_DEBUG << "Polling memory events in interval "
+ << "[" << lastMemoryPoll << ", " << now << "]";
lastMemoryPoll = now;
diff --git a/source/armarx/navigation/components/Navigator/CMakeLists.txt b/source/armarx/navigation/components/Navigator/CMakeLists.txt
index 07a168e3c8025a792b1fcf949ad9117dc0034a91..57f641b3d5aa40db997bf83f10a83b849fd981b0 100644
--- a/source/armarx/navigation/components/Navigator/CMakeLists.txt
+++ b/source/armarx/navigation/components/Navigator/CMakeLists.txt
@@ -17,6 +17,7 @@ armarx_add_component(navigator
RobotAPIComponentPlugins # For ArViz and other plugins.
armem_robot
armem_robot_state
+ armem_vision
# This project ${PROJECT_NAME}Interfaces # For ice interfaces from this
# package. This component
armarx_navigation::server
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 5a73e948efad1dfb83ddf2d256f6184a99ab2beb..e61ad3ea8b79173fcda181fea6919cf9ec138946 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -48,9 +48,15 @@
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "RobotAPI/components/ArViz/Client/elements/Color.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/astar/NavigationCostMap.h"
+#include <armarx/navigation/algorithms/astar/util.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
@@ -96,6 +102,7 @@ namespace armarx::navigation::components
eventsWriter(memoryNameSystem()),
resultsWriter(memoryNameSystem()),
graphReader(memoryNameSystem()),
+ occupancyGridReader(memoryNameSystem()),
virtualRobotReader(memoryNameSystem()),
parameterizationService(¶meterizationReader, ¶meterizationWriter)
// publisher(&resultsWriter, &eventsWriter)
@@ -133,7 +140,7 @@ namespace armarx::navigation::components
// parameterizationReader.connect();
parameterizationWriter.connect();
graphReader.connect();
-
+ occupancyGridReader.connect();
virtualRobotReader.connect();
// initialize scene
@@ -180,6 +187,7 @@ namespace armarx::navigation::components
parameterizationWriter.connect();
graphReader.connect();
virtualRobotReader.connect();
+ occupancyGridReader.connect();
navRemoteGui->enable();
}
@@ -385,7 +393,10 @@ namespace armarx::navigation::components
// def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
// Add an optionalproperty.
- // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+ def->optional(params.occupiedGridThreshold,
+ "p.occupancy_grid.occopied_threshold",
+ "Threshold for each cell to be considered occupied. Increase this value to "
+ "reduce noise.");
resultsWriter.registerPropertyDefinitions(def);
eventsWriter.registerPropertyDefinitions(def);
@@ -393,6 +404,7 @@ namespace armarx::navigation::components
parameterizationWriter.registerPropertyDefinitions(def);
graphReader.registerPropertyDefinitions(def);
virtualRobotReader.registerPropertyDefinitions(def);
+ occupancyGridReader.registerPropertyDefinitions(def);
return def;
}
@@ -405,7 +417,65 @@ namespace armarx::navigation::components
const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
core::StaticScene scene{.objects = util::asSceneObjects(objectPoses)};
- ARMARX_INFO << "The scene consists of " << scene.objects->getSize() << " objects";
+ ARMARX_INFO << "The object scene consists of " << scene.objects->getSize() << " objects";
+
+ const armem::vision::occupancy_grid::client::Reader::Result result =
+ occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
+ .providerName = "CartographerMappingAndLocalization",
+ .timestamp = armem::Time::now()});
+
+ if (result and result.occupancyGrid.has_value())
+ {
+ ARMARX_INFO << "Occupancy grid available!";
+
+ const auto occupancyGridSceneElements = util::asSceneObjects(
+ result.occupancyGrid.value(),
+ OccupancyGridHelper::Params{.freespaceThreshold = 0.45F,
+ .occupiedThreshold = params.occupiedGridThreshold});
+ ARMARX_INFO << occupancyGridSceneElements->getSize()
+ << " scene elements from occupancy grid";
+
+ scene.objects->addSceneObjects(occupancyGridSceneElements);
+
+ // draw
+ auto layer = arviz.layer("occupancy_grid");
+
+ for (const auto& sceneObject : occupancyGridSceneElements->getSceneObjects())
+ {
+ const Eigen::Isometry3f world_T_obj(sceneObject->getGlobalPose());
+ ARMARX_INFO << world_T_obj.translation();
+ ARMARX_INFO << layer.size();
+ layer.add(viz::Box("box_" + std::to_string(layer.size()))
+ .pose(world_T_obj)
+ .size(result.occupancyGrid->resolution)
+ .color(viz::Color::orange()));
+ }
+
+ ARMARX_INFO << "Creating costmap";
+
+ algorithm::astar::NavigationCostMap costmap(
+ getRobot(),
+ scene.objects,
+ algorithm::astar::NavigationCostMap::Parameters{.binaryGrid = false,
+ .cellSize = 100});
+
+ costmap.createCostmap();
+
+ ARMARX_INFO << "Done";
+
+ ARMARX_TRACE;
+ const auto grid = costmap.getGrid();
+
+ ARMARX_TRACE;
+ ARMARX_INFO << "Dumping.";
+ dumpToFile(grid);
+
+ arviz.commit({layer});
+ }
+ else
+ {
+ ARMARX_INFO << "Occupancy grid not available";
+ }
return scene;
}
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 7b5c904c0cb408d4aae336d670afcbd29e77de76..9acc01de474274f3071e61ea905aaad4665dddb8 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -39,13 +39,14 @@
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include "RobotAPI/libraries/armem_robot/types.h"
-#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/components/Navigator/RemoteGui.h>
@@ -190,6 +191,7 @@ namespace armarx::navigation::components
mem::client::events::Writer eventsWriter;
mem::client::stack_result::Writer resultsWriter;
mem::client::graph::Reader graphReader;
+ armem::vision::occupancy_grid::client::Reader occupancyGridReader;
// `robot_state` memory reader and writer
std::optional<armem::robot::RobotDescription> robotDescription;
@@ -203,5 +205,13 @@ namespace armarx::navigation::components
std::unordered_map<std::string, std::unique_ptr<server::MemoryPublisher>> memoryPublishers;
core::ChronoMonotonicTimeServer timeServer;
+
+
+ struct Parameters
+ {
+ float occupiedGridThreshold{0.55F};
+ };
+
+ Parameters params;
};
} // namespace armarx::navigation::components
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 80338c42a1f3d0387a3f5682a645423a66a30994..267f0e5905bad45ad5dc96a0e418c50f5f98e0c4 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -129,7 +129,7 @@ namespace armarx::navigation::glob_plan
}
catch (...)
{
- ARMARX_DEBUG << "Could not plan collision-free path from"
+ ARMARX_INFO << "Could not plan collision-free path from"
<< "(" << start.translation().x() << "," << start.translation().y() << ")"
<< " to "
<< "(" << goal.translation().x() << "," << goal.translation().y() << ")"
@@ -144,7 +144,7 @@ namespace armarx::navigation::glob_plan
if (plan.size() < 2) // failure
{
- ARMARX_DEBUG << "Could not plan collision-free path from"
+ ARMARX_INFO << "Could not plan collision-free path from"
<< "(" << start.translation().x() << "," << start.translation().y() << ")"
<< " to "
<< "(" << goal.translation().x() << "," << goal.translation().y() << ")";
diff --git a/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.xml b/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.xml
index 62d7c38c127a5b6eba804255958147b8417fba7a..ae9739041e6a13bf60a46a7adab93259ef21a56e 100644
--- a/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.xml
+++ b/source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.xml
@@ -2,7 +2,7 @@
<State version="1.2" name="NavigateToLocation" uuid="11145B85-CD8C-4B7C-AF2F-65FCB07F6568" width="800" height="600" type="Normal State">
<InputParameters>
<Parameter name="location" type="::armarx::StringVariantData" docType="string" optional="yes">
- <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"R003/home"}}' docValue="R003/home"/>
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"audimax/handover"}}' docValue="audimax/handover"/>
</Parameter>
</InputParameters>
<OutputParameters/>
diff --git a/source/armarx/navigation/util/CMakeLists.txt b/source/armarx/navigation/util/CMakeLists.txt
index 84e75c4319651b025dc15c24b367f7ae57254b71..03f992bd0ced49f660b0fef641dda4bdf947449d 100644
--- a/source/armarx/navigation/util/CMakeLists.txt
+++ b/source/armarx/navigation/util/CMakeLists.txt
@@ -5,8 +5,11 @@ armarx_add_library(util
# RobotAPI
RobotAPIInterfaces
RobotAPIArmarXObjects
+ armem_vision
# Simox
Simox::VirtualRobot
- SOURCES ./util.cpp
- HEADERS ./util.h
+ SOURCES
+ ./util.cpp
+ HEADERS
+ ./util.h
)
diff --git a/source/armarx/navigation/util/util.cpp b/source/armarx/navigation/util/util.cpp
index 4496f200fce10d667f9032122242e24bf2938e7f..ba409ab3b12782d07b3bc5113b026a681c69b204 100644
--- a/source/armarx/navigation/util/util.cpp
+++ b/source/armarx/navigation/util/util.cpp
@@ -23,10 +23,23 @@
#include "util.h"
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/Primitive.h>
#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/VisualizationNode.h>
+#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>
namespace armarx::navigation::util
{
@@ -48,4 +61,56 @@ namespace armarx::navigation::util
return sceneObjects;
}
+
+ VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const armem::OccupancyGrid& occupancyGrid, const OccupancyGridHelper::Params& params)
+ {
+ const OccupancyGridHelper ocHelper(occupancyGrid, params);
+ const auto obstacles = ocHelper.obstacles();
+
+ const float boxSize = occupancyGrid.resolution;
+ const float resolution = occupancyGrid.resolution;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+
+ ARMARX_CHECK_EQUAL(occupancyGrid.frame, GlobalFrame)
+ << "Only occupancy grid in global frame supported.";
+
+ VirtualRobot::CoinVisualizationFactory factory;
+
+ const auto& world_T_map = occupancyGrid.pose;
+
+ for (int x = 0; x < obstacles.rows(); x++)
+ {
+ for (int y = 0; y < obstacles.cols(); y++)
+ {
+ if (obstacles(x, y))
+ {
+ const Eigen::Vector3f pos{
+ static_cast<float>(x * resolution), static_cast<float>(y * resolution), 0};
+
+ // FIXME: change to Isometry3f
+ Eigen::Affine3f map_T_obj = Eigen::Affine3f::Identity();
+ map_T_obj.translation() = pos;
+
+ Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
+
+ // ARMARX_INFO << world_T_obj.translation();
+
+ auto cube = factory.createBox(boxSize, boxSize, boxSize);
+
+ VirtualRobot::CollisionModelPtr collisionModel(
+ new VirtualRobot::CollisionModel(cube));
+
+ VirtualRobot::SceneObjectPtr sceneObject(new VirtualRobot::SceneObject(
+ "box_" + std::to_string(sceneObjects->getSize()), cube, collisionModel));
+ sceneObject->setGlobalPose(world_T_obj.matrix());
+
+ sceneObjects->addSceneObject(sceneObject);
+ }
+ }
+ }
+
+ return sceneObjects;
+ }
} // namespace armarx::navigation::util
diff --git a/source/armarx/navigation/util/util.h b/source/armarx/navigation/util/util.h
index 5941d8dc77bbbc19607848bf6631e1a36c476209..c15487f1ae194847c8a53e74fa52f389ba786dcd 100644
--- a/source/armarx/navigation/util/util.h
+++ b/source/armarx/navigation/util/util.h
@@ -25,10 +25,13 @@
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
namespace armarx::navigation::util
{
VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+ VirtualRobot::SceneObjectSetPtr asSceneObjects(const armem::OccupancyGrid& occupancyGrid, const OccupancyGridHelper::Params& params);
} // namespace armarx::navigation::util