diff --git a/source/armarx/navigation/factories/test/factoriesTest.cpp b/source/armarx/navigation/factories/test/factoriesTest.cpp
index 2e7d7fdbefdf5ebc2048dc96704d14885f6ca8f8..587ca5051d0cb50eb37dd3549cca0771b46a8e69 100644
--- a/source/armarx/navigation/factories/test/factoriesTest.cpp
+++ b/source/armarx/navigation/factories/test/factoriesTest.cpp
@@ -64,10 +64,10 @@ BOOST_AUTO_TEST_CASE(testStackCreation)
const auto navStack = armarx::navigation::fac::NavigationStackFactory::create(dto, scene);
BOOST_CHECK(navStack.globalPlanner.get() != nullptr);
- BOOST_CHECK(navStack.trajectoryControl.get() != nullptr);
+ // BOOST_CHECK(navStack.trajectoryControl.get() != nullptr);
BOOST_CHECK(dynamic_cast<armarx::navigation::global_planning::AStar*>(
navStack.globalPlanner.get()) != nullptr);
- BOOST_CHECK(dynamic_cast<armarx::navigation::traj_ctrl::TrajectoryFollowingController*>(
- navStack.trajectoryControl.get()) != nullptr);
+ // BOOST_CHECK(dynamic_cast<armarx::navigation::traj_ctrl::TrajectoryFollowingController*>(
+ // navStack.trajectoryControl.get()) != nullptr);
}
diff --git a/source/armarx/navigation/server/execution/DummyExecutor.h b/source/armarx/navigation/server/execution/DummyExecutor.h
index ae5e40c6327f7fde24b393271b0697200690dfc8..3a9d25a9fc19645719a5a1df42b7c2587cf2618f 100644
--- a/source/armarx/navigation/server/execution/DummyExecutor.h
+++ b/source/armarx/navigation/server/execution/DummyExecutor.h
@@ -33,26 +33,16 @@ namespace armarx::navigation::server
}
void
- move(const core::Twist& twist) override
+ execute(const core::Trajectory& trajectory) override
{
- this->twist = twist;
- //const auto now = std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now());
-
- //const float dt = now - lastUpdated;
-
- //lastUpdated = now;
+ }
- const core::Pose diff; // TODO
+ void start() override{};
+ void stop() override{};
- robot->setGlobalPose(robot->getGlobalPose() * diff.matrix());
- }
private:
VirtualRobot::RobotPtr robot;
-
- core::Twist twist;
-
- std::chrono::microseconds lastUpdated;
};
} // namespace armarx::navigation::server
diff --git a/source/armarx/navigation/server/test/serverTest.cpp b/source/armarx/navigation/server/test/serverTest.cpp
index 5fb6ce9232b61ea5dcbfb3d0709a0c87958809dd..d7ba7e7f2643f33365b14c8554034ab36ed02bb5 100644
--- a/source/armarx/navigation/server/test/serverTest.cpp
+++ b/source/armarx/navigation/server/test/serverTest.cpp
@@ -21,6 +21,7 @@
* GNU General Public License
*/
+#include "armarx/navigation/server/scene_provider/SceneProvider.h"
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/services/SimpleEventHandler.h>
#include <armarx/navigation/core/types.h>
@@ -44,6 +45,42 @@
using namespace armarx::navigation;
+namespace armarx::navigation::server::scene_provider
+{
+
+ class DummySceneProvider : public SceneProviderInterface
+ {
+ public:
+ DummySceneProvider() = default;
+
+ const core::Scene&
+ scene() const override
+ {
+ return dummyScene;
+ };
+
+ bool
+ initialize(const armarx::DateTime& /*timestamp*/) override
+ {
+ return true;
+ };
+
+ bool
+ synchronize(const armarx::DateTime& /*timestamp*/) override
+ {
+ return true;
+ };
+
+
+ // non-api
+ ~DummySceneProvider() override = default;
+
+ private:
+ core::Scene dummyScene; // FIXME implement
+ };
+
+} // namespace armarx::navigation::server::scene_provider
+
BOOST_AUTO_TEST_CASE(testNavigator)
{
@@ -56,19 +93,21 @@ BOOST_AUTO_TEST_CASE(testNavigator)
// TODO create static shared ctor
server::NavigationStack stack{.globalPlanner = std::make_shared<global_planning::Point2Point>(
- global_planning::Point2PointParams(), scene),
- .trajectoryControl =
- std::make_shared<traj_ctrl::TrajectoryFollowingController>(
- traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
+ global_planning::Point2PointParams(), scene)};
+ // .trajectoryControl =
+ // std::make_shared<traj_ctrl::TrajectoryFollowingController>(
+ // traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
// Executor.
server::DummyExecutor executor{scene.robot, server::DummyExecutor::Params()};
client::SimpleEventHandler eventHandler;
+ server::scene_provider::DummySceneProvider sceneProvider;
+
server::Navigator navigator(
- server::Navigator::Config{
- .stack = stack, .scene = &scene, .general = server::Navigator::Config::General{}},
- server::Navigator::InjectedServices{.executor = &executor, .publisher = &eventHandler});
+ server::Navigator::Config{.stack = stack, .general = server::Navigator::Config::General{}},
+ server::Navigator::InjectedServices{
+ .executor = &executor, .publisher = &eventHandler, .sceneProvider = &sceneProvider});
navigator.moveTo(std::vector{goal}, core::NavigationFrame::Absolute);
BOOST_CHECK_EQUAL(true, true);
diff --git a/source/armarx/navigation/trajectory_control/test/trajectory_controlTest.cpp b/source/armarx/navigation/trajectory_control/test/trajectory_controlTest.cpp
index 0f080fae25490b79e080b55a49cfa0375e1d14a5..6ad882342c75fd930f1a086df87579aca737fcb1 100644
--- a/source/armarx/navigation/trajectory_control/test/trajectory_controlTest.cpp
+++ b/source/armarx/navigation/trajectory_control/test/trajectory_controlTest.cpp
@@ -64,7 +64,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerOnTrajectory)
params.limits.linear = std::numeric_limits<float>::max();
params.limits.angular = std::numeric_limits<float>::max();
- traj_ctrl::TrajectoryFollowingController controller(params, scene);
+ traj_ctrl::TrajectoryFollowingController controller(scene.robot, params);
constexpr float ffVelocity = 100.0; // ground truth
@@ -106,7 +106,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
params.limits.linear = std::numeric_limits<float>::max();
params.limits.angular = std::numeric_limits<float>::max();
- traj_ctrl::TrajectoryFollowingController controller(params, scene);
+ traj_ctrl::TrajectoryFollowingController controller(scene.robot, params);
constexpr float ffVelocity = 100.0; // ground truth
@@ -188,7 +188,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
params.limits.linear = std::numeric_limits<float>::max();
params.limits.angular = std::numeric_limits<float>::max();
- traj_ctrl::TrajectoryFollowingController controller(params, scene);
+ traj_ctrl::TrajectoryFollowingController controller(scene.robot, params);
core::TrajectoryPoint tpStart;
tpStart.waypoint.pose = core::Pose::Identity();
@@ -269,7 +269,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedLinear)
params.limits.linear = 500.F; // [mm/s]
params.limits.angular = 2.F * M_PIf32 / 10.F; // [rad/s]
- traj_ctrl::TrajectoryFollowingController controller(params, scene);
+ traj_ctrl::TrajectoryFollowingController controller(scene.robot, params);
const core::Twist unclippedTwist{
.linear = 600 * Eigen::Vector3f::UnitX(), // 600 vs 500
@@ -315,7 +315,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedLinear2)
params.limits.linear = 500.F; // [mm/s]
params.limits.angular = 2.F * M_PIf32 / 10.F; // [rad/s]
- traj_ctrl::TrajectoryFollowingController controller(params, scene);
+ traj_ctrl::TrajectoryFollowingController controller(scene.robot, params);
const core::Twist unclippedTwist{
.linear = 600 * Eigen::Vector3f::Ones(), // 600 vs 500
@@ -360,7 +360,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedAngular)
params.limits.linear = 500.F; // [mm/s]
params.limits.angular = 2.F * M_PIf32 / 10.F; // [rad/s]
- traj_ctrl::TrajectoryFollowingController controller(params, scene);
+ traj_ctrl::TrajectoryFollowingController controller(scene.robot, params);
const core::Twist unclippedTwist{
.linear = 400 * Eigen::Vector3f::UnitX(), // below limit
@@ -406,7 +406,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedBoth)
params.limits.linear = 500.F; // [mm/s]
params.limits.angular = 2.F * M_PIf32 / 10.F; // [rad/s]
- traj_ctrl::TrajectoryFollowingController controller(params, scene);
+ traj_ctrl::TrajectoryFollowingController controller(scene.robot, params);
const core::Twist unclippedTwist{
.linear = 600 * Eigen::Vector3f::UnitX(), // 600 vs 500