diff --git a/source/Navigation/components/Navigator/Navigator.cpp b/source/Navigation/components/Navigator/Navigator.cpp
index ab82d8a29927a0e782790d597ae77b2b8f076e93..bda158c830126f36e73698adcd63a2603386df5f 100644
--- a/source/Navigation/components/Navigator/Navigator.cpp
+++ b/source/Navigation/components/Navigator/Navigator.cpp
@@ -21,7 +21,13 @@
*/
#include "Navigator.h"
+
#include <iterator>
+
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_io.hpp>
+
#include <Eigen/src/Geometry/Transform.h>
#include "ArmarXCore/core/services/tasks/TaskUtil.h"
@@ -83,55 +89,49 @@ namespace armarx::nav::components
return p;
}
- void Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
- const aron::data::AronDictPtr& stackConfig,
- const std::string& navigationMode,
- const Ice::Current&)
+ std::string
+ Navigator::createConfig(const aron::data::AronDictPtr& stackConfig,
+ const Ice::Current&)
{
+ // TODO: Not thread-safe.
+ const std::string uuid = boost::uuids::to_string(boost::uuids::random_generator()());
+
server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+ navigators.emplace(uuid, server::Navigator{stack, scene, executor});
- if (navigator.has_value())
- {
- navigator->stop();
- navigator.reset();
- }
+ return uuid;
+ }
- if (not navigator.has_value())
- {
- navigator.emplace(stack, scene, executor);
- }
+ void Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+ const std::string& navigationMode,
+ const std::string& configId,
+ const Ice::Current&)
+ {
+ // TODO: Error handling.
- navigator->moveTo(convert(waypoints), core::NavigationFramesMap.from_name(navigationMode));
+ navigators.at(configId).moveTo(convert(waypoints), core::NavigationFramesMap.from_name(navigationMode));
}
void Navigator::moveTowards(const Eigen::Vector3f& direction,
- const aron::data::AronDictPtr& stackConfig,
const std::string& navigationMode,
+ const std::string& configId,
const Ice::Current&)
{
- server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+ // TODO: Error handling.
- if (navigator.has_value())
- {
- navigator->stop();
- navigator.reset();
- }
+ navigators.at(configId).moveTowards(direction, core::NavigationFramesMap.from_name(navigationMode));
+ }
- if (not navigator.has_value())
- {
- navigator.emplace(stack, scene, executor);
- }
+ void
+ Navigator::pauseMovement(const Ice::Current&)
+ {
- navigator->moveTowards(direction, core::NavigationFramesMap.from_name(navigationMode));
}
- void Navigator::stop(const Ice::Current&)
+ void
+ Navigator::resumeMovement(const Ice::Current&)
{
- if (navigator.has_value())
- {
- navigator->stop();
- navigator.reset();
- }
+
}
armarx::PropertyDefinitionsPtr Navigator::createPropertyDefinitions()
diff --git a/source/Navigation/components/Navigator/Navigator.h b/source/Navigation/components/Navigator/Navigator.h
index 4ec574d3a61311bfc40fd8ae56d0f5c155982eda..a22d4de00766b499cebb59ff16db3fa695fb6c2b 100644
--- a/source/Navigation/components/Navigator/Navigator.h
+++ b/source/Navigation/components/Navigator/Navigator.h
@@ -75,17 +75,22 @@ namespace armarx::nav::components
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+ std::string createConfig(const aron::data::AronDictPtr& stackConfig,
+ const Ice::Current&) override;
+
void moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
- const aron::data::AronDictPtr& config,
const std::string& navigationMode,
+ const std::string& configId,
const Ice::Current&) override;
void moveTowards(const Eigen::Vector3f& direction,
- const aron::data::AronDictPtr& config,
const std::string& navigationMode,
+ const std::string& configId,
const Ice::Current&) override;
- void stop(const Ice::Current&) override;
+ void pauseMovement(const Ice::Current&) override;
+
+ void resumeMovement(const Ice::Current&) override;
protected:
/// @see PropertyUser::createPropertyDefinitions()
@@ -118,7 +123,7 @@ namespace armarx::nav::components
core::Scene scene;
server::PlatformUnitExecutor executor;
- std::optional<server::Navigator> navigator;
+ std::map<std::string, server::Navigator> navigators;
};
} // namespace armarx::nav::components
diff --git a/source/Navigation/components/Navigator/NavigatorInterface.ice b/source/Navigation/components/Navigator/NavigatorInterface.ice
index 2a5ac274039db6211c2b508625628b60f6edb2f4..f292d927239034a7145768908580459acb67e1ca 100644
--- a/source/Navigation/components/Navigator/NavigatorInterface.ice
+++ b/source/Navigation/components/Navigator/NavigatorInterface.ice
@@ -38,9 +38,11 @@ module armarx
interface NavigatorInterface
{
- void moveTo(Eigen::Matrix4fSeq waypoints, aron::data::AronDict config, string navigationFrame);
- void moveTowards(Eigen::Vector3f direction, aron::data::AronDict config, string navigationFrame);
- idempotent void stop();
+ string createConfig(aron::data::AronDict config);
+ void moveTo(Eigen::Matrix4fSeq waypoints, string navigationFrame, string configId);
+ void moveTowards(Eigen::Vector3f direction, string navigationFrame, string configId);
+ idempotent void pauseMovement();
+ idempotent void resumeMovement();
};
};
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index 1fa419999752ca7f3efb99031750bc7e373633e0..da9c759445f807965e78bc55e699700a1a2f5447 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -40,7 +40,7 @@ namespace armarx::nav::server
Navigator::Navigator(const server::NavigationStack& navigationStack,
const core::Scene& scene,
ExecutorInterface& executor) :
- stack(navigationStack), scene(scene), executor{executor}
+ stack(navigationStack), scene(&scene), executor{&executor}
{
setTag("Navigator");
}
@@ -62,7 +62,7 @@ namespace armarx::nav::server
globalWaypoints.reserve(waypoints.size());
std::transform(std::begin(waypoints), std::end(waypoints), std::back_inserter(globalWaypoints), [&](const core::Pose & p)
{
- return core::Pose(scene.robot->getGlobalPose()) * p;
+ return core::Pose(scene->robot->getGlobalPose()) * p;
});
break;
}
@@ -99,7 +99,7 @@ namespace armarx::nav::server
res.safeVelocity = stack.safetyControl->control(res.controlVelocity.value());
}
- executor.move(res.velocity());
+ executor->move(res.velocity());
}
}
diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h
index 499a46259956081a12c44cbe848653a923def274..091aa639f4bbb0e0bcdc3f90f135f66e28d5e114 100644
--- a/source/Navigation/libraries/server/Navigator.h
+++ b/source/Navigation/libraries/server/Navigator.h
@@ -49,7 +49,9 @@ namespace armarx::nav::server
class Navigator : public armarx::Logging
{
+
public:
+
Navigator(const server::NavigationStack& navigationStack, const core::Scene& scene, ExecutorInterface& executor);
virtual ~Navigator();
@@ -61,11 +63,10 @@ namespace armarx::nav::server
void stop();
- protected:
private:
server::NavigationStack stack;
- const core::Scene& scene;
+ const core::Scene* scene;
+ ExecutorInterface* executor;
- ExecutorInterface& executor;
};
} // namespace armarx::nav::server
diff --git a/source/Navigation/libraries/server/execution/PlatformUnitExecutor.cpp b/source/Navigation/libraries/server/execution/PlatformUnitExecutor.cpp
index be4e3d0fabcbc8209cb4a6d136515ef82ae4cef5..9e2efb088e7777168feee03315ec08f80e4fed4c 100644
--- a/source/Navigation/libraries/server/execution/PlatformUnitExecutor.cpp
+++ b/source/Navigation/libraries/server/execution/PlatformUnitExecutor.cpp
@@ -16,21 +16,7 @@ namespace armarx::nav::server
void PlatformUnitExecutor::move(const core::Twist& twist)
{
- if (enabled)
- {
- platformUnit->move(twist.linear.x(), twist.linear.y(), twist.angular.z());
- }
- }
-
- void PlatformUnitExecutor::disableAndStop()
- {
- enabled = false;
- platformUnit->stopPlatform();
- }
-
- void PlatformUnitExecutor::enable()
- {
- enabled = true;
+ platformUnit->move(twist.linear.x(), twist.linear.y(), twist.angular.z());
}
} // namespace armarx::nav::server
diff --git a/source/Navigation/libraries/server/execution/PlatformUnitExecutor.h b/source/Navigation/libraries/server/execution/PlatformUnitExecutor.h
index bccc7a91337d341f2f9699daa8ef4a757a3a8087..91f9cc78402204f41636ffb2e279928076cecde3 100644
--- a/source/Navigation/libraries/server/execution/PlatformUnitExecutor.h
+++ b/source/Navigation/libraries/server/execution/PlatformUnitExecutor.h
@@ -22,15 +22,10 @@ namespace armarx::nav::server
void move(const core::Twist& twist) override;
- void disableAndStop();
- void enable();
-
private:
PlatformUnitInterfacePrx platformUnit;
- std::atomic_bool enabled{true};
-
};
} // namespace armarx::nav::server