GoalReachedMonitor: activated

source/Navigation/libraries/server/monitoring/GoalReachedMonitor.cpp

@@ -19,7 +19,7 @@ namespace armarx::nav::server
     GoalReachedMonitor::goalReached() const noexcept
     {
         // FIXME remove
-        return false;
+        // return false;
 
         const core::Pose world_T_robot(scene.robot->getGlobalPose());
         const core::Pose& world_T_goal(waypoints.back());
@@ -29,6 +29,17 @@ namespace armarx::nav::server
         const bool posReached = robot_T_goal.translation().norm() <= config.posTh;
         const bool oriReached = Eigen::AngleAxisf(robot_T_goal.linear()).angle() <= config.oriTh;
 
+        if (posReached)
+        {
+            ARMARX_INFO << "posReached";
+        }
+
+        if (oriReached)
+        {
+            ARMARX_INFO << "oriReached";
+        }
+
+
         return posReached and oriReached;
     }