diff --git a/source/Navigation/components/Navigator/Navigator.cpp b/source/Navigation/components/Navigator/Navigator.cpp
index b941324fe1d2264b503cea1d845a3e81603f844c..83ccf9fbc61c718bcf6ec2153133e08b9cbdd0f0 100644
--- a/source/Navigation/components/Navigator/Navigator.cpp
+++ b/source/Navigation/components/Navigator/Navigator.cpp
@@ -32,6 +32,11 @@
namespace armarx::nav::components
{
+ Navigator::Navigator() :
+ executor{platformUnit}
+ {
+ }
+
Navigator::~Navigator()
{
}
@@ -51,11 +56,11 @@ namespace armarx::nav::components
void Navigator::onExitComponent()
{
}
-
- void Navigator::updateContext()
+
+ void Navigator::updateContext()
{
- ctx.staticScene = staticScene();
- ctx.robot = getRobot();
+ scene.staticScene = staticScene();
+ scene.robot = getRobot();
}
std::string Navigator::getDefaultName() const
@@ -63,34 +68,34 @@ namespace armarx::nav::components
return "Navigator";
}
- void Navigator::moveTo(const Eigen::Matrix4f& pose,
+ void Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
const aron::data::AronDictPtr& stackConfig,
const std::string& navigationMode,
const Ice::Current&)
{
updateContext();
- server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
- server::Navigator navigator{stack, ctx, executor};
-
- SimplePeriodicTask<> task(
- [](){navigator.updateContext()}
- );
- task.start()
-
+ server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+ server::Navigator navigator{stack, scene, executor};
- navigator.moveTo(core::Pose(pose), core::NavigationFramesMap.from_name(navigationMode));
+ navigator.moveTo(waypoints, core::NavigationFramesMap.from_name(navigationMode));
}
- void Navigator::moveTowards(const Eigen::Matrix4f& direction,
+ void Navigator::moveTowards(const Eigen::Vector3f& direction,
const aron::data::AronDictPtr& stackConfig,
const std::string& navigationMode,
const Ice::Current&)
{
- server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
- server::Navigator navigator{stack, ctx};
+ server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+ server::Navigator navigator{stack, scene, executor};
- // navigator.moveTowards(direction); TODO
+ navigator.moveTowards(direction, core::NavigationFramesMap.from_name(navigationMode));
+ }
+
+ void Navigator::stop(const Ice::Current&)
+ {
+ //server::Navigator navigator;
+ //navigator.stop();
}
armarx::PropertyDefinitionsPtr Navigator::createPropertyDefinitions()
@@ -116,22 +121,21 @@ namespace armarx::nav::components
return def;
}
- core::StaticScenePtr Navigator::staticScene()
+ core::StaticScene Navigator::staticScene()
{
- core::StaticScenePtr scene(new core::StaticScene);
+ core::StaticScene scene;
objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
- scene->objects = util::asSceneObjects(objectPoses);
+ scene.objects = util::asSceneObjects(objectPoses);
return scene;
}
-
- VirtualRobot::RobotPtr Navigator::getRobot()
- {
+ VirtualRobot::RobotPtr Navigator::getRobot()
+ {
auto robot = RemoteRobot::createLocalCloneFromFile(
- getRobotStateComponent(), VirtualRobot::RobotIO::RobotDescription::eFull);
+ getRobotStateComponent(), VirtualRobot::RobotIO::RobotDescription::eFull);
// auto robot = RemoteRobot::createLocalClone(getRobotStateComponent());
ARMARX_CHECK_NOT_NULL(robot);
diff --git a/source/Navigation/components/Navigator/Navigator.h b/source/Navigation/components/Navigator/Navigator.h
index 84f41c1728bd6085e7be87cf9dfe7313c28dbed2..04c7879f625da33fed12c49825be75718d67d86f 100644
--- a/source/Navigation/components/Navigator/Navigator.h
+++ b/source/Navigation/components/Navigator/Navigator.h
@@ -26,6 +26,7 @@
// STD/STL
#include <string>
+#include <vector>
// Eigen
#include <Eigen/Core>
@@ -39,6 +40,7 @@
// Navigation
#include "Navigation/libraries/core/StaticScene.h"
#include "Navigation/libraries/core/types.h"
+#include <Navigation/libraries/server/execution/PlatformUnitExecutor.h>
#include <Navigation/components/Navigator/NavigatorInterface.h>
@@ -64,21 +66,25 @@ namespace armarx::nav::components
{
public:
+
+ Navigator();
~Navigator() override;
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- void moveTo(const Eigen::Matrix4f& pose,
+ void moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
const aron::data::AronDictPtr& config,
const std::string& navigationMode,
const Ice::Current&) override;
- void moveTowards(const Eigen::Matrix4f& direction,
+ void moveTowards(const Eigen::Vector3f& direction,
const aron::data::AronDictPtr& config,
const std::string& navigationMode,
const Ice::Current&) override;
+ void stop(const Ice::Current&) override;
+
protected:
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -97,7 +103,7 @@ namespace armarx::nav::components
void updateContext();
- core::StaticScenePtr staticScene();
+ core::StaticScene staticScene();
VirtualRobot::RobotPtr getRobot();
@@ -105,9 +111,12 @@ namespace armarx::nav::components
private:
// TODO update context periodically
- core::Scene ctx;
+ core::Scene scene;
+
+ server::PlatformUnitExecutor executor;
PlatformUnitListenerPrx platformUnit;
+
};
} // namespace armarx::nav::components
diff --git a/source/Navigation/components/Navigator/NavigatorInterface.ice b/source/Navigation/components/Navigator/NavigatorInterface.ice
index 94c689e6957f97b5da93d9a20ddec933b3f2c15f..2a5ac274039db6211c2b508625628b60f6edb2f4 100644
--- a/source/Navigation/components/Navigator/NavigatorInterface.ice
+++ b/source/Navigation/components/Navigator/NavigatorInterface.ice
@@ -40,6 +40,7 @@ module armarx
{
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();
};
};
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index 749ad8f3195e5f544e6e25532257f1527a78cee8..bf2be7607ea3b91db3efb35df210181e92bd4341 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -1,5 +1,7 @@
#include "Navigator.h"
+#include <algorithm>
+
#include <VirtualRobot/Robot.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
@@ -42,29 +44,32 @@ namespace armarx::nav::server
setTag("Navigator");
}
- void Navigator::moveTo(const core::Pose& goal, const core::NavigationFrames& navigationFrame)
+ void Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints, const core::NavigationFrames& navigationFrame)
{
- core::Pose globalPose;
+ std::vector<Eigen::Matrix4f> globalWaypoints;
switch (navigationFrame)
{
case core::NavigationFrames::Absolute:
- globalPose = goal;
+ globalWaypoints = waypoints;
break;
case core::NavigationFrames::Relative:
- globalPose = core::Pose(scene.robot->getGlobalPose()) * goal;
+ std::transform(std::begin(waypoints), std::end(waypoints), std::begin(globalWaypoints), [&](const Eigen::Matrix4f & m)
+ {
+ return scene.robot->getGlobalPose() * m;
+ });
break;
}
- moveToAbsolute(globalPose);
+ moveToAbsolute(globalWaypoints);
}
- void Navigator::moveToAbsolute(const core::Pose& goal)
+ void Navigator::moveToAbsolute(const std::vector<Eigen::Matrix4f>& waypoints)
{
ARMARX_INFO << "Planning global trajectory";
ARMARX_CHECK_NOT_NULL(stack.globalPlanner);
StackResult res;
- res.globalTrajectory = stack.globalPlanner->plan(goal);
+ res.globalTrajectory = stack.globalPlanner->plan(waypoints.at(0));
if (stack.localPlanner)
{
@@ -90,4 +95,20 @@ namespace armarx::nav::server
executor.move(res.velocity());
}
}
+
+ void Navigator::moveTowards(const Eigen::Vector3f direction, const core::NavigationFrames& navigationFrame)
+ {
+
+ }
+
+ void Navigator::moveTowardsAbsolute(const Eigen::Vector3f direction)
+ {
+
+ }
+
+ void Navigator::stop()
+ {
+
+ }
+
} // namespace armarx::nav::server
diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h
index 848daae375654212fbad448d03b9b9cbac662b10..6ae203bf31b50d1f006853f758fece9a02b836f1 100644
--- a/source/Navigation/libraries/server/Navigator.h
+++ b/source/Navigation/libraries/server/Navigator.h
@@ -52,8 +52,13 @@ namespace armarx::nav::server
public:
Navigator(const server::NavigationStack& navigationStack, const core::Scene& scene, ExecutorInterface& executor);
- void moveTo(const core::Pose& goal, const core::NavigationFrames& navigationFrame);
- void moveToAbsolute(const core::Pose& goal);
+ void moveTo(const std::vector<Eigen::Matrix4f>& waypoints, const core::NavigationFrames& navigationFrame);
+ void moveToAbsolute(const std::vector<Eigen::Matrix4f>& waypoints);
+
+ void moveTowards(const Eigen::Vector3f direction, const core::NavigationFrames& navigationFrame);
+ void moveTowardsAbsolute(const Eigen::Vector3f direction);
+
+ void stop();
protected:
private:
diff --git a/source/Navigation/libraries/server/test/serverTest.cpp b/source/Navigation/libraries/server/test/serverTest.cpp
index 6695d314a095931b6d961e2a00c1843bb196172b..86da740623a8e88f9aa363221b72d9a3827e1b14 100644
--- a/source/Navigation/libraries/server/test/serverTest.cpp
+++ b/source/Navigation/libraries/server/test/serverTest.cpp
@@ -65,7 +65,7 @@ BOOST_AUTO_TEST_CASE(testNavigator)
server::DummyExecutor executor{scene.robot, server::DummyExecutor::Params()};
server::Navigator navigator(stack, scene, executor);
- navigator.moveTo(goal, core::NavigationFrames::Absolute);
+ navigator.moveTo({goal}, core::NavigationFrames::Absolute);
BOOST_CHECK_EQUAL(true, true);
}