From 7bfcf8b53113555b362c711327f3a90d3a718daa Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sun, 1 May 2022 13:46:27 +0200
Subject: [PATCH 1/9] new component
dynamic_distance_to_obstacle_costmap_provider
---
.../navigation/components/CMakeLists.txt | 2 +
.../CMakeLists.txt | 28 +++
.../Component.cpp | 220 ++++++++++++++++++
.../Component.h | 144 ++++++++++++
.../ComponentInterface.ice | 35 +++
5 files changed, 429 insertions(+)
create mode 100644 source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
create mode 100644 source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
create mode 100644 source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
create mode 100644 source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice
diff --git a/source/armarx/navigation/components/CMakeLists.txt b/source/armarx/navigation/components/CMakeLists.txt
index dde47f4f..34e29e91 100644
--- a/source/armarx/navigation/components/CMakeLists.txt
+++ b/source/armarx/navigation/components/CMakeLists.txt
@@ -4,3 +4,5 @@ add_subdirectory(Navigator)
# Examples
add_subdirectory(example_client)
+
+add_subdirectory(dynamic_distance_to_obstacle_costmap_provider)
\ No newline at end of file
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
new file mode 100644
index 00000000..8a1ce9c9
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
@@ -0,0 +1,28 @@
+armarx_add_component(dynamic_distance_to_obstacle_costmap_provider
+ ICE_FILES
+ ComponentInterface.ice
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ # RobotAPIInterfaces
+ # ARON_FILES
+ # aron/my_type.xml
+ SOURCES
+ Component.cpp
+ HEADERS
+ Component.h
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCore
+ ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ # DEPENDENCIES_LEGACY
+ ## Add libraries that do not provide any targets but ${FOO_*} variables.
+ # FOO
+ # If you need a separate shared component library you can enable it with the following flag.
+ # SHARED_COMPONENT_LIBRARY
+)
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
new file mode 100644
index 00000000..af7d9ff7
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -0,0 +1,220 @@
+/**
+ * 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/>.
+ *
+ * @package navigation::ArmarXObjects::dynamic_distance_to_obstacle_costmap_provider
+ * @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 "Component.h"
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+// Include headers you only need in function definitions in the .cpp.
+
+// #include <Eigen/Core>
+
+// #include <SimoxUtility/color/Color.h>
+
+
+namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
+{
+
+ const std::string
+ Component::defaultName = "dynamic_distance_to_obstacle_costmap_provider";
+
+
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
+
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+
+ // Add a required property. (The component won't start without a value being set.)
+ // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+ // Add an optionalproperty.
+ def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+ def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+
+ return def;
+ }
+
+
+ void
+ Component::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+
+ void
+ Component::onConnectComponent()
+ {
+ // Do things after connecting to topics and components.
+
+ /* (Requies the armarx::DebugObserverComponentPluginUser.)
+ // Use the debug observer to log data over time.
+ // The data can be viewed in the ObserverView and the LivePlotter.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ */
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ // Draw boxes in ArViz.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ drawBoxes(properties, arviz);
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ */
+ }
+
+
+ void
+ Component::onDisconnectComponent()
+ {
+ }
+
+
+ void
+ Component::onExitComponent()
+ {
+ }
+
+
+ std::string
+ Component::getDefaultName() const
+ {
+ return Component::defaultName;
+ }
+
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return Component::defaultName;
+ }
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ void
+ Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ // Setup the widgets.
+
+ tab.boxLayerName.setValue(properties.boxLayerName);
+
+ tab.numBoxes.setValue(properties.numBoxes);
+ tab.numBoxes.setRange(0, 100);
+
+ tab.drawBoxes.setLabel("Draw Boxes");
+
+ // Setup the layout.
+
+ GridLayout grid;
+ int row = 0;
+ {
+ grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+ ++row;
+
+ grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
+
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
+ }
+
+ VBoxLayout root = {grid, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void
+ Component::RemoteGui_update()
+ {
+ if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ {
+ std::scoped_lock lock(propertiesMutex);
+ properties.boxLayerName = tab.boxLayerName.getValue();
+ properties.numBoxes = tab.numBoxes.getValue();
+
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ }
+ if (tab.drawBoxes.wasClicked())
+ {
+ // Lock shared variables in methods running in seperate threads
+ // and pass them to functions. This way, the called functions do
+ // not need to think about locking.
+ std::scoped_lock lock(propertiesMutex, arvizMutex);
+ drawBoxes(properties, arviz);
+ }
+ }
+ */
+
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ void
+ Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
+ {
+ // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
+ // See the ArVizExample in RobotAPI for more examples.
+
+ viz::Layer layer = arviz.layer(p.boxLayerName);
+ for (int i = 0; i < p.numBoxes; ++i)
+ {
+ layer.add(viz::Box("box_" + std::to_string(i))
+ .position(Eigen::Vector3f(i * 100, 0, 0))
+ .size(20).color(simox::Color::blue()));
+ }
+ arviz.commit(layer);
+ }
+ */
+
+
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
+
+} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
new file mode 100644
index 00000000..ca919c1f
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -0,0 +1,144 @@
+/**
+ * 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/>.
+ *
+ * @package navigation::ArmarXObjects::dynamic_distance_to_obstacle_costmap_provider
+ * @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 <mutex>
+
+#include <ArmarXCore/core/Component.h>
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
+{
+
+ class Component :
+ virtual public armarx::Component,
+ virtual public armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider::ComponentInterface
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ // , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ /// Get the component's default name.
+ static std::string GetDefaultName();
+
+
+ protected:
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+
+ /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// This function should be called once in onConnect() or when you
+ /// need to re-create the Remote GUI tab.
+ void createRemoteGuiTab();
+
+ /// After calling `RemoteGui_startRunningTask`, this function is
+ /// called periodically in a separate thread. If you update variables,
+ /// make sure to synchronize access to them.
+ void RemoteGui_update() override;
+ */
+
+
+ private:
+
+ // Private methods go here.
+
+ // Forward declare `Properties` if you used it before its defined.
+ // struct Properties;
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ /// Draw some boxes in ArViz.
+ void drawBoxes(const Properties& p, viz::Client& arviz);
+ */
+
+
+ private:
+
+ static const std::string defaultName;
+
+
+ // Private member variables go here.
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ std::string boxLayerName = "boxes";
+ int numBoxes = 10;
+ };
+ Properties properties;
+ /* Use a mutex if you access variables from different threads
+ * (e.g. ice functions and RemoteGui_update()).
+ std::mutex propertiesMutex;
+ */
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ * When used from different threads, an ArViz client needs to be synchronized.
+ /// Protects the arviz client inherited from the ArViz plugin.
+ std::mutex arvizMutex;
+ */
+
+ };
+
+} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice
new file mode 100644
index 00000000..d4980fef
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice
@@ -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/>.
+ *
+ * package navigation::dynamic_distance_to_obstacle_costmap_provider
+ * 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
+
+
+module armarx { module navigation { module components { module dynamic_distance_to_obstacle_costmap_provider
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
--
GitLab
From d54740e2724c92a2aa621de4294af52b41c347c6 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sun, 1 May 2022 14:01:17 +0200
Subject: [PATCH 2/9] renaming namespace "mem" to "memory"
---
.../navigation/components/Navigator/Navigator.h | 12 ++++++------
.../navigation/memory/client/costmap/Reader.cpp | 4 ++--
.../armarx/navigation/memory/client/costmap/Reader.h | 4 ++--
.../navigation/memory/client/costmap/Writer.cpp | 4 ++--
.../armarx/navigation/memory/client/costmap/Writer.h | 4 ++--
.../navigation/memory/client/events/Writer.cpp | 4 ++--
.../armarx/navigation/memory/client/events/Writer.h | 4 ++--
.../armarx/navigation/memory/client/graph/Reader.cpp | 4 ++--
.../armarx/navigation/memory/client/graph/Reader.h | 4 ++--
.../memory/client/parameterization/Reader.cpp | 4 ++--
.../memory/client/parameterization/Reader.h | 4 ++--
.../memory/client/parameterization/Writer.cpp | 4 ++--
.../memory/client/parameterization/Writer.h | 4 ++--
.../navigation/memory/client/stack_result/Writer.cpp | 4 ++--
.../navigation/memory/client/stack_result/Writer.h | 4 ++--
.../server/event_publishing/MemoryPublisher.cpp | 4 ++--
.../server/event_publishing/MemoryPublisher.h | 8 ++++----
.../server/introspection/MemoryIntrospector.cpp | 2 +-
.../server/introspection/MemoryIntrospector.h | 4 ++--
.../MemoryParameterizationService.cpp | 4 ++--
.../parameterization/MemoryParameterizationService.h | 8 ++++----
21 files changed, 49 insertions(+), 49 deletions(-)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 326418c3..817bd115 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -188,13 +188,13 @@ namespace armarx::navigation::components
std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
// `navigation` memory reader and writer
- mem::client::param::Reader parameterizationReader;
- mem::client::param::Writer parameterizationWriter;
- mem::client::events::Writer eventsWriter;
- mem::client::stack_result::Writer resultsWriter;
- mem::client::graph::Reader graphReader;
+ memory::client::param::Reader parameterizationReader;
+ memory::client::param::Writer parameterizationWriter;
+ memory::client::events::Writer eventsWriter;
+ memory::client::stack_result::Writer resultsWriter;
+ memory::client::graph::Reader graphReader;
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
- mem::client::costmap::Writer costmapWriter;
+ memory::client::costmap::Writer costmapWriter;
// `robot_state` memory reader and writer
std::optional<armem::robot::RobotDescription> robotDescription;
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.cpp b/source/armarx/navigation/memory/client/costmap/Reader.cpp
index c3567bfa..93ed8841 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Reader.cpp
@@ -20,7 +20,7 @@
#include "armarx/navigation/algorithms/aron_conversions.h"
#include "armarx/navigation/memory/constants.h"
-namespace armarx::navigation::mem::client::costmap
+namespace armarx::navigation::memory::client::costmap
{
Reader::~Reader() = default;
@@ -127,4 +127,4 @@ namespace armarx::navigation::mem::client::costmap
}
}
-} // namespace armarx::navigation::mem::client::costmap
+} // namespace armarx::navigation::memory::client::costmap
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.h b/source/armarx/navigation/memory/client/costmap/Reader.h
index e8875b73..24b02ada 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.h
+++ b/source/armarx/navigation/memory/client/costmap/Reader.h
@@ -28,7 +28,7 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include "armarx/navigation/algorithms/Costmap.h"
-namespace armarx::navigation::mem::client::costmap
+namespace armarx::navigation::memory::client::costmap
{
class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
@@ -72,4 +72,4 @@ namespace armarx::navigation::mem::client::costmap
Properties defaultProperties() const override;
};
-} // namespace armarx::navigation::mem::client::costmap
+} // namespace armarx::navigation::memory::client::costmap
diff --git a/source/armarx/navigation/memory/client/costmap/Writer.cpp b/source/armarx/navigation/memory/client/costmap/Writer.cpp
index 141d9c0c..aad3598b 100644
--- a/source/armarx/navigation/memory/client/costmap/Writer.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Writer.cpp
@@ -5,7 +5,7 @@
#include "armarx/navigation/memory/constants.h"
-namespace armarx::navigation::mem::client::costmap
+namespace armarx::navigation::memory::client::costmap
{
Writer::~Writer() = default;
@@ -66,4 +66,4 @@ namespace armarx::navigation::mem::client::costmap
memory::constants::CostmapCoreSegmentName};
}
-} // namespace armarx::navigation::mem::client::costmap
+} // namespace armarx::navigation::memory::client::costmap
diff --git a/source/armarx/navigation/memory/client/costmap/Writer.h b/source/armarx/navigation/memory/client/costmap/Writer.h
index f75f40ea..0e44e30d 100644
--- a/source/armarx/navigation/memory/client/costmap/Writer.h
+++ b/source/armarx/navigation/memory/client/costmap/Writer.h
@@ -28,7 +28,7 @@
#include <RobotAPI/libraries/armem_vision/types.h>
#include "armarx/navigation/algorithms/Costmap.h"
-namespace armarx::navigation::mem::client::costmap
+namespace armarx::navigation::memory::client::costmap
{
/**
@@ -60,4 +60,4 @@ namespace armarx::navigation::mem::client::costmap
-} // namespace armarx::navigation::mem::client::costmap
+} // namespace armarx::navigation::memory::client::costmap
diff --git a/source/armarx/navigation/memory/client/events/Writer.cpp b/source/armarx/navigation/memory/client/events/Writer.cpp
index 6ff75455..b15a93ca 100644
--- a/source/armarx/navigation/memory/client/events/Writer.cpp
+++ b/source/armarx/navigation/memory/client/events/Writer.cpp
@@ -11,7 +11,7 @@
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/core/events.h>
-namespace armarx::navigation::mem::client::events
+namespace armarx::navigation::memory::client::events
{
std::string
@@ -123,4 +123,4 @@ namespace armarx::navigation::mem::client::events
return store(update);
}
-} // namespace armarx::navigation::mem::client::events
+} // namespace armarx::navigation::memory::client::events
diff --git a/source/armarx/navigation/memory/client/events/Writer.h b/source/armarx/navigation/memory/client/events/Writer.h
index d1ac26e0..833f3705 100644
--- a/source/armarx/navigation/memory/client/events/Writer.h
+++ b/source/armarx/navigation/memory/client/events/Writer.h
@@ -29,7 +29,7 @@
#include <armarx/navigation/core/types.h>
-namespace armarx::navigation::mem::client::events
+namespace armarx::navigation::memory::client::events
{
class Writer : virtual public armem::client::util::SimpleWriterBase
@@ -62,4 +62,4 @@ namespace armarx::navigation::mem::client::events
bool
storeImpl(const EventT& event, const std::string& eventName, const std::string& clientID);
};
-} // namespace armarx::navigation::mem::client::events
+} // namespace armarx::navigation::memory::client::events
diff --git a/source/armarx/navigation/memory/client/graph/Reader.cpp b/source/armarx/navigation/memory/client/graph/Reader.cpp
index 98049272..cb986d2d 100644
--- a/source/armarx/navigation/memory/client/graph/Reader.cpp
+++ b/source/armarx/navigation/memory/client/graph/Reader.cpp
@@ -18,7 +18,7 @@
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
-namespace armarx::navigation::mem::client::graph
+namespace armarx::navigation::memory::client::graph
{
std::string
Reader::propertyPrefix() const
@@ -216,4 +216,4 @@ namespace armarx::navigation::mem::client::graph
}
}
-} // namespace armarx::navigation::mem::client::graph
+} // namespace armarx::navigation::memory::client::graph
diff --git a/source/armarx/navigation/memory/client/graph/Reader.h b/source/armarx/navigation/memory/client/graph/Reader.h
index a61bc7c6..702a4d1a 100644
--- a/source/armarx/navigation/memory/client/graph/Reader.h
+++ b/source/armarx/navigation/memory/client/graph/Reader.h
@@ -27,7 +27,7 @@
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/core/types.h>
-namespace armarx::navigation::mem::client::graph
+namespace armarx::navigation::memory::client::graph
{
class Reader : virtual public armem::client::util::SimpleReaderBase
@@ -58,4 +58,4 @@ namespace armarx::navigation::mem::client::graph
armem::client::Reader memoryReaderGraphs;
};
-} // namespace armarx::navigation::mem::client::graph
+} // namespace armarx::navigation::memory::client::graph
diff --git a/source/armarx/navigation/memory/client/parameterization/Reader.cpp b/source/armarx/navigation/memory/client/parameterization/Reader.cpp
index 0d155874..311a5f4b 100644
--- a/source/armarx/navigation/memory/client/parameterization/Reader.cpp
+++ b/source/armarx/navigation/memory/client/parameterization/Reader.cpp
@@ -2,7 +2,7 @@
#include "armarx/navigation/memory/constants.h"
-namespace armarx::navigation::mem::client::param
+namespace armarx::navigation::memory::client::param
{
std::string
Reader::propertyPrefix() const
@@ -17,4 +17,4 @@ namespace armarx::navigation::mem::client::param
.coreSegmentName = memory::constants::ParameterizationCoreSegmentName};
}
-} // namespace armarx::navigation::mem::client::param
+} // namespace armarx::navigation::memory::client::param
diff --git a/source/armarx/navigation/memory/client/parameterization/Reader.h b/source/armarx/navigation/memory/client/parameterization/Reader.h
index 9c6d871b..f92fd12e 100644
--- a/source/armarx/navigation/memory/client/parameterization/Reader.h
+++ b/source/armarx/navigation/memory/client/parameterization/Reader.h
@@ -23,7 +23,7 @@
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
-namespace armarx::navigation::mem::client::param
+namespace armarx::navigation::memory::client::param
{
class Reader : virtual public armem::client::util::SimpleReaderBase
@@ -37,4 +37,4 @@ namespace armarx::navigation::mem::client::param
protected:
private:
};
-} // namespace armarx::navigation::mem::client::param
+} // namespace armarx::navigation::memory::client::param
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.cpp b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
index 33898b89..23a8d443 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.cpp
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
@@ -6,7 +6,7 @@
#include <armarx/navigation/memory/constants.h>
-namespace armarx::navigation::mem::client::param
+namespace armarx::navigation::memory::client::param
{
bool
Writer::store(const std::unordered_map<core::StackLayer, aron::data::DictPtr>& stack,
@@ -66,4 +66,4 @@ namespace armarx::navigation::mem::client::param
};
}
-} // namespace armarx::navigation::mem::client::param
+} // namespace armarx::navigation::memory::client::param
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.h b/source/armarx/navigation/memory/client/parameterization/Writer.h
index eda0e83e..58c6cd22 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.h
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.h
@@ -27,7 +27,7 @@
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/core/types.h>
-namespace armarx::navigation::mem::client::param
+namespace armarx::navigation::memory::client::param
{
class Writer : virtual public armem::client::util::SimpleWriterBase
@@ -47,4 +47,4 @@ namespace armarx::navigation::mem::client::param
private:
};
-} // namespace armarx::navigation::mem::client::param
+} // namespace armarx::navigation::memory::client::param
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.cpp b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
index 5e5f1adc..759b42f6 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.cpp
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.cpp
@@ -8,7 +8,7 @@
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
#include <armarx/navigation/core/aron_conversions.h>
-namespace armarx::navigation::mem::client::stack_result
+namespace armarx::navigation::memory::client::stack_result
{
bool
Writer::store(const server::StackResult& result, const std::string& clientID)
@@ -129,4 +129,4 @@ namespace armarx::navigation::mem::client::stack_result
};
}
-} // namespace armarx::navigation::mem::client::stack_result
+} // namespace armarx::navigation::memory::client::stack_result
diff --git a/source/armarx/navigation/memory/client/stack_result/Writer.h b/source/armarx/navigation/memory/client/stack_result/Writer.h
index 5d7dac46..1ece5594 100644
--- a/source/armarx/navigation/memory/client/stack_result/Writer.h
+++ b/source/armarx/navigation/memory/client/stack_result/Writer.h
@@ -28,7 +28,7 @@
#include <armarx/navigation/server/StackResult.h>
#include <armarx/navigation/trajectory_control/TrajectoryController.h>
-namespace armarx::navigation::mem::client::stack_result
+namespace armarx::navigation::memory::client::stack_result
{
class Writer : virtual public armem::client::util::SimpleWriterBase
@@ -67,4 +67,4 @@ namespace armarx::navigation::mem::client::stack_result
private:
};
-} // namespace armarx::navigation::mem::client::stack_result
+} // namespace armarx::navigation::memory::client::stack_result
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
index 0f3cc133..60adb3a6 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp
@@ -77,8 +77,8 @@ namespace armarx::navigation::server
}
MemoryPublisher::MemoryPublisher(
- armarx::navigation::mem::client::stack_result::Writer* resultWriter,
- armarx::navigation::mem::client::events::Writer* eventsWriter,
+ armarx::navigation::memory::client::stack_result::Writer* resultWriter,
+ armarx::navigation::memory::client::events::Writer* eventsWriter,
const std::string& clientId) :
resultWriter(resultWriter), eventsWriter(eventsWriter), clientId(clientId)
{
diff --git a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
index 1b130a83..57204aba 100644
--- a/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
+++ b/source/armarx/navigation/server/event_publishing/MemoryPublisher.h
@@ -12,8 +12,8 @@ namespace armarx::navigation::server
{
public:
- MemoryPublisher(armarx::navigation::mem::client::stack_result::Writer* resultWriter,
- armarx::navigation::mem::client::events::Writer* eventsWriter,
+ MemoryPublisher(armarx::navigation::memory::client::stack_result::Writer* resultWriter,
+ armarx::navigation::memory::client::events::Writer* eventsWriter,
const std::string& clientId);
void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& res) override;
@@ -40,8 +40,8 @@ namespace armarx::navigation::server
~MemoryPublisher() override = default;
private:
- armarx::navigation::mem::client::stack_result::Writer* resultWriter;
- armarx::navigation::mem::client::events::Writer* eventsWriter;
+ armarx::navigation::memory::client::stack_result::Writer* resultWriter;
+ armarx::navigation::memory::client::events::Writer* eventsWriter;
const std::string clientId;
};
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
index bb0e4d0b..226a0c80 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
@@ -4,7 +4,7 @@
namespace armarx::navigation::server
{
- MemoryIntrospector::MemoryIntrospector(mem::client::stack_result::Writer& globPlanWriter,
+ MemoryIntrospector::MemoryIntrospector(memory::client::stack_result::Writer& globPlanWriter,
const std::string& clientID) :
globPlanWriter(globPlanWriter), clientID(clientID)
{
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index 18ecf769..a141d4ba 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -33,7 +33,7 @@ namespace armarx::navigation::server
class MemoryIntrospector : virtual public IntrospectorInterface
{
public:
- MemoryIntrospector(mem::client::stack_result::Writer& globPlanWriter,
+ MemoryIntrospector(memory::client::stack_result::Writer& globPlanWriter,
const std::string& clientID);
~MemoryIntrospector() override = default;
@@ -62,7 +62,7 @@ namespace armarx::navigation::server
protected:
private:
- mem::client::stack_result::Writer& globPlanWriter;
+ memory::client::stack_result::Writer& globPlanWriter;
const std::string clientID;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
index 676dccef..b4334831 100644
--- a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
+++ b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.cpp
@@ -70,8 +70,8 @@ namespace armarx::navigation::server
}
MemoryParameterizationService::MemoryParameterizationService(
- mem::client::param::Reader* reader,
- mem::client::param::Writer* writer) :
+ memory::client::param::Reader* reader,
+ memory::client::param::Writer* writer) :
reader(reader), writer(writer)
{
}
diff --git a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.h b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.h
index 478b456a..0eb1e0b7 100644
--- a/source/armarx/navigation/server/parameterization/MemoryParameterizationService.h
+++ b/source/armarx/navigation/server/parameterization/MemoryParameterizationService.h
@@ -36,8 +36,8 @@ namespace armarx::navigation::server
class MemoryParameterizationService
{
public:
- MemoryParameterizationService(mem::client::param::Reader* reader,
- mem::client::param::Writer* writer);
+ MemoryParameterizationService(memory::client::param::Reader* reader,
+ memory::client::param::Writer* writer);
bool store(const aron::data::DictPtr& params,
const std::string& clientId,
@@ -50,7 +50,7 @@ namespace armarx::navigation::server
protected:
private:
- mem::client::param::Reader* reader;
- mem::client::param::Writer* writer;
+ memory::client::param::Reader* reader;
+ memory::client::param::Writer* writer;
};
} // namespace armarx::navigation::server
--
GitLab
From 386f3ba0e5d5649ca00ff76eddc0c97e930aaa12 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sun, 1 May 2022 14:02:06 +0200
Subject: [PATCH 3/9] component: memory reader and writer
---
.../CMakeLists.txt | 3 ++-
.../Component.cpp | 15 ++++++++++-----
.../Component.h | 18 +++++++++++++-----
.../ComponentInterface.ice | 3 ++-
4 files changed, 27 insertions(+), 12 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
index 8a1ce9c9..d28fc2ee 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
@@ -3,7 +3,7 @@ armarx_add_component(dynamic_distance_to_obstacle_costmap_provider
ComponentInterface.ice
ICE_DEPENDENCIES
ArmarXCoreInterfaces
- # RobotAPIInterfaces
+ RobotAPIInterfaces
# ARON_FILES
# aron/my_type.xml
SOURCES
@@ -20,6 +20,7 @@ armarx_add_component(dynamic_distance_to_obstacle_costmap_provider
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
+ armarx_navigation::memory
# DEPENDENCIES_LEGACY
## Add libraries that do not provide any targets but ${FOO_*} variables.
# FOO
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index af7d9ff7..dbd14c6d 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -35,14 +35,14 @@
namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
{
- const std::string
- Component::defaultName = "dynamic_distance_to_obstacle_costmap_provider";
+ const std::string Component::defaultName = "dynamic_distance_to_obstacle_costmap_provider";
armarx::PropertyDefinitionsPtr
Component::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr def =
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
@@ -58,13 +58,18 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
// Add an optionalproperty.
- def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+ def->optional(
+ properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
return def;
}
+ Component::Component() : costmapReader(memoryNameSystem()), costmapWriter(memoryNameSystem())
+ {
+ }
+
void
Component::onInitComponent()
{
@@ -217,4 +222,4 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
-} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
+} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
index ca919c1f..3c95d37d 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -28,12 +28,17 @@
#include <ArmarXCore/core/Component.h>
+#include "RobotAPI/libraries/armem/client/forward_declarations.h"
+
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/armem/client.h>
+#include "armarx/navigation/memory/client/costmap/Reader.h"
+#include "armarx/navigation/memory/client/costmap/Writer.h"
#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
@@ -42,12 +47,16 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
class Component :
virtual public armarx::Component,
- virtual public armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider::ComponentInterface
+ virtual public armarx::navigation::components::
+ dynamic_distance_to_obstacle_costmap_provider::ComponentInterface
// , virtual public armarx::DebugObserverComponentPluginUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
// , virtual public armarx::ArVizComponentPluginUser
+ ,
+ virtual public armem::ListeningClientPluginUser
{
public:
+ Component();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -57,7 +66,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -87,7 +95,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
private:
-
// Private methods go here.
// Forward declare `Properties` if you used it before its defined.
@@ -100,7 +107,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
private:
-
static const std::string defaultName;
@@ -139,6 +145,8 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
std::mutex arvizMutex;
*/
+ memory::client::costmap::Reader costmapReader;
+ memory::client::costmap::Writer costmapWriter;
};
-} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
+} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice
index d4980fef..3116466b 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice
@@ -23,11 +23,12 @@
#pragma once
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
module armarx { module navigation { module components { module dynamic_distance_to_obstacle_costmap_provider
{
- interface ComponentInterface
+ interface ComponentInterface extends armarx::armem::client::MemoryListenerInterface
{
// Define your interface here.
};
--
GitLab
From 7daeb8bb1b880adf70e3f4880d4a053e824775d8 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 2 May 2022 11:11:55 +0200
Subject: [PATCH 4/9] changes in armem::vision
---
source/armarx/navigation/util/util.cpp | 2 +-
source/armarx/navigation/util/util.h | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/util/util.cpp b/source/armarx/navigation/util/util.cpp
index 42f141f9..89679dd6 100644
--- a/source/armarx/navigation/util/util.cpp
+++ b/source/armarx/navigation/util/util.cpp
@@ -112,7 +112,7 @@ namespace armarx::navigation::util
}
VirtualRobot::SceneObjectSetPtr
- asSceneObjects(const armem::OccupancyGrid& occupancyGrid,
+ asSceneObjects(const armem::vision::OccupancyGrid& occupancyGrid,
const OccupancyGridHelper::Params& params)
{
const OccupancyGridHelper ocHelper(occupancyGrid, params);
diff --git a/source/armarx/navigation/util/util.h b/source/armarx/navigation/util/util.h
index 7f2aa207..bded81af 100644
--- a/source/armarx/navigation/util/util.h
+++ b/source/armarx/navigation/util/util.h
@@ -38,7 +38,7 @@ namespace armarx::navigation::util
VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose);
VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
- VirtualRobot::SceneObjectSetPtr asSceneObjects(const armem::OccupancyGrid& occupancyGrid,
+ VirtualRobot::SceneObjectSetPtr asSceneObjects(const armem::vision::OccupancyGrid& occupancyGrid,
const OccupancyGridHelper::Params& params);
} // namespace armarx::navigation::util
--
GitLab
From 676d988666406fe23c0142e2af9b83e79a4541e8 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 2 May 2022 11:12:17 +0200
Subject: [PATCH 5/9] dynamic_distance_to_obstacle_costmap_provider: draft
---
.../Component.cpp | 45 +++++++++++++++++--
.../Component.h | 16 ++++++-
2 files changed, 56 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index dbd14c6d..bbece92a 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -23,8 +23,12 @@
#include "Component.h"
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include "armarx/navigation/memory/client/costmap/Reader.h"
+
// Include headers you only need in function definitions in the .cpp.
// #include <Eigen/Core>
@@ -58,9 +62,9 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
// Add an optionalproperty.
- def->optional(
- properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
- def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+ // def->optional(
+ // properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+ def->optional(properties.updatePeriodMs, "p.updatePeriodMs", "");
return def;
}
@@ -110,6 +114,30 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
RemoteGui_startRunningTask();
}
*/
+
+ ARMARX_CHECK(readStaticCostmap());
+
+ updateCostmapTask =
+ new PeriodicTask<Component>(this, &Component::updateCostmap, properties.updatePeriodMs);
+ }
+
+ bool
+ Component::readStaticCostmap()
+ {
+ const memory::client::costmap::Reader::Query query{
+ .providerName = properties.staticCostmapProviderName,
+ .name = properties.staticCostmapName,
+ .timestamp = armarx::core::time::Clock::Now()};
+ const auto result = costmapReader.query(query);
+
+ if (result.costmap.has_value())
+ {
+ staticCostmap.emplace(result.costmap.value());
+ return true;
+ }
+
+ ARMARX_WARNING << "Could not read static costmap";
+ return false;
}
@@ -139,6 +167,17 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
}
+ void
+ Component::updateCostmap()
+ {
+
+ ARMARX_CHECK(staticCostmap.has_value());
+
+
+
+ }
+
+
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void
Component::createRemoteGuiTab()
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
index 3c95d37d..713a0070 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -26,6 +26,7 @@
// #include <mutex>
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <ArmarXCore/core/Component.h>
#include "RobotAPI/libraries/armem/client/forward_declarations.h"
@@ -82,6 +83,11 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
void onExitComponent() override;
+ void updateCostmap();
+
+ bool readStaticCostmap();
+
+
/* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
/// This function should be called once in onConnect() or when you
/// need to re-create the Remote GUI tab.
@@ -116,8 +122,10 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
/// Properties shown in the Scenario GUI.
struct Properties
{
- std::string boxLayerName = "boxes";
- int numBoxes = 10;
+ // std::string boxLayerName = "boxes";
+ std::string staticCostmapProviderName = "navigator";
+ std::string staticCostmapName = "distance_to_obstacles";
+ int updatePeriodMs = 100;
};
Properties properties;
/* Use a mutex if you access variables from different threads
@@ -147,6 +155,10 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
memory::client::costmap::Reader costmapReader;
memory::client::costmap::Writer costmapWriter;
+
+ std::optional<algorithms::Costmap> staticCostmap;
+
+ PeriodicTask<Component>::pointer_type updateCostmapTask;
};
} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
--
GitLab
From 2e9039163f9e62eef37508b339cfb6d50e6c59e9 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 2 May 2022 11:42:19 +0200
Subject: [PATCH 6/9] updateCostmap: draft
---
.../CMakeLists.txt | 1 +
.../Component.cpp | 137 ++++++------------
.../Component.h | 17 ++-
3 files changed, 61 insertions(+), 94 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
index d28fc2ee..9c14667f 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
@@ -17,6 +17,7 @@ armarx_add_component(dynamic_distance_to_obstacle_costmap_provider
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
+ armem_vision
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index bbece92a..ef567199 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -26,6 +26,7 @@
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
#include "armarx/navigation/memory/client/costmap/Reader.h"
@@ -66,11 +67,18 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
def->optional(properties.updatePeriodMs, "p.updatePeriodMs", "");
+ costmapReader.registerPropertyDefinitions(def);
+ costmapWriter.registerPropertyDefinitions(def);
+ laserScannerFeaturesReader.registerPropertyDefinitions(def);
+
return def;
}
- Component::Component() : costmapReader(memoryNameSystem()), costmapWriter(memoryNameSystem())
+ Component::Component() :
+ costmapReader(memoryNameSystem()),
+ costmapWriter(memoryNameSystem()),
+ laserScannerFeaturesReader(memoryNameSystem())
{
}
@@ -115,6 +123,11 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
}
*/
+ // connect memory readers and writers
+ costmapReader.connect();
+ costmapWriter.connect();
+ laserScannerFeaturesReader.connect();
+
ARMARX_CHECK(readStaticCostmap());
updateCostmapTask =
@@ -125,8 +138,8 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
Component::readStaticCostmap()
{
const memory::client::costmap::Reader::Query query{
- .providerName = properties.staticCostmapProviderName,
- .name = properties.staticCostmapName,
+ .providerName = properties.staticCostmap.providerName,
+ .name = properties.staticCostmap.name,
.timestamp = armarx::core::time::Clock::Now()};
const auto result = costmapReader.query(query);
@@ -142,121 +155,61 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
void
- Component::onDisconnectComponent()
+ Component::updateCostmap()
{
- }
-
- void
- Component::onExitComponent()
- {
- }
+ ARMARX_CHECK(staticCostmap.has_value());
+ const armem::vision::laser_scanner_features::client::Reader::Query query
+ {
+ .providerName = properties.laserScannerFeatures.providerName,
+ .name = properties.laserScannerFeatures.name,
+ .timestamp = armarx::core::time::Clock::Now()
+ };
- std::string
- Component::getDefaultName() const
- {
- return Component::defaultName;
- }
+ const auto result = laserScannerFeaturesReader.queryData(query);
+ if(result.status != result.Success)
+ {
+ ARMARX_WARNING << "Failed to retrieve data from memory";
+ return;
+ }
- std::string
- Component::GetDefaultName()
- {
- return Component::defaultName;
+ for(const auto& e : result.features)
+ {
+ ARMARX_INFO << "Number of laser scanner clusters for sensor "<< e.frame << ": " << e.features.size();
+ }
}
- void
- Component::updateCostmap()
- {
- ARMARX_CHECK(staticCostmap.has_value());
-
+ void
+ Component::onDisconnectComponent()
+ {
}
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void
- Component::createRemoteGuiTab()
+ Component::onExitComponent()
{
- using namespace armarx::RemoteGui::Client;
-
- // Setup the widgets.
-
- tab.boxLayerName.setValue(properties.boxLayerName);
-
- tab.numBoxes.setValue(properties.numBoxes);
- tab.numBoxes.setRange(0, 100);
-
- tab.drawBoxes.setLabel("Draw Boxes");
-
- // Setup the layout.
-
- GridLayout grid;
- int row = 0;
- {
- grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
- ++row;
-
- grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
- ++row;
-
- grid.add(tab.drawBoxes, {row, 0}, {2, 1});
- ++row;
- }
-
- VBoxLayout root = {grid, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
}
- void
- Component::RemoteGui_update()
+ std::string
+ Component::getDefaultName() const
{
- if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
- {
- std::scoped_lock lock(propertiesMutex);
- properties.boxLayerName = tab.boxLayerName.getValue();
- properties.numBoxes = tab.numBoxes.getValue();
-
- {
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- }
- if (tab.drawBoxes.wasClicked())
- {
- // Lock shared variables in methods running in seperate threads
- // and pass them to functions. This way, the called functions do
- // not need to think about locking.
- std::scoped_lock lock(propertiesMutex, arvizMutex);
- drawBoxes(properties, arviz);
- }
+ return Component::defaultName;
}
- */
- /* (Requires the armarx::ArVizComponentPluginUser.)
- void
- Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
+ std::string
+ Component::GetDefaultName()
{
- // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
- // See the ArVizExample in RobotAPI for more examples.
-
- viz::Layer layer = arviz.layer(p.boxLayerName);
- for (int i = 0; i < p.numBoxes; ++i)
- {
- layer.add(viz::Box("box_" + std::to_string(i))
- .position(Eigen::Vector3f(i * 100, 0, 0))
- .size(20).color(simox::Color::blue()));
- }
- arviz.commit(layer);
+ return Component::defaultName;
}
- */
+
ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
index 713a0070..a6b6052e 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -36,6 +36,7 @@
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
#include <RobotAPI/libraries/armem/client.h>
#include "armarx/navigation/memory/client/costmap/Reader.h"
@@ -123,8 +124,18 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
struct Properties
{
// std::string boxLayerName = "boxes";
- std::string staticCostmapProviderName = "navigator";
- std::string staticCostmapName = "distance_to_obstacles";
+ struct
+ {
+ std::string providerName = "navigator";
+ std::string name = "distance_to_obstacles";
+ } staticCostmap;
+
+ struct
+ {
+ std::string providerName = "LaserScannerFeatureExtraction";
+ std::string name = ""; // all
+ } laserScannerFeatures;
+
int updatePeriodMs = 100;
};
Properties properties;
@@ -156,6 +167,8 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
memory::client::costmap::Reader costmapReader;
memory::client::costmap::Writer costmapWriter;
+ armem::vision::laser_scanner_features::client::Reader laserScannerFeaturesReader;
+
std::optional<algorithms::Costmap> staticCostmap;
PeriodicTask<Component>::pointer_type updateCostmapTask;
--
GitLab
From 05fecf29218856f81a415e01a0d9b20d12a4b3cf Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 2 May 2022 14:33:13 +0200
Subject: [PATCH 7/9] dynamic_distance_to_obstacle_costmap_provider: distance
calc
---
source/armarx/navigation/algorithms/Costmap.h | 3 +-
.../Component.cpp | 105 ++++++++++++++++--
2 files changed, 97 insertions(+), 11 deletions(-)
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 2fa0030c..a59626f0 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -48,6 +48,7 @@ namespace armarx::navigation::algorithms
Vertex toVertex(const Position& v) const;
const Grid& getGrid() const;
+ Grid& getMutableGrid(){ return grid; }
bool isInCollision(const Position& p) const;
@@ -80,7 +81,7 @@ namespace armarx::navigation::algorithms
const std::optional<Mask>& getMask() const noexcept;
std::optional<Costmap::Mask>& getMutableMask() noexcept;
-
+
std::optional<float> value(const Index& index) const;
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index ef567199..e8fa0b60 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -23,11 +23,22 @@
#include "Component.h"
+#include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/assign.hpp>
+#include <boost/geometry/algorithms/detail/distance/interface.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+
+#include <SimoxUtility/algorithm/apply.hpp>
+#include <SimoxUtility/algorithm/vector.hpp>
+
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
+#include "armarx/navigation/algorithms/Costmap.h"
#include "armarx/navigation/memory/client/costmap/Reader.h"
// Include headers you only need in function definitions in the .cpp.
@@ -154,35 +165,110 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
}
+ using point_type = boost::geometry::model::d2::point_xy<float>;
+ using polygon_type = boost::geometry::model::polygon<point_type>;
+
+ polygon_type
+ toPolygon(const std::vector<Eigen::Vector2f>& hull)
+ {
+
+ std::vector<point_type> points = simox::alg::apply(
+ hull, [](const Eigen::Vector2f& pt) { return point_type(pt.x(), pt.y()); });
+
+ polygon_type polygon;
+ boost::geometry::assign_points(polygon, points);
+
+ return polygon;
+ }
+
+ float
+ computeDistance(const std::vector<polygon_type>& obstacles, const Eigen::Vector2f& pt)
+ {
+ const point_type point(pt.x(), pt.y());
+
+ float dist = std::numeric_limits<float>::max();
+ for (const auto& obstacle : obstacles)
+ {
+ const float d = boost::geometry::distance(point, obstacle);
+ if (d < dist)
+ {
+ dist = d;
+ }
+ }
+
+ return dist;
+ }
+
+
+ void
+ fillCostmap(const std::vector<armem::vision::LaserScannerFeatures>& features,
+ algorithms::Costmap& costmap)
+ {
+
+
+ std::vector<polygon_type> obstacles;
+
+ for (const auto& f : features)
+ {
+ for (const auto& ff : f.features)
+ {
+ obstacles.push_back(toPolygon(ff.convexHull));
+ }
+ }
+
+ ARMARX_INFO << "Found " << obstacles.size() << " obstacles/polygons.";
+
+
+ for (int r = 0; r < costmap.getGrid().rows(); r++)
+ {
+ for (int c = 0; c < costmap.getGrid().cols(); c++)
+ {
+ algorithms::Costmap::Index idx(r, c);
+ const auto pos = costmap.toPosition(idx);
+
+ if (costmap.isInCollision(pos))
+ {
+ continue;
+ }
+
+ const float dist = computeDistance(obstacles, pos);
+
+ // update the costmap. combine static and dynamic distances
+ costmap.getMutableGrid()(r, c) = std::min(costmap.getGrid()(r, c), dist);
+ }
+ }
+ }
+
void
Component::updateCostmap()
{
ARMARX_CHECK(staticCostmap.has_value());
- const armem::vision::laser_scanner_features::client::Reader::Query query
- {
+ const armem::vision::laser_scanner_features::client::Reader::Query query{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
- .timestamp = armarx::core::time::Clock::Now()
- };
+ .timestamp = armarx::core::time::Clock::Now()};
const auto result = laserScannerFeaturesReader.queryData(query);
- if(result.status != result.Success)
+ if (result.status != result.Success)
{
ARMARX_WARNING << "Failed to retrieve data from memory";
return;
}
- for(const auto& e : result.features)
+ for (const auto& e : result.features)
{
- ARMARX_INFO << "Number of laser scanner clusters for sensor "<< e.frame << ": " << e.features.size();
+ ARMARX_INFO << "Number of laser scanner clusters for sensor " << e.frame << ": "
+ << e.features.size();
}
- }
-
+ // copy the static costmap. masked out regions don't need to be updated.
+ auto dynamicCostmap = staticCostmap.value();
+ fillCostmap(result.features, dynamicCostmap);
+ }
void
@@ -211,7 +297,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
}
-
ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
--
GitLab
From 20aa959fedc34fee431ac5e1a0b722672e5e51fd Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 3 May 2022 16:44:20 +0200
Subject: [PATCH 8/9] dynamic_distance_to_obstacle_costmap: memory read & write
---
.../algorithms/aron_conversions.cpp | 17 ++-
.../navigation/algorithms/aron_conversions.h | 3 +-
.../CMakeLists.txt | 2 +-
.../Component.cpp | 131 ++++++++++++++++--
.../Component.h | 20 +--
.../memory/client/costmap/Reader.cpp | 8 +-
.../memory/client/costmap/Writer.cpp | 3 +-
7 files changed, 148 insertions(+), 36 deletions(-)
diff --git a/source/armarx/navigation/algorithms/aron_conversions.cpp b/source/armarx/navigation/algorithms/aron_conversions.cpp
index 59ce8f93..24cfb351 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.cpp
+++ b/source/armarx/navigation/algorithms/aron_conversions.cpp
@@ -1,5 +1,7 @@
#include "aron_conversions.h"
+#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>
@@ -54,22 +56,31 @@ namespace armarx::navigation::algorithms
}
Costmap
- fromAron(const arondto::Costmap& dto)
+ fromAron(const armem::wm::EntityInstance& entityInstance)
{
+ const auto aronDto = armem::tryCast<algorithms::arondto::Costmap>(entityInstance);
+ ARMARX_CHECK(aronDto) << "Failed casting to OccupancyGrid";
+ const auto& dto = *aronDto;
+
+ ARMARX_DEBUG << entityInstance.data()->getAllKeys();
+
const Costmap::Parameters parameters{.binaryGrid = false, .cellSize = dto.cellSize};
+ ARMARX_DEBUG << "Converting grid";
const auto gridNavigator =
- aron::data::NDArray::DynamicCast(dto.toAron()->getElement("grid"));
+ aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("grid"));
+
ARMARX_CHECK_NOT_NULL(gridNavigator);
Costmap::Grid grid =
aron::converter::AronEigenConverter::ConvertToDynamicMatrix<Costmap::Grid::value_type>(
*gridNavigator);
+ ARMARX_DEBUG << "Converting mask";
std::optional<Costmap::Mask> mask;
if (const auto maskNavigator =
- aron::data::NDArray::DynamicCast(dto.toAron()->getElement("mask")))
+ aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("mask")))
{
mask = aron::converter::AronEigenConverter::ConvertToDynamicMatrix<
Costmap::Mask::value_type>(*maskNavigator);
diff --git a/source/armarx/navigation/algorithms/aron_conversions.h b/source/armarx/navigation/algorithms/aron_conversions.h
index 9ad4265e..b595e5bc 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.h
+++ b/source/armarx/navigation/algorithms/aron_conversions.h
@@ -21,6 +21,7 @@
#pragma once
+#include "RobotAPI/libraries/armem/core/wm/memory_definitions.h"
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
@@ -40,7 +41,7 @@ namespace armarx::navigation::algorithms
armarx::aron::data::DictPtr toAron(const Costmap& bo);
// Costmap does not provide a default c'tor
- Costmap fromAron(const arondto::Costmap& dto);
+ Costmap fromAron(const armem::wm::EntityInstance& entityInstance);
} // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
index 9c14667f..70c1f50d 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
@@ -20,7 +20,7 @@ armarx_add_component(dynamic_distance_to_obstacle_costmap_provider
armem_vision
## RobotAPICore
## RobotAPIInterfaces
- ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotAPIComponentPlugins # For ArViz and other plugins.
armarx_navigation::memory
# DEPENDENCIES_LEGACY
## Add libraries that do not provide any targets but ${FOO_*} variables.
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index e8fa0b60..ea1b2f47 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -31,15 +31,19 @@
#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/algorithm/vector.hpp>
+#include <SimoxUtility/color/cmaps/colormaps.h>
+#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include "RobotAPI/interface/ArViz/Elements.h"
#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
#include "armarx/navigation/algorithms/Costmap.h"
#include "armarx/navigation/memory/client/costmap/Reader.h"
+#include <armarx/navigation/conversions/eigen.h>
// Include headers you only need in function definitions in the .cpp.
@@ -135,14 +139,19 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
*/
// connect memory readers and writers
+ ARMARX_INFO << "Connecting to costmap segments";
costmapReader.connect();
costmapWriter.connect();
+
+ ARMARX_INFO << "Connecting to laser scanner features segment";
laserScannerFeaturesReader.connect();
ARMARX_CHECK(readStaticCostmap());
updateCostmapTask =
new PeriodicTask<Component>(this, &Component::updateCostmap, properties.updatePeriodMs);
+
+ updateCostmapTask->start();
}
bool
@@ -152,6 +161,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
.providerName = properties.staticCostmap.providerName,
.name = properties.staticCostmap.name,
.timestamp = armarx::core::time::Clock::Now()};
+
const auto result = costmapReader.query(query);
if (result.costmap.has_value())
@@ -160,7 +170,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
return true;
}
- ARMARX_WARNING << "Could not read static costmap";
+ ARMARX_WARNING << "Could not read static costmap. Reason: " << result.errorMessage;
return false;
}
@@ -175,6 +185,8 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
std::vector<point_type> points = simox::alg::apply(
hull, [](const Eigen::Vector2f& pt) { return point_type(pt.x(), pt.y()); });
+ points.push_back(points.front()); // close polygon
+
polygon_type polygon;
boost::geometry::assign_points(polygon, points);
@@ -205,18 +217,25 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
algorithms::Costmap& costmap)
{
-
std::vector<polygon_type> obstacles;
for (const auto& f : features)
{
+ const auto& global_T_sens = f.frameGlobalPose;
for (const auto& ff : f.features)
{
- obstacles.push_back(toPolygon(ff.convexHull));
+
+ std::vector<Eigen::Vector2f> hull = ff.convexHull;
+ for (auto& h : hull)
+ {
+ h = conv::to2D(global_T_sens * conv::to3D(h));
+ }
+
+ obstacles.push_back(toPolygon(hull));
}
}
- ARMARX_INFO << "Found " << obstacles.size() << " obstacles/polygons.";
+ ARMARX_VERBOSE << "Found " << obstacles.size() << " obstacles/polygons.";
for (int r = 0; r < costmap.getGrid().rows(); r++)
@@ -226,29 +245,107 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
algorithms::Costmap::Index idx(r, c);
const auto pos = costmap.toPosition(idx);
+ // if(costmap.getMask().has_value())
+ // {
+ // costmap.getMutableMask().value()(r,c) = true; // validate this cell
+ // }
+
if (costmap.isInCollision(pos))
{
continue;
}
- const float dist = computeDistance(obstacles, pos);
+ constexpr float robotRadius = 500;
+
+ const float dist = computeDistance(obstacles, pos) - robotRadius;
+
+ if (dist <= 0) // in collision?
+ {
+ if (costmap.getMask().has_value())
+ {
+ costmap.getMutableMask().value()(r, c) = false; // invalidate this cell
+ }
+ }
// update the costmap. combine static and dynamic distances
- costmap.getMutableGrid()(r, c) = std::min(costmap.getGrid()(r, c), dist);
+ costmap.getMutableGrid()(r, c) =
+ std::min(costmap.getGrid()(r, c), std::max(dist, 0.F));
+ // costmap.getMutableGrid()(r, c) = std::max(dist, 0.F);
}
}
}
+
+ void
+ Component::drawCostmap(const armarx::navigation::algorithms::Costmap& costmap,
+ const std::string& name,
+ const float heightOffset)
+ {
+ const auto cmap = simox::color::cmaps::viridis();
+ const float vmax = costmap.getGrid().maxCoeff();
+
+ ARMARX_VERBOSE << "Grid `" << name << "` max value is " << vmax;
+
+ const auto asColor = [&cmap, &vmax](const float distance) -> armarx::viz::data::Color
+ {
+ const auto color = cmap.at(distance, 0.F, vmax);
+ return {color.a, color.r, color.g, color.b};
+ };
+
+ const armarx::viz::data::Color colorInvalid(0, 0, 0, 0);
+
+ auto layer = arviz.layer("costmap_" + name);
+
+ const std::int64_t cols = costmap.getGrid().cols();
+ const std::int64_t rows = costmap.getGrid().rows();
+
+ auto mesh = armarx::viz::Mesh("");
+
+ const Eigen::Vector3f heightOffsetV = Eigen::Vector3f::UnitZ() * heightOffset;
+
+ std::vector<std::vector<Eigen::Vector3f>> vertices;
+ std::vector<std::vector<armarx::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) =
+ armarx::navigation::conv::to3D(costmap.toPosition({r, c})) + heightOffsetV;
+
+ colorsRow.at(c) = [&]()
+ {
+ if (costmap.isValid({r, c}))
+ {
+ return asColor(costmap.getGrid()(r, c));
+ }
+
+ return colorInvalid;
+ }();
+ }
+ }
+
+ mesh.grid2D(vertices, colors);
+
+ layer.add(mesh);
+
+ arviz.commit(layer);
+ }
+
void
Component::updateCostmap()
{
ARMARX_CHECK(staticCostmap.has_value());
+ const auto timestamp = armarx::core::time::Clock::Now();
+
const armem::vision::laser_scanner_features::client::Reader::Query query{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
- .timestamp = armarx::core::time::Clock::Now()};
+ .timestamp = timestamp};
const auto result = laserScannerFeaturesReader.queryData(query);
@@ -260,14 +357,28 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
for (const auto& e : result.features)
{
- ARMARX_INFO << "Number of laser scanner clusters for sensor " << e.frame << ": "
- << e.features.size();
+ ARMARX_VERBOSE << "Number of laser scanner clusters for sensor " << e.frame << ": "
+ << e.features.size();
}
// copy the static costmap. masked out regions don't need to be updated.
auto dynamicCostmap = staticCostmap.value();
- fillCostmap(result.features, dynamicCostmap);
+ // create costmap and store it in memory
+ {
+ const auto start = armarx::core::time::Clock::Now();
+ fillCostmap(result.features, dynamicCostmap);
+ const auto end = armarx::core::time::Clock::Now();
+
+ ARMARX_INFO << deactivateSpam(1) << "Creation of dynamic costmap took "
+ << (end - start).toMilliSeconds() << " milliseconds";
+ }
+
+
+ costmapWriter.store(dynamicCostmap, "dynamic_distance_to_obstacles", getName(), timestamp);
+
+ // drawing
+ drawCostmap(dynamicCostmap, "dynamic_costmap", 10);
}
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
index a6b6052e..95b94b74 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -23,20 +23,12 @@
#pragma once
-
-// #include <mutex>
-
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <ArmarXCore/core/Component.h>
#include "RobotAPI/libraries/armem/client/forward_declarations.h"
-
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-
-// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client.h>
#include "armarx/navigation/memory/client/costmap/Reader.h"
@@ -50,11 +42,10 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
class Component :
virtual public armarx::Component,
virtual public armarx::navigation::components::
- dynamic_distance_to_obstacle_costmap_provider::ComponentInterface
+ dynamic_distance_to_obstacle_costmap_provider::ComponentInterface,
// , virtual public armarx::DebugObserverComponentPluginUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
- ,
+ virtual public armarx::ArVizComponentPluginUser,
virtual public armem::ListeningClientPluginUser
{
public:
@@ -88,6 +79,10 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
bool readStaticCostmap();
+ void drawCostmap(const armarx::navigation::algorithms::Costmap& costmap,
+ const std::string& name,
+ float heightOffset);
+
/* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
/// This function should be called once in onConnect() or when you
@@ -113,7 +108,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
*/
- private:
static const std::string defaultName;
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.cpp b/source/armarx/navigation/memory/client/costmap/Reader.cpp
index 93ed8841..17a48dbc 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Reader.cpp
@@ -70,11 +70,7 @@ namespace armarx::navigation::memory::client::costmap
return false;
});
ARMARX_CHECK_NOT_NULL(entityInstance);
-
- const auto aronDto = armem::tryCast<algorithms::arondto::Costmap>(*entityInstance);
- ARMARX_CHECK(aronDto) << "Failed casting to OccupancyGrid";
-
- return algorithms::fromAron(*aronDto);
+ return algorithms::fromAron(*entityInstance);
}
Reader::Result
@@ -82,7 +78,7 @@ namespace armarx::navigation::memory::client::costmap
{
const auto qb = buildQuery(query);
- ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
diff --git a/source/armarx/navigation/memory/client/costmap/Writer.cpp b/source/armarx/navigation/memory/client/costmap/Writer.cpp
index aad3598b..f076fe1c 100644
--- a/source/armarx/navigation/memory/client/costmap/Writer.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Writer.cpp
@@ -17,7 +17,7 @@ namespace armarx::navigation::memory::client::costmap
{
std::lock_guard g{memoryWriterMutex()};
- const auto result = memoryWriter().addSegment(properties().coreSegmentName, providerName);
+ const auto result = memoryWriter().addSegment(memory::constants::CostmapCoreSegmentName, providerName);
if (not result.success)
{
@@ -60,7 +60,6 @@ namespace armarx::navigation::memory::client::costmap
armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
Writer::defaultProperties() const
{
-
return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
.coreSegmentName =
memory::constants::CostmapCoreSegmentName};
--
GitLab
From 5fc287e23ce2f6a656db19d23a4873531100a03a Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 3 May 2022 16:44:58 +0200
Subject: [PATCH 9/9] scenario update
---
.../config/VisionMemory.cfg | 80 +++++++++++++++-
.../config/example_client.cfg | 2 +-
.../config/navigation_memory.cfg | 93 +++++++++++++++++--
.../PlatformNavigation/config/navigator.cfg | 26 ++++--
4 files changed, 178 insertions(+), 23 deletions(-)
diff --git a/scenarios/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
index 5f13a297..9c59a9f6 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -218,6 +218,15 @@
# ArmarX.VisionMemory.mem.ltm..buffer.storeFreq = 10
+# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
# ArmarX.VisionMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
@@ -227,6 +236,24 @@
# ArmarX.VisionMemory.mem.ltm.enabled = false
+# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.imageExtractor.Enabled = true
+
+
# ArmarX.VisionMemory.mem.ltm.memFreqFilter.Enabled:
# Attributes:
# - Default: false
@@ -244,20 +271,63 @@
# ArmarX.VisionMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-# ArmarX.VisionMemory.mem.ltm.memSnapFilter.WaitingTime: Waiting time in MS after each Entity update.
+# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.pngConverter.Enabled = true
+
+
+# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+
+
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.Enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
# Attributes:
# - Default: -1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.VisionMemory.mem.ltm.memSnapFilter.WaitingTime = -1
+# ArmarX.VisionMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage.
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled:
# Attributes:
-# - Default: /tmp/MemoryExport
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
+# ArmarX.VisionMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
+# Attributes:
+# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.VisionMemory.mem.ltm.storagepath = /tmp/MemoryExport
+# ArmarX.VisionMemory.mem.ltm.storagepath = Default value not mapped.
# ArmarX.VisionMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/example_client.cfg b/scenarios/PlatformNavigation/config/example_client.cfg
index 384be319..df53acaf 100644
--- a/scenarios/PlatformNavigation/config/example_client.cfg
+++ b/scenarios/PlatformNavigation/config/example_client.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/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index d02930d4..604df43d 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -158,6 +158,15 @@
# ArmarX.NavigationMemory.mem.ltm..buffer.storeFreq = 10
+# ArmarX.NavigationMemory.mem.ltm.depthImageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.depthImageExtractor.Enabled = true
+
+
# ArmarX.NavigationMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
@@ -167,7 +176,34 @@
# ArmarX.NavigationMemory.mem.ltm.enabled = false
-# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.WaitingTime: Withdraw time in MS after each LTM update.
+# ArmarX.NavigationMemory.mem.ltm.exrConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.exrConverter.Enabled = true
+
+
+# ArmarX.NavigationMemory.mem.ltm.imageExtractor.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.imageExtractor.Enabled = true
+
+
+# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.Enabled = false
+
+
+# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
# Attributes:
# - Default: -1
# - Case sensitivity: yes
@@ -175,20 +211,63 @@
# ArmarX.NavigationMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-# ArmarX.NavigationMemory.mem.ltm.memSnapFilter.WaitingTime: Withdraw time in MS after each Entity update.
+# ArmarX.NavigationMemory.mem.ltm.pngConverter.Enabled:
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.pngConverter.Enabled = true
+
+
+# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
+
+
+# ArmarX.NavigationMemory.mem.ltm.snapEqFilter.Enabled:
# Attributes:
-# - Default: 1000
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.memSnapFilter.WaitingTime = 1000
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.snapEqFilter.Enabled = false
-# ArmarX.NavigationMemory.mem.ltm.storagepath: The path to the memory storage.
+# ArmarX.NavigationMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
# Attributes:
-# - Default: /tmp/MemoryExport
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NavigationMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
+
+
+# ArmarX.NavigationMemory.mem.ltm.snapFreqFilter.Enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.NavigationMemory.mem.ltm.snapFreqFilter.Enabled = false
+
+
+# ArmarX.NavigationMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.NavigationMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
+
+
+# ArmarX.NavigationMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
+# Attributes:
+# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.storagepath = /tmp/MemoryExport
+# ArmarX.NavigationMemory.mem.ltm.storagepath = Default value not mapped.
# ArmarX.NavigationMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index d3a831f2..4f2c5e40 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -310,6 +310,22 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.Navigator.mem.robot_state.proprioceptionSegment = Proprioception
+# ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment: No Description
+# Attributes:
+# - Default: OccupancyGrid
+# - Case sensitivity: no
+# - Required: no
+ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
+
+
+# ArmarX.Navigator.mem.vision.occupancy_grid.Memory: No Description
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: no
+# - 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:
@@ -412,13 +428,3 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
ArmarX.Verbosity = Verbose
-# ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment:
-# Attributes:
-ArmarX.Navigator.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
-
-
-# ArmarX.Navigator.mem.vision.occupancy_grid.Memory:
-# Attributes:
-ArmarX.Navigator.mem.vision.occupancy_grid.Memory = Vision
-
-
--
GitLab