diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 7090cb6ef73a6303e69290386e6b69b79b0a86ac..5c4f03377560d1d80e7ae636cbac84fd5e036fbd 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -58,6 +58,7 @@
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/elements/Color.h>
@@ -84,7 +85,6 @@
#include <armarx/navigation/factories/NavigationStackFactory.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/util/util.h>
@@ -117,7 +117,7 @@ namespace armarx::navigation::components
components::Navigator::Navigator() : parameterizationService(nullptr, nullptr)
// publisher(&resultsWriter, &eventsWriter)
{
- scene.timeServer = &timeServer;
+ // scene.timeServer = &timeServer;
addPlugin(parameterizationReaderPlugin);
addPlugin(parameterizationWriterPlugin);
@@ -163,7 +163,7 @@ namespace armarx::navigation::components
ARMARX_TRACE;
initializeScene();
- executor = server::PlatformUnitExecutor(platformUnit);
+ executor.emplace(server::PlatformControllerExecutor(getControlComponentPlugin()));
introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
@@ -172,12 +172,12 @@ namespace armarx::navigation::components
// TODO param (10)
- robotStateUpdateTask = new PeriodicTask<Navigator>(this,
- &Navigator::updateRobot,
- params.robotUpdatePeriodMs,
- false,
- "RobotStateUpdateTask");
- robotStateUpdateTask->start();
+ // robotStateUpdateTask = new PeriodicTask<Navigator>(this,
+ // &Navigator::updateRobot,
+ // params.robotUpdatePeriodMs,
+ // false,
+ // "RobotStateUpdateTask");
+ // robotStateUpdateTask->start();
navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
navRemoteGui->enable();
@@ -222,7 +222,7 @@ namespace armarx::navigation::components
{
ARMARX_TRACE;
auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
- params.robotName,
+ getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit()->getKinematicUnit()->getRobotName(),
armarx::core::time::Clock::Now(),
VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
true);
@@ -239,8 +239,6 @@ namespace armarx::navigation::components
scene.robot = getRobot();
{
- std::lock_guard g{scene.staticSceneMtx};
-
if (not scene.staticScene.has_value())
{
scene.staticScene = staticScene();
@@ -253,7 +251,6 @@ namespace armarx::navigation::components
// TODO dynamic scene
{
- std::lock_guard g{scene.graphMtx};
scene.graph = core::SceneGraph{.subgraphs = graphReaderPlugin->get().graphs()};
}
}
@@ -392,7 +389,7 @@ namespace armarx::navigation::components
navigators.at(configId).stop();
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{scene.timeServer->now()},
+ const core::UserAbortTriggeredEvent event{{timeServer.now()},
core::Pose(scene.robot->getGlobalPose())};
memoryPublishers.at(configId)->userAbortTriggered(event);
}
@@ -409,7 +406,7 @@ namespace armarx::navigation::components
}
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{scene.timeServer->now()},
+ const core::UserAbortTriggeredEvent event{{timeServer.now()},
core::Pose(scene.robot->getGlobalPose())};
for (auto& [callerId, memoryPublisher] : memoryPublishers)
{
@@ -449,14 +446,10 @@ namespace armarx::navigation::components
// def->topic<PlatformUnitListener>("MyTopic");
// Use (and depend on) another component (passing the ComponentInterfacePrx).
- def->component(platformUnit);
-
def->component(remoteGui, "RemoteGuiProvider");
// Add a required property.
// def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
- def->optional(params.robotName, "p.robotName");
-
// Add an optionalproperty.
def->optional(params.occupiedGridThreshold,
"p.occupancy_grid.occopied_threshold",
@@ -640,7 +633,7 @@ namespace armarx::navigation::components
ARMARX_TRACE;
const auto robotDescription =
- virtualRobotReader.queryDescription(params.robotName, timestamp);
+ virtualRobotReader.queryDescription(scene.robot->getType(), timestamp);
ARMARX_CHECK(robotDescription.has_value());
// synchronizing platform state
@@ -651,8 +644,6 @@ namespace armarx::navigation::components
// TODO / FIXME make this thread safe!
{
- std::lock_guard g{scene.platformVelocityMtx};
-
if (not scene.platformVelocity.has_value())
{
scene.platformVelocity = core::Twist();
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 6fa6459d03cf26b18d4735adff071787ed6e7ec7..618b378daf6ff0cc48b25651f3baca7fc340ea69 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -33,14 +33,15 @@
#include <Ice/Object.h>
+#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h"
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
-#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
@@ -50,6 +51,7 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include "armarx/navigation/memory/client/costmap/Writer.h"
+#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/components/Navigator/RemoteGui.h>
#include <armarx/navigation/core/time/ChronoMonotonicTimeServer.h>
@@ -59,8 +61,8 @@
#include <armarx/navigation/memory/client/stack_result/Writer.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
#include <armarx/navigation/server/introspection/ArvizIntrospector.h>
+#include <armarx/navigation/server/execution/PlatformControllerExecutor.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/server/parameterization/MemoryParameterizationService.h>
@@ -83,7 +85,8 @@ namespace armarx::navigation::components
virtual public Component,
virtual public client::NavigatorInterface,
virtual public ObjectPoseClientPluginUser,
- virtual public ArVizComponentPluginUser
+ virtual public ArVizComponentPluginUser,
+ virtual public armarx::control::client::ComponentPluginUser
// virtual public armem::ListeningClientPluginUser
{
@@ -163,16 +166,12 @@ namespace armarx::navigation::components
private:
void visualizeSPFA();
- // TODO update context periodically
-
- // TODO: Remove dependency to PlatformUnit. This component is generally
- // capable to also control drones or the like...
- PlatformUnitInterfacePrx platformUnit;
RemoteGuiInterfacePrx remoteGui;
core::Scene scene;
- std::optional<server::PlatformUnitExecutor> executor;
+
+ std::optional<server::PlatformControllerExecutor> executor;
std::optional<server::ArvizIntrospector> introspector;
std::unordered_map<std::string, server::Navigator> navigators;
@@ -214,8 +213,6 @@ namespace armarx::navigation::components
{
float occupiedGridThreshold{0.55F};
- std::string robotName = "Armar6";
-
int robotUpdatePeriodMs = 10;
};
diff --git a/source/armarx/navigation/components/Navigator/RemoteGui.cpp b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
index 2f8afd275ed36e8b8b850f0ba2a20ec9bce09ade..37a191ba49d9b2b1926f7d6a4e68feaf4c46d1c2 100644
--- a/source/armarx/navigation/components/Navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/Navigator/RemoteGui.cpp
@@ -23,7 +23,6 @@
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/server/Navigator.h>
-#include <armarx/navigation/server/execution/PlatformUnitExecutor.h>
#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
#include <armarx/navigation/util/util.h>