diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index 2fa0030c38e13f33aa4e45f8a33d792d420cac1b..a59626f0f6d9daa0e2e8b1bbd6f7446223846842 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/algorithms/aron_conversions.cpp b/source/armarx/navigation/algorithms/aron_conversions.cpp
index 59ce8f93a16bfd7edd7ca03afdd82be2bf22aacc..24cfb351e6e91e4320e876a880f7d477a0aa9e0b 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 9ad4265efbdc1410dc19b1ce6ef60c55ac4e8814..b595e5bc1d1a56054c4ae97347ce0b891b6f094a 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/CMakeLists.txt b/source/armarx/navigation/components/CMakeLists.txt
index dde47f4f7c71b5a3b4960a67d44b098563d379d0..34e29e918478e7440e083272c0c28d589bf3fa0f 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/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 3e5aa8824e7485191323ca712be593a4e5f5edb0..5a7c32ec756a7b136253062e956a289a9fec0548 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -187,13 +187,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/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 0000000000000000000000000000000000000000..70c1f50da52ab4c0c03dde28e95c47c9a5f1a18b
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt
@@ -0,0 +1,30 @@
+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
+ armem_vision
+ ## 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
+ # 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 0000000000000000000000000000000000000000..ea1b2f473f8b114ae854f1a2071b0d0c1ea46d2c
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -0,0 +1,413 @@
+/**
+ * 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 <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 <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.
+
+// #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.updatePeriodMs, "p.updatePeriodMs", "");
+
+ costmapReader.registerPropertyDefinitions(def);
+ costmapWriter.registerPropertyDefinitions(def);
+ laserScannerFeaturesReader.registerPropertyDefinitions(def);
+
+ return def;
+ }
+
+
+ Component::Component() :
+ costmapReader(memoryNameSystem()),
+ costmapWriter(memoryNameSystem()),
+ laserScannerFeaturesReader(memoryNameSystem())
+ {
+ }
+
+ 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();
+ }
+ */
+
+ // 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
+ Component::readStaticCostmap()
+ {
+ const memory::client::costmap::Reader::Query query{
+ .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())
+ {
+ staticCostmap.emplace(result.costmap.value());
+ return true;
+ }
+
+ ARMARX_WARNING << "Could not read static costmap. Reason: " << result.errorMessage;
+ return false;
+ }
+
+
+ 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()); });
+
+ points.push_back(points.front()); // close polygon
+
+ 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)
+ {
+ const auto& global_T_sens = f.frameGlobalPose;
+ for (const auto& ff : f.features)
+ {
+
+ 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_VERBOSE << "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.getMask().has_value())
+ // {
+ // costmap.getMutableMask().value()(r,c) = true; // validate this cell
+ // }
+
+ if (costmap.isInCollision(pos))
+ {
+ continue;
+ }
+
+ 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), 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 = timestamp};
+
+ const auto result = laserScannerFeaturesReader.queryData(query);
+
+ if (result.status != result.Success)
+ {
+ ARMARX_WARNING << "Failed to retrieve data from memory";
+ return;
+ }
+
+ for (const auto& e : result.features)
+ {
+ 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();
+
+ // 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);
+ }
+
+
+ void
+ Component::onDisconnectComponent()
+ {
+ }
+
+
+ void
+ Component::onExitComponent()
+ {
+ }
+
+
+ std::string
+ Component::getDefaultName() const
+ {
+ return Component::defaultName;
+ }
+
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return Component::defaultName;
+ }
+
+
+ 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 0000000000000000000000000000000000000000..95b94b748ed594b553e13f4c11e03cdb3a4a0be4
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -0,0 +1,171 @@
+/**
+ * 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 "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/Component.h>
+
+#include "RobotAPI/libraries/armem/client/forward_declarations.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"
+#include "armarx/navigation/memory/client/costmap/Writer.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,
+ virtual public armem::ListeningClientPluginUser
+ {
+ public:
+ Component();
+
+ /// @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;
+
+
+ void updateCostmap();
+
+ 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
+ /// 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);
+ */
+
+
+ static const std::string defaultName;
+
+
+ // Private member variables go here.
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ // std::string boxLayerName = "boxes";
+ 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;
+ /* 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;
+ */
+
+ 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;
+ };
+
+} // 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 0000000000000000000000000000000000000000..3116466bc7400dfce7b4d2c96d72dad8cd0d0bf3
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice
@@ -0,0 +1,36 @@
+/*
+ * 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
+
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
+
+module armarx { module navigation { module components { module dynamic_distance_to_obstacle_costmap_provider
+{
+
+ interface ComponentInterface extends armarx::armem::client::MemoryListenerInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.cpp b/source/armarx/navigation/memory/client/costmap/Reader.cpp
index c3567bfa76922d7d4ada54b26f764534ec7c720f..17a48dbcd66bb523492bc885c6ded10251f60b52 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;
@@ -70,11 +70,7 @@ namespace armarx::navigation::mem::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::mem::client::costmap
{
const auto qb = buildQuery(query);
- ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
@@ -127,4 +123,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 e8875b73edbaa1adad2be29ccafe923687f0be0a..24b02adab251453551dbe579afe18e0d08933d21 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 141d9c0c528d4fcfa9463a3979a22856dfade187..f076fe1ceb1dbec0b370c9a1207697e4b476eb4c 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;
@@ -17,7 +17,7 @@ namespace armarx::navigation::mem::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,10 +60,9 @@ namespace armarx::navigation::mem::client::costmap
armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
Writer::defaultProperties() const
{
-
return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
.coreSegmentName =
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 f75f40eae940b456f8ad3782997b71650925de90..0e44e30d886e17a7d218ecb32d78d08e9724fafe 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 6ff75455e10ae89237973a963a9c7dba941a66e1..b15a93ca4cf232f4b304129ea51b3e462f592811 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 d1ac26e04543e63286870eea2b97f82745fa4024..833f37052d34829fc98092320a4e9827717da937 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 9804927250f74b59c7f160cc65cf7f753de9431a..cb986d2db0149e9f84411018404df0226d990d7d 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 a61bc7c6dbafcbf24e5e36afa82e3704566b988c..702a4d1a1971107d9f13a61a02aeaeefdfcf1df5 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 0d155874c43225a391f3fe3be7337ee229966848..311a5f4b771b6c22e1b22965992bb4da9f1f1145 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 9c6d871b5d18e402def2a49c03c67a8f670156fb..f92fd12ef912d1e07720accad577c998609d1373 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 33898b8969b07373bd982a1298f3f8188ef34715..23a8d443d51a244aaa53bef002aeb1b7599b8322 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 eda0e83ea2e8fd32b2e28952916f53ab858e108f..58c6cd2263061b2a37d55c134d688b1efb1ed76f 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 5e5f1adcf514f7331149d2c641089b75c1ab7ad7..759b42f6aadb4241566b9a4e412c912a13b436bf 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 5d7dac465e68cfb11f68c60af70c8bc4a365f5f1..1ece55943f1c3bceeecaf6d7385918df78866412 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 0f3cc13354c0cb23dc7b694132911422dfb473e3..60adb3a685b6f88f9f1976b891db36fe6d4e1978 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 1b130a83f09e0440e0ce3aca24cfb4e3bd54a5b5..57204aba218df6073fd5d36359e4fbfa4079ba6e 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 bb0e4d0b01e7c30a084dfd57383cdbe8670b7749..226a0c80806a9700efcff8231f00b418a62c6d83 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 18ecf76906c6e5310bee7765b17a72347ee66c7a..a141d4ba648d13c08e1ec8dacd9dc35c2944dbcf 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 676dccef819ede51808e128243ea4cbc0eaf21ef..b4334831882a68d6947b90459b0ca03e72c0939d 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 478b456ae964f8b9cafa27195b42b0e524a79b9f..0eb1e0b7637c9fa0c37edbc5ea768e048150a698 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
diff --git a/source/armarx/navigation/util/util.cpp b/source/armarx/navigation/util/util.cpp
index 42f141f928fcebf4dedab101a2c79b991bab9d27..89679dd68d448c845bc0acd6f1a60da24743a951 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 7f2aa207971f80819821a7a961ab909d4af0b11a..bded81afafac01d34480a595a919eb6a4192db28 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