From 8d05593c6e78b34682104cc5b96ef3c74489fb5d Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 28 Jun 2022 22:52:19 +0200
Subject: [PATCH] scene provider /updater
---
.../server/scene_provider/SceneProvider.cpp | 104 ++++++++++++++++++
.../server/scene_provider/SceneProvider.h | 84 ++++++++++++++
.../scene_provider/SceneProviderInterface.h | 46 ++++++++
3 files changed, 234 insertions(+)
create mode 100644 source/armarx/navigation/server/scene_provider/SceneProvider.cpp
create mode 100644 source/armarx/navigation/server/scene_provider/SceneProvider.h
create mode 100644 source/armarx/navigation/server/scene_provider/SceneProviderInterface.h
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
new file mode 100644
index 00000000..04412bb3
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -0,0 +1,104 @@
+#include "SceneProvider.h"
+
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include <armarx/navigation/algorithms/CostmapBuilder.h>
+#include <armarx/navigation/util/util.h>
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ SceneProvider::SceneProvider(const InjectedServices& srv, const Config& config) :
+ srv(srv), config(config)
+ {
+ }
+
+ const core::Scene&
+ SceneProvider::scene() const
+ {
+ return scn;
+ }
+
+ bool
+ SceneProvider::initialize(const DateTime& timestamp)
+ {
+ scn.timestamp = timestamp;
+
+ scn.robot = srv.virtualRobotReader->getRobot(
+ config.robotName, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ scn.staticScene = getStaticScene(timestamp);
+ scn.dynamicScene = getDynamicScene(timestamp);
+ scn.graph = getSceneGraph(timestamp);
+
+ return true; // TODO(fabian.reister): return false if sync fails
+ }
+
+ bool
+ SceneProvider::synchronize(const DateTime& timestamp)
+ {
+ scn.timestamp = timestamp;
+
+ ARMARX_CHECK_NOT_NULL(srv.virtualRobotReader);
+ srv.virtualRobotReader->synchronizeRobot(*scn.robot, timestamp);
+
+ scn.dynamicScene = getDynamicScene(timestamp);
+ scn.graph = getSceneGraph(timestamp);
+
+ return true; // TODO(fabian.reister): return false if sync fails
+ }
+
+ core::StaticScene
+ SceneProvider::getStaticScene(const DateTime& timestamp) const
+ {
+ ARMARX_TRACE;
+
+ const objpose::ObjectPoseSeq objectPoses = srv.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(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_TRACE;
+
+ return {.objects = objects, .costmap = std::make_unique<algorithms::Costmap>(costmap)};
+ }
+
+ core::DynamicScene
+ SceneProvider::getDynamicScene(const DateTime& timestamp) const
+ {
+ return {}; // FIXME implement
+ }
+
+ core::SceneGraph
+ SceneProvider::getSceneGraph(const DateTime& /*timestamp*/) const
+ {
+ ARMARX_CHECK_NOT_NULL(srv.graphReader);
+ return {.subgraphs = srv.graphReader->graphs()};
+ }
+
+} // namespace armarx::navigation::server::scene_provider
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.h b/source/armarx/navigation/server/scene_provider/SceneProvider.h
new file mode 100644
index 00000000..cd93e072
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.h
@@ -0,0 +1,84 @@
+/**
+ * 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/>.
+ *
+ * @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 <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+#include <armarx/navigation/core/DynamicScene.h>
+#include <armarx/navigation/core/StaticScene.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/memory/client/graph/Reader.h>
+#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
+
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ class SceneProvider : virtual public SceneProviderInterface
+ {
+ public:
+ struct InjectedServices
+ {
+
+ memory::client::graph::Reader* graphReader;
+
+ memory::client::costmap::Reader* costmapReader;
+ // armem::vision::occupancy_grid::client::Reader occupancyGridReader;
+
+ // `robot_state` memory reader and writer
+ armem::robot_state::VirtualRobotReader* virtualRobotReader;
+
+ objpose::ObjectPoseClient objectPoseClient;
+ };
+
+ struct Config
+ {
+ std::string robotName;
+ };
+
+ SceneProvider(const InjectedServices& srv, const Config& config);
+
+ const core::Scene& scene() const override;
+
+ bool initialize(const DateTime& timestamp) override;
+ bool synchronize(const DateTime& timestamp) override;
+
+
+ private:
+ VirtualRobot::RobotPtr getRobot(const DateTime& timestamp) const;
+ core::StaticScene getStaticScene(const DateTime& timestamp) const;
+ core::DynamicScene getDynamicScene(const DateTime& timestamp) const;
+ core::SceneGraph getSceneGraph(const DateTime& timestamp) const;
+
+ InjectedServices srv;
+ Config config;
+
+ core::Scene scn;
+ };
+} // namespace armarx::navigation::server::scene_provider
diff --git a/source/armarx/navigation/server/scene_provider/SceneProviderInterface.h b/source/armarx/navigation/server/scene_provider/SceneProviderInterface.h
new file mode 100644
index 00000000..10869aab
--- /dev/null
+++ b/source/armarx/navigation/server/scene_provider/SceneProviderInterface.h
@@ -0,0 +1,46 @@
+/**
+ * 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/>.
+ *
+ * @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/time/DateTime.h>
+
+namespace armarx::navigation::core
+{
+ struct Scene;
+}
+
+namespace armarx::navigation::server::scene_provider
+{
+
+ class SceneProviderInterface
+ {
+ public:
+ virtual const core::Scene& scene() const = 0;
+
+ virtual bool initialize(const DateTime& timestamp) = 0;
+ virtual bool synchronize(const DateTime& timestamp) = 0;
+
+
+ // non-api
+ virtual ~SceneProviderInterface() = default;
+ };
+} // namespace armarx::navigation::server::scene_provider
--
GitLab