.gitignore

@@ -72,3 +72,6 @@ etc/python/dist/*
 .cmake/api/v1/query/cache-v2
 .cmake/api/v1/query/codemodel-v2
 .cmake/api/v1/query/cmakeFiles-v1
+
+.clang-tidy
+.clang-format

scenarios/PlatformNavigation/config/ObjectMemory.cfg

@@ -577,10 +577,10 @@
 
 # ArmarX.ObjectMemory.mem.ltm.configuration:  
 #  Attributes:
-#  - Default:            {}
+#  - Default:            {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.ObjectMemory.mem.ltm.configuration = {}
+# ArmarX.ObjectMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 
 
 # ArmarX.ObjectMemory.mem.ltm.enabled:  

scenarios/PlatformNavigation/config/SkillsMemory.cfg

@@ -169,10 +169,10 @@
 
 # ArmarX.SkillMemory.mem.ltm.configuration:  
 #  Attributes:
-#  - Default:            {}
+#  - Default:            {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.SkillMemory.mem.ltm.configuration = {}
+# ArmarX.SkillMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 
 
 # ArmarX.SkillMemory.mem.ltm.enabled:  

scenarios/PlatformNavigation/config/VisionMemory.cfg

@@ -212,10 +212,10 @@
 
 # ArmarX.VisionMemory.mem.ltm.configuration:  
 #  Attributes:
-#  - Default:            {}
+#  - Default:            {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.VisionMemory.mem.ltm.configuration = {}
+# ArmarX.VisionMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 
 
 # ArmarX.VisionMemory.mem.ltm.enabled:  
@@ -253,6 +253,70 @@
 # ArmarX.VisionMemory.occupancyGridMaxHistorySize = 20
 
 
+# ArmarX.VisionMemory.pcxyz.seg.CoreMaxHistorySize:  Maximal size of the PointCloudXYZ entity histories (-1 for infinite).
+#  Attributes:
+#  - Default:            20
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.pcxyz.seg.CoreMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pcxyz.seg.CoreSegmentName:  Name of the PointCloudXYZ core segment.
+#  Attributes:
+#  - Default:            PointCloudXYZ
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.pcxyz.seg.CoreSegmentName = PointCloudXYZ
+
+
+# ArmarX.VisionMemory.pcxyzl.seg.CoreMaxHistorySize:  Maximal size of the PointCloudXYZL entity histories (-1 for infinite).
+#  Attributes:
+#  - Default:            20
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.pcxyzl.seg.CoreMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pcxyzl.seg.CoreSegmentName:  Name of the PointCloudXYZL core segment.
+#  Attributes:
+#  - Default:            PointCloudXYZL
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.pcxyzl.seg.CoreSegmentName = PointCloudXYZL
+
+
+# ArmarX.VisionMemory.pcxyzrgba.seg.CoreMaxHistorySize:  Maximal size of the PointCloudXYZRGBA entity histories (-1 for infinite).
+#  Attributes:
+#  - Default:            20
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.pcxyzrgba.seg.CoreMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pcxyzrgba.seg.CoreSegmentName:  Name of the PointCloudXYZRGBA core segment.
+#  Attributes:
+#  - Default:            PointCloudXYZRGBA
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.pcxyzrgba.seg.CoreSegmentName = PointCloudXYZRGBA
+
+
+# ArmarX.VisionMemory.pcxyzrgbl.seg.CoreMaxHistorySize:  Maximal size of the PointCloudXYZRGBL entity histories (-1 for infinite).
+#  Attributes:
+#  - Default:            20
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.pcxyzrgbl.seg.CoreMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pcxyzrgbl.seg.CoreSegmentName:  Name of the PointCloudXYZRGBL core segment.
+#  Attributes:
+#  - Default:            PointCloudXYZRGBL
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.VisionMemory.pcxyzrgbl.seg.CoreSegmentName = PointCloudXYZRGBL
+
+
 # ArmarX.VisionMemory.pointCloudMaxHistorySize:  
 #  Attributes:
 #  - Default:            20

scenarios/PlatformNavigation/config/control_memory.cfg

@@ -94,10 +94,10 @@
 
 # ArmarX.ControlMemory.mem.ltm.configuration:  
 #  Attributes:
-#  - Default:            {}
+#  - Default:            {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.ControlMemory.mem.ltm.configuration = {}
+# ArmarX.ControlMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 
 
 # ArmarX.ControlMemory.mem.ltm.enabled:  

scenarios/PlatformNavigation/config/distance_to_obstacle_costmap_provider.cfg

@@ -257,6 +257,6 @@
 #  - Default:            Armar6
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.distance_to_obstacle_costmap_provider.p.robotName = Armar6
+ArmarX.distance_to_obstacle_costmap_provider.p.robotName = ArmarDE
 
 

scenarios/PlatformNavigation/config/navigation_memory.cfg

@@ -228,10 +228,10 @@ ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graph
 
 # ArmarX.navigation_memory.mem.ltm.configuration:  
 #  Attributes:
-#  - Default:            {}
+#  - Default:            {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.navigation_memory.mem.ltm.configuration = {}
+# ArmarX.navigation_memory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
 
 
 # ArmarX.navigation_memory.mem.ltm.enabled:  
@@ -261,14 +261,6 @@ ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graph
 # ArmarX.navigation_memory.mns.MemoryNameSystemName = MemoryNameSystem
 
 
-# ArmarX.navigation_memory.p.locationGraph.visuFrequency:  Visualization frequeny of locations and graph edges [Hz].
-#  Attributes:
-#  - Default:            2
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.navigation_memory.p.locationGraph.visuFrequency = 2
-
-
 # ArmarX.navigation_memory.p.locationGraph.visuGraphEdges:  Enable visualization of navigation graph edges.
 #  Attributes:
 #  - Default:            true
@@ -296,3 +288,47 @@ ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graph
 ArmarX.navigation_memory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/R003
 
 
+# ArmarX.navigation_memory.p.visuCostmaps:  Enable visualization of costmaps.
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.visuCostmaps = true
+
+
+# ArmarX.navigation_memory.p.visuFrequency:  Visualization frequeny of locations and graph edges [Hz].
+#  Attributes:
+#  - Default:            2
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.navigation_memory.p.visuFrequency = 2
+
+
+# ArmarX.navigation_memory.p.visuHumans:  Enable visualization of humans.
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.visuHumans = true
+
+
+# ArmarX.navigation_memory.p.visuRooms:  Enable visualization of rooms.
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.visuRooms = true
+
+
+# ArmarX.navigation_memory.p.visuTransparent:  Enable visualization of humans a bit transparent.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.visuTransparent = false
+
+

scenarios/PlatformNavigation/config/navigator.cfg

@@ -102,10 +102,10 @@ ArmarX.Navigator.ObjectName = navigator
 
 # ArmarX.Navigator.RobotUnitName:  No Description
 #  Attributes:
-#  - Default:            Armar6Unit
+#  - Default:            ArmarDEUnit
 #  - Case sensitivity:   no
 #  - Required:           no
-ArmarX.Navigator.RobotUnitName = Armar6Unit
+ArmarX.Navigator.RobotUnitName = ArmarDEUnit
 
 
 # ArmarX.Navigator.p.occupancy_grid.occopied_threshold:  No Description
@@ -246,7 +246,7 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
 #  Attributes:
 #  - Case sensitivity:   yes
 #  - Required:           yes
-ArmarX.navigator.RobotUnitName = Armar6Unit
+ArmarX.navigator.RobotUnitName = ArmarDEUnit
 
 
 # ArmarX.navigator.cmp.RemoteGui:  Ice object name of the `RemoteGui` component.
@@ -440,7 +440,7 @@ ArmarX.navigator.RobotUnitName = Armar6Unit
 #  Attributes:
 #  - Case sensitivity:   yes
 #  - Required:           yes
-ArmarX.navigator.p.scene.robotName = Armar6
+ArmarX.navigator.p.scene.robotName = ArmarDE
 
 
 # ArmarX.navigator.p.scene.staticCostmapName:  

source/armarx/navigation/components/navigation_memory/Component.cpp

@@ -142,7 +142,7 @@ namespace armarx::navigation::components::navigation_memory
         //                                navigation::core::arondto::Twist::ToAronType());
 
         workingMemory().addCoreSegment("Events"); //, armem::example::ExampleData::ToAronType());
-        // workingMemory.addCoreSegment("Exceptions");  //, armem::example::ExampleData::ToAronType());
+        workingMemory().addCoreSegment("Exceptions");  //, armem::example::ExampleData::ToAronType());
 
 
         workingMemory().addCoreSegment(navigation::location::coreSegmentID.coreSegmentName,
@@ -160,10 +160,14 @@ namespace armarx::navigation::components::navigation_memory
 
         if (not properties.snapshotToLoad.empty())
         {
+            ARMARX_INFO << "Loading snapshots from " << properties.snapshotToLoad;
+
             std::filesystem::path snapshotToLoadPath(
                 ArmarXDataPath::resolvePath(properties.snapshotToLoad));
             if (std::filesystem::is_directory(snapshotToLoadPath))
             {
+                ARMARX_DEBUG << "Found the folder";
+
                 // This section loads the snapshot specified by the scenario parameters
                 // resolve the paths for the locations and graphs
                 const std::filesystem::path graph = snapshotToLoadPath / "Graph";
@@ -180,6 +184,8 @@ namespace armarx::navigation::components::navigation_memory
                 // The file contains json with the globalRobotPose (4x4 matrix) and relativeToObject information
                 if (std::filesystem::is_regular_file(locationsFilename))
                 {
+                    ARMARX_DEBUG << "Found the locations file";
+
                     armem::Commit c;
                     armem::MemoryID providerID = workingMemory().id();
                     providerID.coreSegmentName = "Location";
@@ -405,6 +411,10 @@ namespace armarx::navigation::components::navigation_memory
                                << "'. Continue with empty memory.";
             }
         }
+        else
+        {
+            ARMARX_IMPORTANT << "Not loading any predefined locations or graphs as the params was empty...";
+        }
     }
 
 

source/armarx/navigation/server/execution/DummyExecutor.h

@@ -30,6 +30,9 @@ namespace armarx::navigation::server
         void start(ControllerType controllerType) override{};
         void stop() override{};
 
+        void
+        ensureIsActive(ControllerType controllerType) override{};
+
 
     private:
         VirtualRobot::RobotPtr robot;

source/armarx/navigation/skills/NavigateTo.cpp

 #include "NavigateTo.h"
 
 #include <memory>
+#include <future>
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -19,6 +20,31 @@
 
 namespace armarx::navigation::skills
 {
+    template <typename R>
+    bool
+    is_ready(std::future<R> const& f)
+    {
+        if (!f.valid())
+        {
+            return true;
+        }
+        std::future_status status = f.wait_for(std::chrono::seconds(0));
+        if (status == std::future_status::ready)
+        {
+            return true;
+        }
+        else if (status == std::future_status::deferred)
+        {
+            // if the task has not been started --> start the task
+            f.wait();
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+
     NavigateTo::NavigateTo() : Base(DefaultSkillDescription())
     {
     }
@@ -85,7 +111,21 @@ namespace armarx::navigation::skills
 
         // Wait until goal is reached
         ARMARX_INFO << "Waiting until goal is reached.";
-        client::StopEvent se = navigator->waitForStop();
+
+        auto future =
+            std::async(std::launch::async,
+                       [this]() { return navigator->waitForStop(); });
+        while (not is_ready(future))
+        {
+            if (checkWhetherSkillShouldStopASAP())
+            {
+                ARMARX_INFO << "Skill was aborted by user " << stopped << ", " << timeoutReached;
+                navigator->stop();
+                break;
+            }
+        }
+
+        auto se = future.get();
         if (se)
         {
             ARMARX_INFO << "Goal `" << Eigen::Isometry3f(in.params.targetPose).translation()

source/armarx/navigation/skills/NavigateTo.h

@@ -66,7 +66,7 @@ namespace armarx::navigation::skills
             return {constants::skill_names::NavigateTo,
                     "",
                     {},
-                    armarx::core::time::Duration(),
+                    armarx::core::time::Duration::Days(42),
                     Params::ToAronType()};
         }
 

source/armarx/navigation/skills/NavigateToLocation.cpp

 #include "NavigateToLocation.h"
 
 #include <memory>
+#include <future>
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -19,6 +20,32 @@
 
 namespace armarx::navigation::skills
 {
+
+    template <typename R>
+    bool
+    is_ready(std::future<R> const& f)
+    {
+        if (!f.valid())
+        {
+            return true;
+        }
+        std::future_status status = f.wait_for(std::chrono::seconds(0));
+        if (status == std::future_status::ready)
+        {
+            return true;
+        }
+        else if (status == std::future_status::deferred)
+        {
+            // if the task has not been started --> start the task
+            f.wait();
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+
     NavigateToLocation::NavigateToLocation() : Base(DefaultSkillDescription())
     {
     }
@@ -92,7 +119,20 @@ namespace armarx::navigation::skills
 
         // Wait until goal is reached
         ARMARX_INFO << "Waiting until goal is reached.";
-        client::StopEvent se = navigator->waitForStop();
+        auto future =
+            std::async(std::launch::async,
+                       [this]() { return navigator->waitForStop(); });
+        while (not is_ready(future))
+        {
+            if (checkWhetherSkillShouldStopASAP())
+            {
+                ARMARX_INFO << "Skill was aborted by user";
+                navigator->stop();
+                break;
+            }
+        }
+
+        auto se = future.get();
         if (se)
         {
             ARMARX_INFO << "Goal `" << location << "`reached.";
@@ -113,7 +153,7 @@ namespace armarx::navigation::skills
                 ARMARX_ERROR << "Aborted by user!";
 
                 return ::armarx::skills::Skill::MainResult{
-                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
+                    .status = ::armarx::skills::TerminatedSkillStatus::Aborted};
             }
             if (se.isInternalErrorEvent())
             {

source/armarx/navigation/skills/NavigateToLocation.h

@@ -68,7 +68,7 @@ namespace armarx::navigation::skills
             return {constants::skill_names::NavigateToLocation,
                     "",
                     {},
-                    armarx::core::time::Duration(),
+                    armarx::core::time::Duration::Days(42),
                     Params::ToAronType()};
         }
 

source/armarx/navigation/statecharts/NavigationGroup/NavigateToLocation.xml

@@ -2,6 +2,7 @@
 <State version="1.2" name="NavigateToLocation" uuid="11145B85-CD8C-4B7C-AF2F-65FCB07F6568" width="800" height="600" type="Normal State">
 	<InputParameters>
 		<Parameter name="location" type="::armarx::StringVariantData" docType="string" optional="yes">
+			<DefaultValue profile="Armar6Base" value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"euROBIN_seville/center"}}' docValue="euROBIN_seville/center"/>
 			<DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"R003/sh_center"}}' docValue="R003/sh_center"/>
 		</Parameter>
 	</InputParameters>

source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.cpp

@@ -68,7 +68,7 @@ namespace armarx::navigation::traj_ctrl::global
                std::numeric_limits<double>::max(),
                false,
                std::vector<bool>{false, false, false}),
-        pidPosTarget(params.pidPos.Kp / 2,
+        pidPosTarget(params.pidPos.Kp,
                      params.pidPos.Ki,
                      params.pidPos.Kd,
                      std::numeric_limits<double>::max(),
@@ -129,12 +129,12 @@ namespace armarx::navigation::traj_ctrl::global
         constexpr float eps = 0.001;
 
         // pedantic checks
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear + eps);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear + eps);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear + eps);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular + eps);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular + eps);
-        ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular + eps);
+        // ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear + eps);
+        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear + eps);
+        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear + eps);
+        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular + eps);
+        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular + eps);
+        //ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular + eps);
 
         return twist;
     }