From 54466920c92ee945f68301f74a1a2bec8a30ea21 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 16 Aug 2022 21:36:22 +0200
Subject: [PATCH] distance to obstacle costmap as separate component
---
.../navigation/components/CMakeLists.txt | 3 +-
.../NavigationMemory/CMakeLists.txt | 1 +
.../NavigationMemory/NavigationMemory.cpp | 13 +-
.../NavigationMemory/NavigationMemory.h | 2 +
.../components/NavigationMemory/Visu.cpp | 93 +++++-
.../components/NavigationMemory/Visu.h | 7 +-
.../components/Navigator/Navigator.cpp | 43 +--
.../components/Navigator/Navigator.h | 5 +-
.../CMakeLists.txt | 34 +++
.../Component.cpp | 268 ++++++++++++++++++
.../Component.h | 152 ++++++++++
.../ComponentInterface.ice | 35 +++
.../Component.cpp | 8 +
.../Component.h | 2 +-
source/armarx/navigation/core/StaticScene.h | 3 +-
.../navigation/global_planning/AStar.cpp | 2 +-
.../navigation/global_planning/SPFA.cpp | 7 +-
.../memory/client/costmap/Reader.cpp | 4 +-
.../server/scene_provider/SceneProvider.cpp | 35 +--
19 files changed, 635 insertions(+), 82 deletions(-)
create mode 100644 source/armarx/navigation/components/distance_to_obstacle_costmap_provider/CMakeLists.txt
create mode 100644 source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.cpp
create mode 100644 source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.h
create mode 100644 source/armarx/navigation/components/distance_to_obstacle_costmap_provider/ComponentInterface.ice
diff --git a/source/armarx/navigation/components/CMakeLists.txt b/source/armarx/navigation/components/CMakeLists.txt
index 3f13b407..24217c76 100644
--- a/source/armarx/navigation/components/CMakeLists.txt
+++ b/source/armarx/navigation/components/CMakeLists.txt
@@ -4,4 +4,5 @@ add_subdirectory(Navigator)
add_subdirectory(dynamic_distance_to_obstacle_costmap_provider)
-add_subdirectory(dynamic_scene_provider)
\ No newline at end of file
+add_subdirectory(dynamic_scene_provider)
+add_subdirectory(distance_to_obstacle_costmap_provider)
\ No newline at end of file
diff --git a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
index 06bd4080..3f8da95e 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
@@ -17,6 +17,7 @@ armarx_add_component(navigation_memory
## NavigationMemoryInterfaces # If you defined a component ice interface above.
armarx_navigation::graph
armarx_navigation::location
+ armarx_navigation::algorithms
SOURCES
NavigationMemory.cpp
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index 0189ad50..a18764b0 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -27,6 +27,8 @@
#include <fstream>
#include <iostream>
+#include "ArmarXCore/core/time/Metronome.h"
+#include "ArmarXCore/core/time/forward_declarations.h"
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
@@ -429,7 +431,9 @@ namespace armarx::navigation
{
memory::Visu visu{arviz,
workingMemory().getCoreSegment(navigation::location::coreSegmentID),
- workingMemory().getCoreSegment(navigation::graph::coreSegmentID)};
+ workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName)};
+
Properties::LocationGraph p;
{
@@ -437,7 +441,7 @@ namespace armarx::navigation
p = properties.locationGraph;
}
- CycleUtil cycle(static_cast<int>(1000 / p.visuFrequency));
+ Metronome metronome(Frequency::HertzDouble(p.visuFrequency));
while (tasks.visuTask and not tasks.visuTask->isStopped())
{
ARMARX_TRACE;
@@ -455,9 +459,12 @@ namespace armarx::navigation
// Graph Edges
visu.drawGraphs(layers, p.visuGraphEdges);
+ // Costmaps
+ visu.drawCostmaps(layers, p.visuCostmaps);
+
arviz.commit(layers);
- cycle.waitForCycleDuration();
+ metronome.waitForNextTick();
}
}
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
index 0b4b95a5..c67da7fc 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
@@ -96,6 +96,8 @@ namespace armarx::navigation
{
bool visuLocations = true;
bool visuGraphEdges = true;
+ bool visuCostmaps = true;
+
float visuFrequency = 2;
};
LocationGraph locationGraph;
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.cpp b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
index 07f364bc..73df72de 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
@@ -22,8 +22,14 @@
#include "Visu.h"
+#include <SimoxUtility/color/cmaps/colormaps.h>
+
+#include "RobotAPI/components/ArViz/Client/Layer.h"
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include "armarx/navigation/conversions/eigen.h"
+#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
@@ -35,10 +41,12 @@ namespace armarx::navigation::memory
Visu::Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
- const armem::server::wm::CoreSegment& graphSegment) :
+ const armem::server::wm::CoreSegment& graphSegment,
+ const armem::server::wm::CoreSegment& costmapSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
+ costmapSegment(costmapSegment),
visu(std::make_unique<graph::GraphVisu>())
{
}
@@ -117,4 +125,87 @@ namespace armarx::navigation::memory
}
}
+ namespace
+ {
+
+ void
+ visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name)
+ {
+ const auto cmap = simox::color::cmaps::viridis();
+ const float vmax = costmap.getGrid().array().maxCoeff();
+
+ const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
+ {
+ const auto color = cmap.at(distance, 0.F, vmax);
+ return {color.a, color.r, color.g, color.b};
+ };
+
+ const std::int64_t cols = costmap.getGrid().cols();
+ const std::int64_t rows = costmap.getGrid().rows();
+
+ auto mesh = viz::Mesh(name);
+
+ std::vector<std::vector<Eigen::Vector3f>> vertices;
+ std::vector<std::vector<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) = conv::to3D(costmap.toPositionGlobal({r, c}));
+ colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
+ }
+ }
+
+ mesh.grid2D(vertices, colors);
+
+ layer.add(mesh);
+ }
+
+ } // namespace
+
+ void
+ Visu::drawCostmaps(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ if (not enabled)
+ {
+ return;
+ }
+
+ std::map<std::string, std::vector<std::pair<std::string, algorithms::Costmap>>>
+ namedProviderCostmaps;
+
+ costmapSegment.doLocked(
+ [&]()
+ {
+ using namespace armem::server;
+
+ costmapSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ if (const wm::EntityInstance* instance = entity.findLatestInstance())
+ {
+ navigation::algorithms::Costmap costmap =
+ algorithms::fromAron(*instance);
+
+ namedProviderCostmaps[instance->id().providerSegmentName].emplace_back(
+ instance->id().entityName, std::move(costmap));
+ }
+ });
+ });
+
+ for (const auto& [providerName, namedCostmaps] : namedProviderCostmaps)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("costmaps_" + providerName));
+ for (const auto& [name, costmap] : namedCostmaps)
+ {
+ visualize(costmap, layer, name);
+ }
+ }
+ }
+
+
} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.h b/source/armarx/navigation/components/NavigationMemory/Visu.h
index f28fa6d8..cfd732ad 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.h
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.h
@@ -25,8 +25,10 @@
#include <memory>
#include <vector>
+#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include "armarx/navigation/algorithms/Costmap.h"
namespace armarx::navigation::graph
@@ -42,12 +44,14 @@ namespace armarx::navigation::memory
public:
Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
- const armem::server::wm::CoreSegment& graphSegment);
+ const armem::server::wm::CoreSegment& graphSegment,
+ const armem::server::wm::CoreSegment& costmapSegment);
~Visu();
void drawLocations(std::vector<viz::Layer>& layers, bool enabled);
void drawGraphs(std::vector<viz::Layer>& layers, bool enabled);
+ void drawCostmaps(std::vector<viz::Layer>& layers, bool enabled);
public:
@@ -55,6 +59,7 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment;
const armem::server::wm::CoreSegment& graphSegment;
+ const armem::server::wm::CoreSegment& costmapSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
};
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 8b56fd6d..e10c47a8 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -123,7 +123,6 @@ namespace armarx::navigation::components
addPlugin(resultsWriterPlugin);
addPlugin(graphReaderPlugin);
addPlugin(costmapReaderPlugin);
- addPlugin(costmapWriterPlugin);
addPlugin(virtualRobotReaderPlugin);
@@ -174,7 +173,7 @@ namespace armarx::navigation::components
->getRobotName();
const server::scene_provider::SceneProvider::Config cfg{.robotName = robotName};
- sceneProvider = server::scene_provider::SceneProvider(srv, cfg);
+ sceneProvider.emplace(srv, cfg);
}
initializeScene();
@@ -449,46 +448,6 @@ namespace armarx::navigation::components
return def;
}
- void
- visualize(const algorithms::Costmap& costmap, viz::Client& arviz, const std::string& name)
- {
- const auto cmap = simox::color::cmaps::viridis();
- const float vmax = costmap.getGrid().array().maxCoeff();
-
- const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
- {
- const auto color = cmap.at(distance, 0.F, vmax);
- return {color.a, color.r, color.g, color.b};
- };
-
- auto layer = arviz.layer("costmap_" + name);
-
- const std::int64_t cols = costmap.getGrid().cols();
- const std::int64_t rows = costmap.getGrid().rows();
-
- auto mesh = viz::Mesh("");
-
- std::vector<std::vector<Eigen::Vector3f>> vertices;
- std::vector<std::vector<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) = conv::to3D(costmap.toPositionGlobal({r, c}));
- colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
- }
- }
-
- mesh.grid2D(vertices, colors);
-
- layer.add(mesh);
-
- arviz.commit(layer);
- }
server::Navigator*
components::Navigator::activeNavigator()
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index d20a2101..4c25c56b 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -42,7 +42,6 @@
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
@@ -54,7 +53,6 @@
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/components/Navigator/RemoteGui.h>
#include <armarx/navigation/core/types.h>
-#include <armarx/navigation/memory/client/costmap/Writer.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/memory/client/parameterization/Reader.h>
#include <armarx/navigation/memory/client/stack_result/Writer.h>
@@ -195,8 +193,7 @@ namespace armarx::navigation::components
graphReaderPlugin = nullptr;
armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Reader>*
costmapReaderPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>*
- costmapWriterPlugin = nullptr;
+
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
// `robot_state` memory reader and writer
diff --git a/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/CMakeLists.txt b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/CMakeLists.txt
new file mode 100644
index 00000000..a637fdcb
--- /dev/null
+++ b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/CMakeLists.txt
@@ -0,0 +1,34 @@
+armarx_add_component(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
+ armem_robot_state
+ armem_robot
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ RobotAPIComponentPlugins # For ArViz and other plugins.
+ armarx_navigation::memory
+ armarx_navigation::algorithms
+ armarx_navigation::util
+ # 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/distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.cpp
new file mode 100644
index 00000000..6c52b133
--- /dev/null
+++ b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.cpp
@@ -0,0 +1,268 @@
+/**
+ * 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::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 <VirtualRobot/SceneObjectSet.h>
+
+#include "ArmarXCore/core/Component.h"
+#include "ArmarXCore/core/services/tasks/RunningTask.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h"
+
+#include "armarx/navigation/algorithms/CostmapBuilder.h"
+#include "armarx/navigation/util/util.h"
+
+
+namespace armarx::navigation::components::distance_to_obstacle_costmap_provider
+{
+ Component::Component()
+ {
+ addPlugin(virtualRobotReaderPlugin);
+ addPlugin(costmapWriterPlugin);
+ }
+
+
+ const std::string Component::defaultName = "distance_to_obstacle_costmap_provider";
+
+
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def =
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ def->optional(properties.robotName, "p.robotName", "Robot name.");
+
+ 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();
+ }
+ */
+
+ runningTask = new RunningTask<Component>(this, &Component::run, "createAndStoreCostmap");
+ runningTask->start();
+ }
+
+
+ void
+ Component::run()
+ {
+ // FIXME: whenever the static scene changes, update the costmap
+
+ createAndStoreCostmap();
+ }
+
+ bool
+ Component::createAndStoreCostmap()
+ {
+ auto robot = virtualRobotReaderPlugin->get().getRobot(
+ properties.robotName,
+ armarx::DateTime::Invalid(),
+ VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
+
+ const auto objectPoseClient = ObjectPoseClientPluginUser::getClient();
+
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient.fetchObjectPoses();
+
+ // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
+ const auto objectPosesStatic =
+ armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
+
+ const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
+
+ ARMARX_CHECK_NOT_NULL(objects);
+ ARMARX_INFO << objects->getSize() << " objects in the scene";
+
+ ARMARX_INFO << "Creating costmap";
+ ARMARX_CHECK_NOT_NULL(robot);
+
+ // FIXME: move costmap creation out of this component
+ // FIXME create costmap writer enum: type of costmaps
+ algorithms::CostmapBuilder costmapBuilder(
+ robot,
+ objects,
+ algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ "Platform-navigation-colmodel");
+
+ const auto costmap = costmapBuilder.create();
+
+ ARMARX_INFO << "Storing costmap in the memory.";
+ return costmapWriterPlugin->get().store(
+ costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
+ }
+
+
+ void
+ Component::onDisconnectComponent()
+ {
+ runningTask->stop();
+ }
+
+
+ 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::distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.h
new file mode 100644
index 00000000..bd6a9bee
--- /dev/null
+++ b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/Component.h
@@ -0,0 +1,152 @@
+/**
+ * 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::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/RunningTask.h"
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+#include "armarx/navigation/memory/client/costmap/Writer.h"
+#include <armarx/navigation/components/distance_to_obstacle_costmap_provider/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::distance_to_obstacle_costmap_provider
+{
+
+ class Component :
+ virtual public armarx::Component,
+ virtual public ObjectPoseClientPluginUser,
+ virtual public armarx::navigation::components::distance_to_obstacle_costmap_provider::
+ ComponentInterface
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ // , virtual public armarx::ArVizComponentPluginUser
+ {
+ 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;
+
+
+ /* (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;
+ */
+
+ bool createAndStoreCostmap();
+
+ void run();
+
+ 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 robotName = "Armar6";
+ };
+ 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;
+ */
+ std::optional<armem::robot::RobotDescription> robotDescription;
+ armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin = nullptr;
+
+
+ armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>*
+ costmapWriterPlugin = nullptr;
+
+
+ RunningTask<Component>::pointer_type runningTask;
+ };
+
+} // namespace armarx::navigation::components::distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/ComponentInterface.ice b/source/armarx/navigation/components/distance_to_obstacle_costmap_provider/ComponentInterface.ice
new file mode 100644
index 00000000..469a4290
--- /dev/null
+++ b/source/armarx/navigation/components/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::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 distance_to_obstacle_costmap_provider
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
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 fa926fb7..6b89eb56 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
@@ -84,6 +84,14 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// def->optional(
// properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
def->optional(properties.updatePeriodMs, "p.updatePeriodMs", "");
+ def->optional(properties.staticCostmap.name, "p.staticCostmap.name", "");
+ def->required(properties.staticCostmap.providerName, "p.staticCostmap.providerName", "");
+
+ def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
+ def->optional(properties.laserScannerFeatures.providerName, "p.laserScannerFeatures.providerName", "");
+
+ def->optional(properties.robot.name, "p.robot.name", "");
+
costmapReader.registerPropertyDefinitions(def);
costmapWriter.registerPropertyDefinitions(def);
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 6094a09e..1837d8ac 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
@@ -124,7 +124,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// std::string boxLayerName = "boxes";
struct
{
- std::string providerName = "navigator";
+ std::string providerName;
std::string name = "distance_to_obstacles";
} staticCostmap;
diff --git a/source/armarx/navigation/core/StaticScene.h b/source/armarx/navigation/core/StaticScene.h
index de0f008d..534b4345 100644
--- a/source/armarx/navigation/core/StaticScene.h
+++ b/source/armarx/navigation/core/StaticScene.h
@@ -35,8 +35,7 @@ namespace armarx::navigation::core
{
VirtualRobot::SceneObjectSetPtr objects;
- // TODO(fabian.reister): rename, why unique_ptr
- std::unique_ptr<algorithms::Costmap> costmap;
+ std::optional<algorithms::Costmap> distanceToObstaclesCostmap;
};
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index eafc77da..67db7790 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -159,7 +159,7 @@ namespace armarx::navigation::global_planning
ARMARX_TRACE;
// FIXME check if costmap is available
- algorithm::astar::AStarPlanner planner(*scene.staticScene->costmap);
+ algorithm::astar::AStarPlanner planner(*scene.staticScene->distanceToObstaclesCostmap);
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
index ed53c513..9491705a 100644
--- a/source/armarx/navigation/global_planning/SPFA.cpp
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -91,7 +91,7 @@ namespace armarx::navigation::global_planning
// FIXME check if costmap is available
algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams;
- algorithms::spfa::ShortestPathFasterAlgorithm planner(*scene.staticScene->costmap,
+ algorithms::spfa::ShortestPathFasterAlgorithm planner(*scene.staticScene->distanceToObstaclesCostmap,
spfaParams);
const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
@@ -211,14 +211,15 @@ namespace armarx::navigation::global_planning
// auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
// smoothTrajectory.setMaxVelocity(params.linearVelocity);
- const auto costmap = scene.staticScene->costmap.get();
+ ARMARX_CHECK(scene.staticScene->distanceToObstaclesCostmap.has_value());
+ const auto& costmap = scene.staticScene->distanceToObstaclesCostmap.value();
for (auto& point : result.trajectory->mutablePoints())
{
const float distance = std::min<float>(
spfaParams.obstacleMaxDistance,
- costmap->value(Eigen::Vector2f{point.waypoint.pose.translation().head<2>()})
+ costmap.value(Eigen::Vector2f{point.waypoint.pose.translation().head<2>()})
.value_or(0.F));
if (spfaParams.obstacleDistanceCosts)
diff --git a/source/armarx/navigation/memory/client/costmap/Reader.cpp b/source/armarx/navigation/memory/client/costmap/Reader.cpp
index 436fd2b9..ef078b77 100644
--- a/source/armarx/navigation/memory/client/costmap/Reader.cpp
+++ b/source/armarx/navigation/memory/client/costmap/Reader.cpp
@@ -96,7 +96,7 @@ namespace armarx::navigation::memory::client::costmap
if (not coreSegment.hasProviderSegment(query.providerName))
{
- ARMARX_WARNING << "Provider segment `" << query.providerName
+ ARMARX_VERBOSE << "Provider segment `" << query.providerName
<< "` does not exist (yet).";
return {.costmap = std::nullopt, .status = Result::Status::NoData};
}
@@ -106,7 +106,7 @@ namespace armarx::navigation::memory::client::costmap
if (providerSegment.empty())
{
- ARMARX_WARNING << "No entities.";
+ ARMARX_VERBOSE << "No entities.";
return {.costmap = std::nullopt,
.status = Result::Status::NoData,
.errorMessage = "No entities"};
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
index b51208e2..308cb088 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -3,10 +3,12 @@
#include <VirtualRobot/SceneObjectSet.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/time/Clock.h"
#include "RobotAPI/libraries/armem_robot/types.h"
#include "armarx/navigation/core/types.h"
+#include "armarx/navigation/memory/client/costmap/Reader.h"
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/util/util.h>
@@ -33,8 +35,8 @@ namespace armarx::navigation::server::scene_provider
config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
ARMARX_CHECK_NOT_NULL(scn.robot);
- scn.staticScene = getStaticScene(timestamp);
- scn.dynamicScene = getDynamicScene(timestamp);
+ scn.staticScene.emplace(getStaticScene(timestamp));
+ scn.dynamicScene.emplace(getDynamicScene(timestamp));
scn.graph = getSceneGraph(timestamp);
return true; // TODO(fabian.reister): return false if sync fails
@@ -77,28 +79,19 @@ namespace armarx::navigation::server::scene_provider
ARMARX_CHECK_NOT_NULL(objects);
ARMARX_INFO << objects->getSize() << " objects in the scene";
- ARMARX_INFO << "Creating costmap";
- ARMARX_CHECK_NOT_NULL(scn.robot);
-
- // FIXME: move costmap creation out of this component
- // FIXME create costmap writer enum: type of costmaps
- algorithms::CostmapBuilder costmapBuilder(
- scn.robot,
- objects,
- algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- "Platform-navigation-colmodel");
-
- const auto costmap = costmapBuilder.create();
-
- // ARMARX_INFO << "Storing costmap in memory";
- // costmapWriterPlugin->get().store(
- // costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
-
- ARMARX_INFO << "Done";
+ ARMARX_INFO << "Retrieving costmap in memory";
+ const memory::client::costmap::Reader::Query query
+ {
+ .providerName = "",
+ .name = "distance_to_obstacles",
+ .timestamp = armarx::Clock::Now()
+ };
ARMARX_TRACE;
+ const auto costmap = srv.costmapReader->query(query);
+ ARMARX_INFO << "Done";
- return {.objects = objects, .costmap = std::make_unique<algorithms::Costmap>(costmap)};
+ return {.objects = objects, .distanceToObstaclesCostmap = costmap.costmap};
}
core::DynamicScene
--
GitLab