diff --git a/source/Navigation/components/Navigator/CMakeLists.txt b/source/Navigation/components/Navigator/CMakeLists.txt
index f876b2f18ebedc460b65deeef70e37548194b357..fd803d7b2d32d908b742a00b184babe3d2f58ef4 100644
--- a/source/Navigation/components/Navigator/CMakeLists.txt
+++ b/source/Navigation/components/Navigator/CMakeLists.txt
@@ -24,7 +24,8 @@ armarx_add_component(
# RobotAPI
RobotAPICore
RobotAPIInterfaces
- ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotAPIArmarXObjects
+ RobotAPIComponentPlugins # For ArViz and other plugins.
# This project
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
@@ -32,6 +33,7 @@ armarx_add_component(
NavigatorInterfaces
Navigation::server
+ Navigation::util
Navigation::factories
SOURCES
diff --git a/source/Navigation/components/Navigator/Navigator.cpp b/source/Navigation/components/Navigator/Navigator.cpp
index 83ccf9fbc61c718bcf6ec2153133e08b9cbdd0f0..157fef079711e741470cab9e40f857271e1a97ae 100644
--- a/source/Navigation/components/Navigator/Navigator.cpp
+++ b/source/Navigation/components/Navigator/Navigator.cpp
@@ -21,8 +21,13 @@
*/
#include "Navigator.h"
+#include <iterator>
+#include <Eigen/src/Geometry/Transform.h>
+
#include "ArmarXCore/core/services/tasks/TaskUtil.h"
+#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
+
// Navigation
#include "Navigation/libraries/core/types.h"
#include "Navigation/libraries/util/util.h"
@@ -32,8 +37,7 @@
namespace armarx::nav::components
{
- Navigator::Navigator() :
- executor{platformUnit}
+ Navigator::Navigator() : executor{platformUnit}
{
}
@@ -68,6 +72,17 @@ namespace armarx::nav::components
return "Navigator";
}
+ std::vector<core::Pose> convert(const std::vector<Eigen::Matrix4f>& wps)
+ {
+ std::vector<core::Pose> p;
+ p.reserve(wps.size());
+ std::transform(wps.begin(), wps.end(), std::back_inserter(p), [](const auto & p)
+ {
+ return Eigen::Affine3f(p);
+ });
+ return p;
+ }
+
void Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
const aron::data::AronDictPtr& stackConfig,
const std::string& navigationMode,
@@ -78,7 +93,7 @@ namespace armarx::nav::components
server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
server::Navigator navigator{stack, scene, executor};
- navigator.moveTo(waypoints, core::NavigationFramesMap.from_name(navigationMode));
+ navigator.moveTo(convert(waypoints), core::NavigationFramesMap.from_name(navigationMode));
}
void Navigator::moveTowards(const Eigen::Vector3f& direction,
diff --git a/source/Navigation/components/Navigator/Navigator.h b/source/Navigation/components/Navigator/Navigator.h
index 04c7879f625da33fed12c49825be75718d67d86f..95ee26e2cc2492682852bfdc3c3cc8362aa6673b 100644
--- a/source/Navigation/components/Navigator/Navigator.h
+++ b/source/Navigation/components/Navigator/Navigator.h
@@ -111,11 +111,12 @@ namespace armarx::nav::components
private:
// TODO update context periodically
- core::Scene scene;
+ PlatformUnitInterfacePrx platformUnit;
+
+ core::Scene scene;
server::PlatformUnitExecutor executor;
- PlatformUnitListenerPrx platformUnit;
};
} // namespace armarx::nav::components
diff --git a/source/Navigation/libraries/core/types.h b/source/Navigation/libraries/core/types.h
index faf02bb82208f8b511edfd57cd93476b49f6c5b7..b00682034dd561e7edce9141fb8303c04aedf523 100644
--- a/source/Navigation/libraries/core/types.h
+++ b/source/Navigation/libraries/core/types.h
@@ -46,7 +46,9 @@ namespace armarx::nav::core
{NavigationFrames::Absolute, "Absolute"}};
using Pose = Eigen::Affine3f;
-
+
+ using Direction = Eigen::Vector3f;
+
struct Twist
{
Eigen::Vector3f linear;
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index bf2be7607ea3b91db3efb35df210181e92bd4341..7b77b759c05264ac6f060d0aebaade02ae666070 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/source/Navigation/libraries/server/Navigator.cpp
@@ -1,6 +1,7 @@
#include "Navigator.h"
#include <algorithm>
+#include <Eigen/src/Geometry/Transform.h>
#include <VirtualRobot/Robot.h>
@@ -44,18 +45,19 @@ namespace armarx::nav::server
setTag("Navigator");
}
- void Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints, const core::NavigationFrames& navigationFrame)
+ void Navigator::moveTo(const std::vector<core::Pose>& waypoints, const core::NavigationFrames& navigationFrame)
{
- std::vector<Eigen::Matrix4f> globalWaypoints;
+ std::vector<core::Pose> globalWaypoints;
switch (navigationFrame)
{
case core::NavigationFrames::Absolute:
globalWaypoints = waypoints;
break;
case core::NavigationFrames::Relative:
- std::transform(std::begin(waypoints), std::end(waypoints), std::begin(globalWaypoints), [&](const Eigen::Matrix4f & m)
+ globalWaypoints.reserve(waypoints.size());
+ std::transform(std::begin(waypoints), std::end(waypoints), std::back_inserter(globalWaypoints), [&](const core::Pose & p)
{
- return scene.robot->getGlobalPose() * m;
+ return core::Pose(scene.robot->getGlobalPose()) * p;
});
break;
}
@@ -63,13 +65,13 @@ namespace armarx::nav::server
moveToAbsolute(globalWaypoints);
}
- void Navigator::moveToAbsolute(const std::vector<Eigen::Matrix4f>& waypoints)
+ void Navigator::moveToAbsolute(const std::vector<core::Pose>& waypoints)
{
ARMARX_INFO << "Planning global trajectory";
ARMARX_CHECK_NOT_NULL(stack.globalPlanner);
StackResult res;
- res.globalTrajectory = stack.globalPlanner->plan(waypoints.at(0));
+ res.globalTrajectory = stack.globalPlanner->plan(Eigen::Affine3f(waypoints.at(0)));
if (stack.localPlanner)
{
@@ -96,12 +98,12 @@ namespace armarx::nav::server
}
}
- void Navigator::moveTowards(const Eigen::Vector3f direction, const core::NavigationFrames& navigationFrame)
+ void Navigator::moveTowards(const core::Direction& direction, const core::NavigationFrames& navigationFrame)
{
}
- void Navigator::moveTowardsAbsolute(const Eigen::Vector3f direction)
+ void Navigator::moveTowardsAbsolute(const core::Direction& direction)
{
}
diff --git a/source/Navigation/libraries/server/Navigator.h b/source/Navigation/libraries/server/Navigator.h
index 6ae203bf31b50d1f006853f758fece9a02b836f1..4e07f21b550c839a4d657e023a2da6f3b6d0dd8f 100644
--- a/source/Navigation/libraries/server/Navigator.h
+++ b/source/Navigation/libraries/server/Navigator.h
@@ -52,11 +52,11 @@ namespace armarx::nav::server
public:
Navigator(const server::NavigationStack& navigationStack, const core::Scene& scene, ExecutorInterface& executor);
- void moveTo(const std::vector<Eigen::Matrix4f>& waypoints, const core::NavigationFrames& navigationFrame);
- void moveToAbsolute(const std::vector<Eigen::Matrix4f>& waypoints);
+ void moveTo(const std::vector<core::Pose>& waypoints, const core::NavigationFrames& navigationFrame);
+ void moveToAbsolute(const std::vector<core::Pose>& waypoints);
- void moveTowards(const Eigen::Vector3f direction, const core::NavigationFrames& navigationFrame);
- void moveTowardsAbsolute(const Eigen::Vector3f direction);
+ void moveTowards(const core::Direction& direction, const core::NavigationFrames& navigationFrame);
+ void moveTowardsAbsolute(const core::Direction& direction);
void stop();
diff --git a/source/Navigation/libraries/server/test/serverTest.cpp b/source/Navigation/libraries/server/test/serverTest.cpp
index 86da740623a8e88f9aa363221b72d9a3827e1b14..61047bd4dcd194c32953040b54875058a7638968 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(std::vector{goal}, core::NavigationFrames::Absolute);
BOOST_CHECK_EQUAL(true, true);
}