CMakeLists.txt

@@ -12,26 +12,34 @@ armarx_project(navigation NAMESPACE armarx)
 
 # Specify each ArmarX Package dependency with the following macro
 
-# - required
+# Required ArmarX dependencies.
+# ===================================
+
 armarx_find_package(PUBLIC RobotAPI REQUIRED)
 
-# - optional
+# Optional ArmarX dependencies.
+# ===================================
+
 armarx_find_package(PUBLIC armarx::control)
 armarx_find_package(PUBLIC ArmarXGui)
 armarx_find_package(PUBLIC MemoryX QUIET)
 armarx_find_package(PUBLIC VisionX QUIET)
 armarx_find_package(PUBLIC ArmarXSimulation QUIET)
 
-# System dependencies
-# - required
+# Required system dependencies.
+# ===================================
+
+
+# Optional system dependencies.
+# ===================================
 
-# - optional
 armarx_find_package(PUBLIC OpenMP QUIET)
 armarx_find_package(PUBLIC Ceres QUIET)
 armarx_find_package(PUBLIC VTK QUIET)
 armarx_find_package(PUBLIC SemanticObjectRelations QUIET)
 armarx_find_package(PUBLIC OpenCV QUIET)  # Required as RobotAPI is a legacy project.
 armarx_find_package(PUBLIC range-v3 QUIET)
+
 # human aware navigation
 armarx_find_package(PUBLIC teb_local_planner QUIET)
 armarx_find_package(PUBLIC teb_extension QUIET)
@@ -45,12 +53,6 @@ add_subdirectory(etc)
 # )
 # FetchContent_MakeAvailable(inotify_cpp)
 
-add_definitions(-Werror=init-self)
-add_definitions(-Werror=uninitialized)
-add_definitions(-Werror=missing-field-initializers)
-add_definitions(-Werror=reorder)
-add_definitions(-Werror=narrowing)
-
 add_subdirectory(source)
 add_subdirectory(examples)
 

examples/components/example_client/CMakeLists.txt

@@ -12,6 +12,7 @@ armarx_add_component(example_client
         armem_robot_state
         armem_robot
         armarx_navigation::client
+        armarx_navigation::memory
         Simox::SimoxUtility
         Eigen3::Eigen
 )

examples/components/example_client/Component.cpp

@@ -33,6 +33,8 @@
 #include <ArmarXCore/core/time/forward_declarations.h>
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
+#include "armarx/navigation/algorithms/util.h"
+#include "armarx/navigation/conversions/eigen.h"
 #include "armarx/navigation/core/types.h"
 #include "armarx/navigation/global_planning/SPFA.h"
 #include "armarx/navigation/local_planning/TimedElasticBands.h"
@@ -49,13 +51,11 @@ namespace armarx::navigation::components::example_client
     Component::Component()
     {
         addPlugin(virtualRobotReaderPlugin);
+        addPlugin(costmapReaderPlugin);
     }
 
 
-    Component::~Component()
-    {
-        // pass
-    }
+    Component::~Component() = default;
 
 
     void
@@ -124,6 +124,10 @@ namespace armarx::navigation::components::example_client
                 ARMARX_IMPORTANT << "Running `Mode::UpdateNavigator`";
                 exampleNavigationUpdateNavigator();
                 break;
+            case Mode::WanderAround:
+                ARMARX_IMPORTANT << "Running `Mode::WanderAround`";
+                exampleNavigationWanderAround();
+                break;
         }
     }
 
@@ -426,6 +430,92 @@ namespace armarx::navigation::components::example_client
         getNavigator().pause();
     }
 
+    void
+    Component::exampleNavigationWanderAround()
+    {
+        // Import relevant namespaces.
+        using namespace armarx::navigation;
+
+        ARMARX_INFO << "Configuring navigator";
+
+        // Create an example configuration valid for the following move* calls.
+        configureNavigator(
+            client::NavigationStackConfig()
+                .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
+                .globalPlanner(global_planning::SPFAParams())
+                .localPlanner(local_planning::TimedElasticBandsParams())
+                .trajectoryController(
+                    traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
+
+        Clock::WaitFor(Duration::Seconds(1));
+
+        const algorithms::Costmap costmap = [&]() -> algorithms::Costmap
+        {
+            while (true)
+            {
+                const auto timestamp = Clock::Now();
+
+                const memory::client::costmap::Reader::Query costmapQuery{
+                    .providerName = "distance_to_obstacle_costmap_provider", // TODO check
+                    .name = "distance_to_obstacles",
+                    .timestamp = timestamp};
+
+                const memory::client::costmap::Reader::Result costmapResult =
+                    costmapReaderPlugin->get().query(costmapQuery);
+
+                // if costmap is not available yet, wait
+                if (costmapResult.status != memory::client::costmap::Reader::Result::Success)
+                {
+                    Clock::WaitFor(Duration::MilliSeconds(100));
+                    continue;
+                }
+
+                ARMARX_CHECK_EQUAL(costmapResult.status,
+                                   memory::client::costmap::Reader::Result::Success);
+
+                ARMARX_TRACE;
+                ARMARX_CHECK(costmapResult.costmap.has_value());
+                return costmapResult.costmap.value();
+            };
+        }();
+
+
+        while (true)
+        {
+
+            ARMARX_INFO << "Moving to goal pose";
+            // Start moving to goal position using above config.
+            const auto sampledPose = algorithms::sampleValidPositionInMap(costmap);
+            ARMARX_CHECK(sampledPose.has_value());
+            core::Pose goal = conv::to3D(sampledPose.value());
+
+            getNavigator().moveTo(goal, core::NavigationFrame::Absolute);
+
+            // Wait until goal is reached
+            armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
+            if (se)
+            {
+                ARMARX_INFO << "Goal reached.";
+            }
+            else
+            {
+                if (se.isSafetyStopTriggeredEvent())
+                {
+                    ARMARX_ERROR << "Safety stop was triggered!";
+                }
+                else if (se.isUserAbortTriggeredEvent())
+                {
+                    ARMARX_ERROR << "Aborted by user!";
+                }
+                else if (se.isInternalErrorEvent())
+                {
+                    ARMARX_ERROR << "Unknown internal error occured! "
+                                 << se.toInternalErrorEvent().message;
+                }
+            }
+        }
+    }
+
 
     void
     Component::goalReached()
@@ -450,7 +540,8 @@ namespace armarx::navigation::components::example_client
             .map({{"standard", Mode::Standard},
                   {"complex", Mode::Complex},
                   {"point_to_point", Mode::PointToPoint},
-                  {"update_navigator", Mode::UpdateNavigator}});
+                  {"update_navigator", Mode::UpdateNavigator},
+                  {"wander_around", Mode::WanderAround}});
 
         return def;
     }

examples/components/example_client/Component.h

@@ -26,10 +26,12 @@
 
 #include <ArmarXCore/util/tasks.h>
 
-#include <armarx/navigation/client.h>
+#include <RobotAPI/libraries/armem/client/plugins.h>
 #include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
 #include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
-#include <RobotAPI/libraries/armem/client/plugins.h>
+
+#include "armarx/navigation/memory/client/costmap/Reader.h"
+#include <armarx/navigation/client.h>
 
 
 namespace armarx::navigation::components::example_client
@@ -40,7 +42,8 @@ namespace armarx::navigation::components::example_client
         Standard,
         Complex,
         PointToPoint,
-        UpdateNavigator
+        UpdateNavigator,
+        WanderAround
     };
 
     /**
@@ -105,10 +108,13 @@ namespace armarx::navigation::components::example_client
 
         void exampleNavigationUpdateNavigator();
 
+        void exampleNavigationWanderAround();
+
     private:
         void goalReached();
 
-        struct{
+        struct
+        {
             std::string robotName = "Armar6";
             float relativeMovement = 200; // [mm]
 
@@ -117,9 +123,15 @@ namespace armarx::navigation::components::example_client
 
         armarx::RunningTask<Component>::pointer_type task;
 
-        armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr;
+        template <typename T>
+        using ReaderWriterPlugin = armem::client::plugins::ReaderWriterPlugin<T>;
+
 
+        ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
+            nullptr;
 
+        // for exampleNavigationWanderAround
+        ReaderWriterPlugin<memory::client::costmap::Reader>* costmapReaderPlugin = nullptr;
     };
 
 } // namespace armarx::navigation::components::example_client

scenarios/HumanAwareNavigation/HumanAwareNavigation.scx

@@ -9,9 +9,10 @@
 	<application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="VisionMemory" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
-	<application name="dynamic_distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
+	<application name="dynamic_distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="dynamic_scene_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
 	<application name="HumanMemoryApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
 	<application name="distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
+	<application name="human_simulator" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
 </scenario>
 

scenarios/HumanAwareNavigation/config/human_simulator.cfg

+# ==================================================================
+# human_simulator properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages:  List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName:  Application name
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath:  Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+#  Attributes:
+#  - Default:            mongo/.cache
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config:  Comma-separated list of configuration files 
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath:  Semicolon-separated search list for data files
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages:  List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig:  Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+#  Attributes:
+#  - Default:            ./config/dependencies.cfg
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging:  Turn logging off in whole application
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling:  Enable profiling of CPU load produced by this application
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries:  Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup:  The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout:  The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+#  Attributes:
+#  - Default:            3000
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay:  The startup will be delayed by this number of seconds (useful for debugging)
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash:  If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize:  Size of the ArmarX ThreadPool that is always running.
+#  Attributes:
+#  - Default:            1
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix:  Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer:  Enable using a global Timeserver (e.g. from ArmarXSimulator)
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity:  Global logging level for whole application
+#  Attributes:
+#  - Default:            Info
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
+# ArmarX.human_simulator.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.human_simulator.EnableProfiling = false
+
+
+# ArmarX.human_simulator.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.human_simulator.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.human_simulator.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.ObjectName = ""
+
+
+# ArmarX.human_simulator.mem.nav.costmap.CoreSegment:  
+#  Attributes:
+#  - Default:            Costmap
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.human_simulator.mem.nav.costmap.Memory:  
+#  Attributes:
+#  - Default:            Navigation
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.human_simulator.mem.nav.human.CoreSegment:  
+#  Attributes:
+#  - Default:            Human
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.mem.nav.human.CoreSegment = Human
+
+
+# ArmarX.human_simulator.mem.nav.human.Memory:  
+#  Attributes:
+#  - Default:            Navigation
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.mem.nav.human.Memory = Navigation
+
+
+# ArmarX.human_simulator.mem.nav.human.Provider:  Name of this provider
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.mem.nav.human.Provider = ""
+
+
+# ArmarX.human_simulator.mem.robot_state.Memory:  
+#  Attributes:
+#  - Default:            RobotState
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.human_simulator.mem.robot_state.localizationSegment:  Name of the localization memory core segment to use.
+#  Attributes:
+#  - Default:            Localization
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.human_simulator.mns.MemoryNameSystemEnabled:  Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+#  Attributes:
+#  - Default:            true
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.human_simulator.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.human_simulator.mns.MemoryNameSystemName:  Name of the Memory Name System (MNS) component.
+#  Attributes:
+#  - Default:            MemoryNameSystem
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.human_simulator.p.nHumans:  Number of humans to spawn.
+#  Attributes:
+#  - Default:            4
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.human_simulator.p.nHumans = 4
+
+
+# ArmarX.human_simulator.p.taskPeriodMs:  
+#  Attributes:
+#  - Default:            100
+#  - Case sensitivity:   yes
+#  - Required:           no
+ArmarX.human_simulator.p.taskPeriodMs = 100
+
+

scenarios/PlatformNavigation/PlatformNavigation.scx

@@ -6,7 +6,7 @@
 	<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="navigator" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
 	<application name="navigation_memory" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
-	<application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
+	<application name="example_client" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="VisionMemory" instance="" package="VisionX" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
 	<application name="distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>

scenarios/PlatformNavigation/config/ObjectMemory.cfg

@@ -259,6 +259,15 @@
 # ArmarX.ObjectMemory.mem.inst.calibration.offset = 0
 
 
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName:  Name of robot whose note can be calibrated.
+# If not given, the 'fallbackName' is used.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName = ""
+
+
 # ArmarX.ObjectMemory.mem.inst.calibration.robotNode:  Robot node which can be calibrated.
 #  Attributes:
 #  - Default:            Neck_2_Pitch
@@ -341,6 +350,14 @@
 # ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
 
 
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName:  Robot name to use as fallback if the robot name is not specified in a provided object pose.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName = ""
+
+
 # ArmarX.ObjectMemory.mem.inst.scene.10_Package:  ArmarX package containing the scene snapshots.
 # Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.
 #  Attributes:
@@ -572,14 +589,6 @@
 # ArmarX.ObjectMemory.prediction.TimeWindow = 2
 
 
-# ArmarX.ObjectMemory.robotName:  
-#  Attributes:
-#  - Default:            Armar6
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.ObjectMemory.robotName = Armar6
-
-
 # ArmarX.ObjectMemory.tpc.pub.DebugObserver:  Name of the `DebugObserver` topic to publish data to.
 #  Attributes:
 #  - Default:            DebugObserver

scenarios/PlatformNavigation/config/example_client.cfg

@@ -76,6 +76,14 @@
 # ArmarX.EnableProfiling = false
 
 
+# ArmarX.ExampleClient.nav.NavigatorName:  No Description
+#  Attributes:
+#  - Default:            navigator
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.ExampleClient.nav.NavigatorName = navigator
+
+
 # ArmarX.LoadLibraries:  Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
 #  Attributes:
 #  - Default:            ""
@@ -253,8 +261,3 @@ ArmarX.example_client.mode = standard
 # ArmarX.example_client.robotName = Armar6
 
 
-# ArmarX.ExampleClient.nav.NavigatorName:  
-#  Attributes:
-ArmarX.ExampleClient.nav.NavigatorName = navigator
-
-

scenarios/PlatformNavigation/config/navigation_memory.cfg

@@ -92,6 +92,14 @@
 # ArmarX.LoggingGroup = ""
 
 
+# ArmarX.NavigationMemory.p.snapshotToLoad:  No Description
+#  Attributes:
+#  - Default:            ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
+
+
 # ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
 #  Attributes:
 #  - Default:            true
@@ -288,8 +296,3 @@
 # ArmarX.navigation_memory.p.snapshotToLoad = ""
 
 
-# ArmarX.NavigationMemory.p.snapshotToLoad:  
-#  Attributes:
-ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/audimax-science-week-opening
-
-

scenarios/PlatformNavigation/config/navigator.cfg

@@ -225,14 +225,6 @@ ArmarX.navigator.ObjectName = navigator
 ArmarX.navigator.RobotUnitName = Armar6Unit
 
 
-# ArmarX.navigator.cmp.PlatformUnit:  No Description
-#  Attributes:
-#  - Default:            Armar6PlatformUnit
-#  - Case sensitivity:   no
-#  - Required:           no
-ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
-
-
 # ArmarX.navigator.cmp.RemoteGui:  Ice object name of the `RemoteGui` component.
 #  Attributes:
 #  - Default:            RemoteGuiProvider
@@ -412,13 +404,3 @@ ArmarX.navigator.cmp.PlatformUnit = Armar6PlatformUnit
 ArmarX.navigator.p.occupancy_grid.occopied_threshold = 0.8
 
 
-# ArmarX.Navigator.ObjectName:  
-#  Attributes:
-ArmarX.Navigator.ObjectName = navigator
-
-
-# ArmarX.Navigator.RobotUnitName:  
-#  Attributes:
-ArmarX.Navigator.RobotUnitName = Armar6Unit
-
-

source/armarx/navigation/CMakeLists.txt

@@ -16,6 +16,7 @@ add_subdirectory(location)
 add_subdirectory(memory)
 add_subdirectory(server)
 add_subdirectory(platform_controller)
+add_subdirectory(simulation)
 
 # Components.
 add_subdirectory(components)

source/armarx/navigation/algorithms/util.cpp

@@ -22,10 +22,9 @@
 #include "util.h"
 
 #include <algorithm>
+#include <cstddef>
 #include <iterator>
 
-#include <range/v3/all.hpp>
-
 #include <opencv2/core.hpp>
 #include <opencv2/core/eigen.hpp>
 #include <opencv2/imgproc.hpp>
@@ -33,16 +32,18 @@
 #include <SimoxUtility/algorithm/apply.hpp>
 #include <VirtualRobot/BoundingBox.h>
 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/Random.h>
 #include <VirtualRobot/SceneObjectSet.h>
 #include <VirtualRobot/Workspace/WorkspaceGrid.h>
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
-#include <armarx/navigation/algorithms/persistence.h>
 #include <armarx/navigation/algorithms/Costmap.h>
 #include <armarx/navigation/algorithms/CostmapBuilder.h>
+#include <armarx/navigation/algorithms/persistence.h>
 #include <armarx/navigation/algorithms/types.h>
+#include <range/v3/all.hpp>
 
 
 namespace armarx::navigation::algorithms
@@ -217,7 +218,8 @@ namespace armarx::navigation::algorithms
         const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getLocalSceneBounds(),
                                                             costmaps.front().params());
 
-        Costmap mergedCostmap(grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
+        Costmap mergedCostmap(
+            grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
 
         const auto addMode = [&]() -> Costmap::AddMode
         {
@@ -257,7 +259,8 @@ namespace armarx::navigation::algorithms
         const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getLocalSceneBounds(),
                                                             costmaps.front().params());
 
-        Costmap mergedCostmap(grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
+        Costmap mergedCostmap(
+            grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
 
         // foreach pair (costmap, weight): add it to there merged costmap
         ranges::for_each(ranges::views::zip(costmaps, weights),
@@ -328,4 +331,34 @@ namespace armarx::navigation::algorithms
     }
 
 
+    std::optional<core::Pose2D>
+    sampleValidPositionInMap(const algorithms::Costmap& costmap)
+    {
+        const auto sizeX = costmap.getGrid().cols();
+        const auto sizeY = costmap.getGrid().rows();
+
+        constexpr std::size_t maxIterations = 1000;
+
+        // sample a valid pose in the costmap
+
+        for (std::size_t iteration = 0; iteration < maxIterations; iteration++)
+        {
+            const float iX = VirtualRobot::RandomFloat() * static_cast<float>(sizeX);
+            const float iY = VirtualRobot::RandomFloat() * static_cast<float>(sizeY);
+
+            algorithms::Costmap::Index idx(iX, iY);
+
+            if (not costmap.isValid(idx))
+            {
+                continue;
+            }
+
+            core::Pose2D pose = core::Pose2D::Identity();
+            pose.translation() = costmap.toPositionGlobal(idx);
+            return pose;
+        }
+
+        ARMARX_ERROR << "Failed to sample pose in costmap!";
+        return std::nullopt;
+    }
 } // namespace armarx::navigation::algorithms

source/armarx/navigation/algorithms/util.h

@@ -64,4 +64,6 @@ namespace armarx::navigation::algorithms
 
     Costmap scaleCostmap(const Costmap& costmap, float cellSize);
 
+    std::optional<core::Pose2D> sampleValidPositionInMap(const algorithms::Costmap& costmap);
+
 } // namespace armarx::navigation::algorithms

source/armarx/navigation/client/services/EventSubscriptionInterface.h

@@ -13,12 +13,15 @@ namespace armarx::navigation::client
 
     using GlobalTrajectoryUpdatedCallback =
         std::function<void(const global_planning::GlobalPlannerResult&)>;
-    using LocalTrajectoryUpdatedCallback = std::function<void(const local_planning::LocalPlannerResult&)>;
+    using LocalTrajectoryUpdatedCallback =
+        std::function<void(const local_planning::LocalPlannerResult&)>;
     using TrajectoryControllerUpdatedCallback =
         std::function<void(const traj_ctrl::local::TrajectoryControllerResult&)>;
     using GlobalPlanningFailedCallback =
         std::function<void(const core::GlobalPlanningFailedEvent&)>;
 
+    using LocalPlanningFailedCallback = std::function<void(const core::LocalPlanningFailedEvent&)>;
+
     using OnMovementStartedCallback = std::function<void(const core::MovementStartedEvent&)>;
 
     using OnGoalReachedCallback = std::function<void(const core::GoalReachedEvent&)>;
@@ -49,6 +52,8 @@ namespace armarx::navigation::client
         virtual void onInternalError(const OnInternalErrorCallback& callback) = 0;
 
         virtual void onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback) = 0;
+        virtual void onLocalPlanningFailed(const LocalPlanningFailedCallback& callback) = 0;
+
 
         // Non-API.
         virtual ~EventSubscriptionInterface() = default;

source/armarx/navigation/client/services/SimpleEventHandler.cpp

@@ -61,6 +61,13 @@ armarx::navigation::client::SimpleEventHandler::onGlobalPlanningFailed(
     subscriptions.globalPlanningFailedCallbacks.push_back(callback);
 }
 
+void
+armarx::navigation::client::SimpleEventHandler::onLocalPlanningFailed(
+    const LocalPlanningFailedCallback& callback)
+{
+    subscriptions.localPlanningFailedCallbacks.push_back(callback);
+}
+
 void
 armarx::navigation::client::SimpleEventHandler::goalReached(const core::GoalReachedEvent& event)
 {
@@ -163,6 +170,15 @@ namespace armarx::navigation::client
         }
     }
 
+    void
+    SimpleEventHandler::localPlanningFailed(const core::LocalPlanningFailedEvent& event)
+    {
+        for (const auto& callback : subscriptions.localPlanningFailedCallbacks)
+        {
+            callback(event);
+        }
+    }
+
     void
     SimpleEventHandler::movementStarted(const core::MovementStartedEvent& event)
     {

source/armarx/navigation/client/services/SimpleEventHandler.h

@@ -27,12 +27,14 @@ namespace armarx::navigation::client
 
         void onMovementStarted(const OnMovementStartedCallback& callback) override;
         void onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback) override;
+        void onLocalPlanningFailed(const LocalPlanningFailedCallback& callback) override;
 
         // EventPublishingInterface
         void globalTrajectoryUpdated(const global_planning::GlobalPlannerResult& event) override;
         void localTrajectoryUpdated(const local_planning::LocalPlannerResult& event) override;
         // void trajectoryControllerUpdated(const traj_ctrl::local::TrajectoryControllerResult& event) override;
         void globalPlanningFailed(const core::GlobalPlanningFailedEvent& event) override;
+        void localPlanningFailed(const core::LocalPlanningFailedEvent& event) override;
 
         void movementStarted(const core::MovementStartedEvent& event) override;
         void goalReached(const core::GoalReachedEvent& event) override;
@@ -49,6 +51,7 @@ namespace armarx::navigation::client
             std::vector<LocalTrajectoryUpdatedCallback> localTrajectoryUpdatedCallbacks;
             std::vector<TrajectoryControllerUpdatedCallback> trajectoryControllerUpdatedCallbacks;
             std::vector<GlobalPlanningFailedCallback> globalPlanningFailedCallbacks;
+            std::vector<LocalPlanningFailedCallback> localPlanningFailedCallbacks;
 
             std::vector<OnMovementStartedCallback> movementStartedCallbacks;
             std::vector<OnGoalReachedCallback> goalReachedCallbacks;

source/armarx/navigation/components/CMakeLists.txt

@@ -17,3 +17,5 @@ add_subdirectory(distance_to_obstacle_costmap_provider)
 # others
 # ===================================
 add_subdirectory(dynamic_scene_provider)
+
+add_subdirectory(human_simulator)
\ No newline at end of file

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

@@ -137,7 +137,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
         }
         */
 
-        robot = virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
+        while(true)
+        {
+            robot = virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
+            if(robot != nullptr)
+            {
+                break;
+            }
+
+            Clock::WaitFor(Duration::MilliSeconds(100));
+        }
         ARMARX_CHECK_NOT_NULL(robot);
 
         humanTracker.reset();
@@ -197,7 +206,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
         ARMARX_INFO << "Querying humans";
 
         const armem::human::client::Reader::Query humanPoseQuery{.providerName = properties.humanPoseProvider,
-                                                                 .timestamp = timestamp};
+                                                                 .timestamp = timestamp, .maxAge = Duration::MilliSeconds(500)};
 
         const armem::human::client::Reader::Result humanPoseResult =
             humanPoseReaderPlugin->get().query(humanPoseQuery);

source/armarx/navigation/components/human_simulator/CMakeLists.txt

+armarx_add_component(human_simulator
+    ICE_FILES
+        ComponentInterface.ice
+    ICE_DEPENDENCIES
+        ArmarXCoreInterfaces
+    SOURCES
+        Component.cpp
+    HEADERS
+        Component.h
+    DEPENDENCIES
+         # ArmarXCore
+         ArmarXCore
+         # RobotAPI
+         armem
+         armem_robot
+         armem_robot_state
+         # VisionX
+         armem_human
+         armem_vision
+         # navigation
+         armarx_navigation::util
+         armarx_navigation::memory
+         armarx_navigation::dynamic_scene
+         armarx_navigation::teb_human
+         armarx_navigation::simulation
+)