update

source/Navigation/libraries/trajectory_control/test/CMakeLists.txt

-
 # Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore trajectory_control)
- 
-armarx_add_test(trajectory_controlTest trajectory_controlTest.cpp "${LIBS}")
\ No newline at end of file
+set(
+    LIBS
+    ${LIBS}
+    ArmarXCore
+    Navigation::TrajectoryControl
+)
+
+armarx_add_test(
+    trajectory_controlTest
+    trajectory_controlTest.cpp
+    "${LIBS}"
+)

source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp

@@ -21,6 +21,7 @@
  */
 
 #include <limits>
+#include <thread>
 
 #include <Eigen/Geometry>
 #include <Eigen/src/Geometry/AngleAxis.h>
@@ -89,9 +90,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
 {
     core::Scene scene;
     scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
-    scene.robot->setGlobalPose(Eigen::Matrix4f::Identity() *
-                               core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(),
-                               false);
+    scene.robot->setGlobalPose(
+        Eigen::Matrix4f::Identity() * core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(), false);
 
     traj_ctrl::TrajectoryFollowingControllerParams params;
     params.pidPos = {1, 0, 0};
@@ -153,6 +153,11 @@ private:
     const Params params;
 };
 
+class TimeAwareRobot: virtual public VirtualRobot::LocalRobot
+{
+
+};
+
 /**
  * @brief Tests the control
  *
@@ -161,9 +166,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
 {
     core::Scene scene;
     scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
-    scene.robot->setGlobalPose(Eigen::Matrix4f::Identity() *
-                               core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(),
-                               false);
+    scene.robot->setGlobalPose(
+        Eigen::Matrix4f::Identity() * core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(), false);
 
     traj_ctrl::TrajectoryFollowingControllerParams params;
     params.pidPos = {1, 0, 0};
@@ -175,7 +179,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
 
     core::TrajectoryPoint wpBefore;
     wpBefore.waypoint.pose = core::Pose::Identity();
-    wpBefore.waypoint.pose.translation().x() -= 100.F;
+    // wpBefore.waypoint.pose.translation().x() -= 100.F;
     wpBefore.twist.linear = Eigen::Vector3f::UnitX();
     wpBefore.twist.angular = Eigen::Vector3f::Zero();
 
@@ -186,14 +190,30 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
 
     auto timestamp = std::chrono::steady_clock::now();
 
-    TargetReachedCondition condition(wpAfter,
-    TargetReachedCondition::Params{.distance{
-            .linear = 50.F, .angular = simox::math::deg_to_rad(10.F)}});
+    TargetReachedCondition condition(
+        wpAfter,
+        TargetReachedCondition::Params
+    {
+        .distance{.linear = 50.F, .angular = simox::math::deg_to_rad(10.F)}});
 
-    for (int i = 0; i < 50; i++)
+    const auto isFinite = [](const auto & x)
+    {
+        return ((x - x).array() == (x - x).array()).all();
+        // return (x.array() != 0.F).any();
+    };
+
+    for (int i = 0; i < 10000; i++)
     {
         core::Twist controlVal = controller.control(traj);
-        ARMARX_DEBUG << "Control val " << controlVal.linear;
+        BOOST_CHECK(isFinite(controlVal.linear));
+        BOOST_CHECK(isFinite(controlVal.angular));
+
+        if (not isFinite(controlVal.linear) or not isFinite(controlVal.angular))
+        {
+            return;
+        }
+
+        ARMARX_INFO << "Control val " << controlVal.linear;
 
         auto now = std::chrono::steady_clock::now();
 
@@ -202,14 +222,16 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
         core::Pose dp = controlVal.poseDiff(dt);
 
         const core::Pose newPose = core::Pose(scene.robot->getGlobalPose()) * dp;
-        scene.robot->setGlobalPose(newPose.matrix());
+        scene.robot->setGlobalPose(newPose.matrix(), false);
 
         if (condition.check(newPose))
         {
             break;
         }
 
+        ARMARX_INFO << "new pos " << newPose.translation();
 
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
 
     BOOST_CHECK(condition.check(core::Pose(scene.robot->getGlobalPose())));

source/Navigation/libraries/util/CMakeLists.txt

-set(LIB_NAME       util)
+set(LIB_NAME       ${PROJECT_NAME}Util)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
@@ -18,7 +18,7 @@ armarx_add_library(
         ./util.h
 )
 
-add_library(Navigation::util ALIAS util)
+add_library(Navigation::Util ALIAS ${PROJECT_NAME}Util)
 
 #find_package(MyLib QUIET)
 #armarx_build_if(MyLib_FOUND "MyLib not available")
@@ -29,4 +29,4 @@ add_library(Navigation::util ALIAS util)
 #endif()
 
 # add unit tests
-add_subdirectory(test)
+# add_subdirectory(test)